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
06_AttitudeControl/attitudeControl.cpp
- Committer:
- ahmed_lv
- Date:
- 2018-03-20
- Revision:
- 30:44676e1b38f8
- Parent:
- 19:7345688260b2
File content as of revision 30:44676e1b38f8:
/** * @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]= 60;//3; ks_beta[i]=30; ks_theta[i]=5; kt_beta[i]=30; kt_theta[i]=5; i=1; //Control parameters for flag = 1 kx_beta[i]=1; kx_theta[i]=60;//3; ks_beta[i]=30; ks_theta[i]=5; kt_beta[i]=20; kt_theta[i]=5; // Gamma Control // Control gains k1 = 30;//250;//160; k2 = 200;//200;//80;//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, float* u1_, float* u2_) { // 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); *u1_ = u1; // Gamma Control //float dphi = 0.5*(dtheta_R - dtheta_L); float dphi = 0.5*(dtheta_R - dtheta_L); float gamma_e = ref_gamma - gamma; //convert heading error into -pi -> +pi if(gamma_e < -1*M_PI) gamma_e += 2*M_PI; if(gamma_e > M_PI) gamma_e -= 2*M_PI; 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; float u2 = (ref_ddgamma + k1*ref_dgamma + k2*gamma_e)/A; *u2_ = u2; // Control Inputs for the drive wheels float dtheta_input = dtheta + u1*tc; //float dphi_input = dphi + u2*tc; float dphi_input = u2*tc; //float dphi_input = 0.0; *w_L = dtheta_input - dphi_input; *w_R = dtheta_input + dphi_input; }