attitudeControl.cpp

00001 /**
00002  *  @file attitudeControl.cpp
00003  *  @brief This file implements attitudeController function
00004  */
00005 
00006 #include "attitudeControl.h"
00007 
00008 attitudeControl::attitudeControl()
00009 {
00010     // System parameters
00011     // Sphere
00012     ms = 20;
00013     rs = 30;
00014     rs_inner = 58.5/2;
00015     Is = ( (2.0/5.0) * ms * (pow(rs,5) - pow(rs_inner,5)) / (pow(rs,3) - pow(rs_inner,3)) );
00016 
00017     // Cart
00018     mc = 70;
00019     rc = 4.5;
00020     ri = (12.76 + 9.5/2);
00021     Ic_x = 15163.66;
00022     Ic_z = 13938.73;
00023 
00024     // Drive wheel
00025     mw = 0.19;
00026     rw = 9.5/2;
00027     Iw = 2.52;
00028     h = 14;
00029 
00030     // Gravity
00031     gravity = 9.82*1000;
00032 
00033     // Theta and Beta Control
00034     // Control parameters
00035     int i;
00036     i=0; //Control parameters for flag = 0
00037     kx_beta[i]=1;
00038     kx_theta[i]= 60;//3;
00039     ks_beta[i]=30;
00040     ks_theta[i]=5;
00041     kt_beta[i]=30;
00042     kt_theta[i]=5;
00043 
00044     i=1; //Control parameters for flag = 1
00045     kx_beta[i]=1;
00046     kx_theta[i]=60;//3;
00047     ks_beta[i]=30;
00048     ks_theta[i]=5;
00049     kt_beta[i]=20;
00050     kt_theta[i]=5;
00051 
00052     // Gamma Control
00053     // Control gains
00054     k1 = 30;//250;//160;
00055     k2 = 200;//200;//80;//k1*k1/4;
00056     
00057 }
00058 
00059 void attitudeControl::GenWheelVelocities(float* w_L, float* w_R, int flag, float tc,
00060         float beta, float dbeta, float gamma, float dgamma,
00061         float theta_L, float theta_R, float dtheta_L, float dtheta_R,
00062         float ref_ddbeta, float ref_dbeta, float ref_beta,
00063         float ref_ddtheta, float ref_dtheta, float ref_theta,
00064         float ref_ddgamma,  float ref_dgamma, float ref_gamma,
00065         float* u1_, float* u2_)
00066 {
00067     // Theta and Beta Control
00068     if (!flag) ref_theta = 0.5*(theta_L + theta_R);
00069     
00070     float theta = 0.5*(theta_L + theta_R);
00071     float dtheta = 0.5*(dtheta_L + dtheta_R);
00072     float M11 = (ms+mc)*rs*rs + Is + mc*rc*rc + Ic_x - 2*mc*rc*rs*cos(beta);
00073     float M12 = ((ms+mc)*rs*rs + Is)*rw/ri + 2*Iw - mc*rc*rs*cos(beta)*rw/ri;
00074     float fx1 = -(mc*rc*rs*sin(beta)*dbeta*dbeta + mc*rc*gravity*sin(beta))/M11;
00075     float fx2 = 0;
00076     float gx1 = -M12/M11;
00077     float gx2 = 1;
00078 
00079     float s_beta = (ref_dbeta - dbeta) + kx_beta[flag]*(ref_beta - beta);
00080     float s_theta = (ref_dtheta - dtheta) + kx_theta[flag]*(ref_theta - theta);
00081 
00082     float temp1 = ref_ddbeta - fx1 + kx_beta[flag]*(ref_dbeta - dbeta) + ks_beta[flag]*tanh(kt_beta[flag]*s_beta);
00083     float temp2 = ref_ddtheta - fx2 + kx_theta[flag]*(ref_dtheta - dtheta) + ks_theta[flag]*tanh(kt_theta[flag]*s_theta);
00084     float u1 = (gx1*temp1 + gx2*temp2)/(gx1*gx1 + gx2*gx2);
00085     *u1_ = u1;
00086     
00087     // Gamma Control
00088     //float dphi = 0.5*(dtheta_R - dtheta_L);
00089     float dphi = 0.5*(dtheta_R - dtheta_L);
00090     float gamma_e = ref_gamma - gamma;
00091     //convert heading error into -pi -> +pi 
00092     if(gamma_e < -1*M_PI)   gamma_e += 2*M_PI;
00093     if(gamma_e > M_PI)   gamma_e -= 2*M_PI;
00094     
00095     float dgamma_e = ref_dgamma - dgamma;
00096     float A = -rw*Is/(h*(Is+Ic_z));
00097     //float u2 = (ref_ddgamma + k1*dgamma_e + k2*gamma_e)/A;
00098     float u2 = (ref_ddgamma + k1*ref_dgamma + k2*gamma_e)/A;
00099     *u2_ = u2;
00100     
00101     // Control Inputs for the drive wheels
00102     float dtheta_input = dtheta + u1*tc;
00103     //float dphi_input = dphi + u2*tc;
00104     float dphi_input = u2*tc;
00105     //float dphi_input = 0.0;
00106     *w_L = dtheta_input - dphi_input;
00107     *w_R = dtheta_input + dphi_input;
00108 }