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@19:7345688260b2, 2016-06-07 (annotated)
- Committer:
- akashvibhute
- Date:
- Tue Jun 07 03:48:56 2016 +0000
- Revision:
- 19:7345688260b2
- Parent:
- 11:49344285c82a
working code to control speedster under SMC with waypoint tracking
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
akashvibhute | 11:49344285c82a | 1 | /** |
akashvibhute | 11:49344285c82a | 2 | * @file attitudeControl.cpp |
akashvibhute | 11:49344285c82a | 3 | * @brief This file implements attitudeController function |
akashvibhute | 11:49344285c82a | 4 | */ |
akashvibhute | 11:49344285c82a | 5 | |
akashvibhute | 11:49344285c82a | 6 | #include "attitudeControl.h" |
akashvibhute | 11:49344285c82a | 7 | |
akashvibhute | 11:49344285c82a | 8 | attitudeControl::attitudeControl() |
akashvibhute | 11:49344285c82a | 9 | { |
akashvibhute | 11:49344285c82a | 10 | // System parameters |
akashvibhute | 11:49344285c82a | 11 | // Sphere |
akashvibhute | 11:49344285c82a | 12 | ms = 20; |
akashvibhute | 11:49344285c82a | 13 | rs = 30; |
akashvibhute | 11:49344285c82a | 14 | rs_inner = 58.5/2; |
akashvibhute | 11:49344285c82a | 15 | Is = ( (2.0/5.0) * ms * (pow(rs,5) - pow(rs_inner,5)) / (pow(rs,3) - pow(rs_inner,3)) ); |
akashvibhute | 11:49344285c82a | 16 | |
akashvibhute | 11:49344285c82a | 17 | // Cart |
akashvibhute | 11:49344285c82a | 18 | mc = 70; |
akashvibhute | 11:49344285c82a | 19 | rc = 4.5; |
akashvibhute | 11:49344285c82a | 20 | ri = (12.76 + 9.5/2); |
akashvibhute | 11:49344285c82a | 21 | Ic_x = 15163.66; |
akashvibhute | 11:49344285c82a | 22 | Ic_z = 13938.73; |
akashvibhute | 11:49344285c82a | 23 | |
akashvibhute | 11:49344285c82a | 24 | // Drive wheel |
akashvibhute | 11:49344285c82a | 25 | mw = 0.19; |
akashvibhute | 11:49344285c82a | 26 | rw = 9.5/2; |
akashvibhute | 11:49344285c82a | 27 | Iw = 2.52; |
akashvibhute | 11:49344285c82a | 28 | h = 14; |
akashvibhute | 11:49344285c82a | 29 | |
akashvibhute | 11:49344285c82a | 30 | // Gravity |
akashvibhute | 11:49344285c82a | 31 | gravity = 9.82*1000; |
akashvibhute | 11:49344285c82a | 32 | |
akashvibhute | 11:49344285c82a | 33 | // Theta and Beta Control |
akashvibhute | 11:49344285c82a | 34 | // Control parameters |
akashvibhute | 11:49344285c82a | 35 | int i; |
akashvibhute | 11:49344285c82a | 36 | i=0; //Control parameters for flag = 0 |
akashvibhute | 19:7345688260b2 | 37 | kx_beta[i]=1; |
akashvibhute | 19:7345688260b2 | 38 | kx_theta[i]= 60;//3; |
akashvibhute | 11:49344285c82a | 39 | ks_beta[i]=30; |
akashvibhute | 11:49344285c82a | 40 | ks_theta[i]=5; |
akashvibhute | 19:7345688260b2 | 41 | kt_beta[i]=30; |
akashvibhute | 11:49344285c82a | 42 | kt_theta[i]=5; |
akashvibhute | 11:49344285c82a | 43 | |
akashvibhute | 11:49344285c82a | 44 | i=1; //Control parameters for flag = 1 |
akashvibhute | 11:49344285c82a | 45 | kx_beta[i]=1; |
akashvibhute | 19:7345688260b2 | 46 | kx_theta[i]=60;//3; |
akashvibhute | 11:49344285c82a | 47 | ks_beta[i]=30; |
akashvibhute | 11:49344285c82a | 48 | ks_theta[i]=5; |
akashvibhute | 11:49344285c82a | 49 | kt_beta[i]=20; |
akashvibhute | 11:49344285c82a | 50 | kt_theta[i]=5; |
akashvibhute | 11:49344285c82a | 51 | |
akashvibhute | 11:49344285c82a | 52 | // Gamma Control |
akashvibhute | 11:49344285c82a | 53 | // Control gains |
akashvibhute | 19:7345688260b2 | 54 | k1 = 30;//250;//160; |
akashvibhute | 19:7345688260b2 | 55 | k2 = 200;//200;//80;//k1*k1/4; |
akashvibhute | 19:7345688260b2 | 56 | |
akashvibhute | 11:49344285c82a | 57 | } |
akashvibhute | 11:49344285c82a | 58 | |
akashvibhute | 11:49344285c82a | 59 | void attitudeControl::GenWheelVelocities(float* w_L, float* w_R, int flag, float tc, |
akashvibhute | 11:49344285c82a | 60 | float beta, float dbeta, float gamma, float dgamma, |
akashvibhute | 11:49344285c82a | 61 | float theta_L, float theta_R, float dtheta_L, float dtheta_R, |
akashvibhute | 11:49344285c82a | 62 | float ref_ddbeta, float ref_dbeta, float ref_beta, |
akashvibhute | 11:49344285c82a | 63 | float ref_ddtheta, float ref_dtheta, float ref_theta, |
akashvibhute | 19:7345688260b2 | 64 | float ref_ddgamma, float ref_dgamma, float ref_gamma, |
akashvibhute | 19:7345688260b2 | 65 | float* u1_, float* u2_) |
akashvibhute | 11:49344285c82a | 66 | { |
akashvibhute | 11:49344285c82a | 67 | // Theta and Beta Control |
akashvibhute | 11:49344285c82a | 68 | if (!flag) ref_theta = 0.5*(theta_L + theta_R); |
akashvibhute | 11:49344285c82a | 69 | |
akashvibhute | 11:49344285c82a | 70 | float theta = 0.5*(theta_L + theta_R); |
akashvibhute | 11:49344285c82a | 71 | float dtheta = 0.5*(dtheta_L + dtheta_R); |
akashvibhute | 11:49344285c82a | 72 | float M11 = (ms+mc)*rs*rs + Is + mc*rc*rc + Ic_x - 2*mc*rc*rs*cos(beta); |
akashvibhute | 11:49344285c82a | 73 | float M12 = ((ms+mc)*rs*rs + Is)*rw/ri + 2*Iw - mc*rc*rs*cos(beta)*rw/ri; |
akashvibhute | 11:49344285c82a | 74 | float fx1 = -(mc*rc*rs*sin(beta)*dbeta*dbeta + mc*rc*gravity*sin(beta))/M11; |
akashvibhute | 11:49344285c82a | 75 | float fx2 = 0; |
akashvibhute | 11:49344285c82a | 76 | float gx1 = -M12/M11; |
akashvibhute | 11:49344285c82a | 77 | float gx2 = 1; |
akashvibhute | 11:49344285c82a | 78 | |
akashvibhute | 11:49344285c82a | 79 | float s_beta = (ref_dbeta - dbeta) + kx_beta[flag]*(ref_beta - beta); |
akashvibhute | 11:49344285c82a | 80 | float s_theta = (ref_dtheta - dtheta) + kx_theta[flag]*(ref_theta - theta); |
akashvibhute | 11:49344285c82a | 81 | |
akashvibhute | 11:49344285c82a | 82 | float temp1 = ref_ddbeta - fx1 + kx_beta[flag]*(ref_dbeta - dbeta) + ks_beta[flag]*tanh(kt_beta[flag]*s_beta); |
akashvibhute | 11:49344285c82a | 83 | float temp2 = ref_ddtheta - fx2 + kx_theta[flag]*(ref_dtheta - dtheta) + ks_theta[flag]*tanh(kt_theta[flag]*s_theta); |
akashvibhute | 11:49344285c82a | 84 | float u1 = (gx1*temp1 + gx2*temp2)/(gx1*gx1 + gx2*gx2); |
akashvibhute | 19:7345688260b2 | 85 | *u1_ = u1; |
akashvibhute | 19:7345688260b2 | 86 | |
akashvibhute | 11:49344285c82a | 87 | // Gamma Control |
akashvibhute | 19:7345688260b2 | 88 | //float dphi = 0.5*(dtheta_R - dtheta_L); |
akashvibhute | 11:49344285c82a | 89 | float dphi = 0.5*(dtheta_R - dtheta_L); |
akashvibhute | 11:49344285c82a | 90 | float gamma_e = ref_gamma - gamma; |
akashvibhute | 19:7345688260b2 | 91 | //convert heading error into -pi -> +pi |
akashvibhute | 19:7345688260b2 | 92 | if(gamma_e < -1*M_PI) gamma_e += 2*M_PI; |
akashvibhute | 19:7345688260b2 | 93 | if(gamma_e > M_PI) gamma_e -= 2*M_PI; |
akashvibhute | 19:7345688260b2 | 94 | |
akashvibhute | 11:49344285c82a | 95 | float dgamma_e = ref_dgamma - dgamma; |
akashvibhute | 11:49344285c82a | 96 | float A = -rw*Is/(h*(Is+Ic_z)); |
akashvibhute | 19:7345688260b2 | 97 | //float u2 = (ref_ddgamma + k1*dgamma_e + k2*gamma_e)/A; |
akashvibhute | 19:7345688260b2 | 98 | float u2 = (ref_ddgamma + k1*ref_dgamma + k2*gamma_e)/A; |
akashvibhute | 19:7345688260b2 | 99 | *u2_ = u2; |
akashvibhute | 19:7345688260b2 | 100 | |
akashvibhute | 11:49344285c82a | 101 | // Control Inputs for the drive wheels |
akashvibhute | 11:49344285c82a | 102 | float dtheta_input = dtheta + u1*tc; |
akashvibhute | 11:49344285c82a | 103 | //float dphi_input = dphi + u2*tc; |
akashvibhute | 19:7345688260b2 | 104 | float dphi_input = u2*tc; |
akashvibhute | 19:7345688260b2 | 105 | //float dphi_input = 0.0; |
akashvibhute | 11:49344285c82a | 106 | *w_L = dtheta_input - dphi_input; |
akashvibhute | 11:49344285c82a | 107 | *w_R = dtheta_input + dphi_input; |
akashvibhute | 11:49344285c82a | 108 | } |