Implement new controller
Dependencies: mbed-rtos mbed QEI BNO055 MPU6050_DMP_Nucleo-I2Cdev virgo3_imuHandler_Orion_PCB MAX17048 Servo
Fork of Orion_newPCB_test by
Diff: 06_AttitudeControl/attitudeControl.cpp
- Revision:
- 2:761e3c932ce0
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/06_AttitudeControl/attitudeControl.cpp Mon Jan 18 06:00:43 2016 +0000 @@ -0,0 +1,97 @@ +/** + * @file attitudeControl.cpp + * @brief This file implements attitudeController function + */ + +#include "attitudeControl.h" + +attitudeControl::attitudeControl() +{ + // System parameters + // Sphere + ms = 20; + rs = 30; + rs_inner = 58.5/2; + Is = ( (2.0/5.0) * ms * (pow(rs,5) - pow(rs_inner,5)) / (pow(rs,3) - pow(rs_inner,3)) ); + + // Cart + mc = 70; + rc = 4.5; + ri = (12.76 + 9.5/2); + Ic_x = 15163.66; + Ic_z = 13938.73; + + // Drive wheel + mw = 0.19; + rw = 9.5/2; + Iw = 2.52; + h = 14; + + // Gravity + gravity = 9.82*1000; + + // Theta and Beta Control + // Control parameters + int i; + i=0; //Control parameters for flag = 0 + kx_beta[i]=1; + kx_theta[i]=3; + ks_beta[i]=30; + ks_theta[i]=5; + kt_beta[i]=20; + kt_theta[i]=5; + + i=1; //Control parameters for flag = 1 + kx_beta[i]=1; + kx_theta[i]=3; + ks_beta[i]=30; + ks_theta[i]=5; + kt_beta[i]=20; + kt_theta[i]=5; + + // Gamma Control + // Control gains + k1 = 2; + k2 = k1*k1/4; +} + +void attitudeControl::GenWheelVelocities(float* w_L, float* w_R, int flag, float tc, + float beta, float dbeta, float gamma, float dgamma, + float theta_L, float theta_R, float dtheta_L, float dtheta_R, + float ref_ddbeta, float ref_dbeta, float ref_beta, + float ref_ddtheta, float ref_dtheta, float ref_theta, + float ref_ddgamma, float ref_dgamma, float ref_gamma) +{ + // Theta and Beta Control + if (!flag) ref_theta = 0.5*(theta_L + theta_R); + + float theta = 0.5*(theta_L + theta_R); + float dtheta = 0.5*(dtheta_L + dtheta_R); + float M11 = (ms+mc)*rs*rs + Is + mc*rc*rc + Ic_x - 2*mc*rc*rs*cos(beta); + float M12 = ((ms+mc)*rs*rs + Is)*rw/ri + 2*Iw - mc*rc*rs*cos(beta)*rw/ri; + float fx1 = -(mc*rc*rs*sin(beta)*dbeta*dbeta + mc*rc*gravity*sin(beta))/M11; + float fx2 = 0; + float gx1 = -M12/M11; + float gx2 = 1; + + float s_beta = (ref_dbeta - dbeta) + kx_beta[flag]*(ref_beta - beta); + float s_theta = (ref_dtheta - dtheta) + kx_theta[flag]*(ref_theta - theta); + + float temp1 = ref_ddbeta - fx1 + kx_beta[flag]*(ref_dbeta - dbeta) + ks_beta[flag]*tanh(kt_beta[flag]*s_beta); + float temp2 = ref_ddtheta - fx2 + kx_theta[flag]*(ref_dtheta - dtheta) + ks_theta[flag]*tanh(kt_theta[flag]*s_theta); + float u1 = (gx1*temp1 + gx2*temp2)/(gx1*gx1 + gx2*gx2); + + // Gamma Control + float dphi = 0.5*(dtheta_R - dtheta_L); + float gamma_e = ref_gamma - gamma; + float dgamma_e = ref_dgamma - dgamma; + float A = -rw*Is/(h*(Is+Ic_z)); + float u2 = (ref_ddgamma + k1*dgamma_e + k2*gamma_e)/A; + + // Control Inputs for the drive wheels + float dtheta_input = dtheta + u1*tc; + //float dphi_input = dphi + u2*tc; + float dphi_input = 0.0; + *w_L = dtheta_input - dphi_input; + *w_R = dtheta_input + dphi_input; +} \ No newline at end of file