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@11:49344285c82a, 2016-04-24 (annotated)
- Committer:
- akashvibhute
- Date:
- Sun Apr 24 23:54:50 2016 +0000
- Revision:
- 11:49344285c82a
- Child:
- 19:7345688260b2
Purepursuit + pid pitch control works nicely; this is the demo setup for the french contigent - 25/Apr/2016
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 | 11:49344285c82a | 37 | kx_beta[i]=1000; |
akashvibhute | 11:49344285c82a | 38 | kx_theta[i]=3; |
akashvibhute | 11:49344285c82a | 39 | ks_beta[i]=30; |
akashvibhute | 11:49344285c82a | 40 | ks_theta[i]=5; |
akashvibhute | 11:49344285c82a | 41 | kt_beta[i]=20; |
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 | 11:49344285c82a | 46 | kx_theta[i]=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 | 11:49344285c82a | 54 | k1 = 2; |
akashvibhute | 11:49344285c82a | 55 | k2 = k1*k1/4; |
akashvibhute | 11:49344285c82a | 56 | } |
akashvibhute | 11:49344285c82a | 57 | |
akashvibhute | 11:49344285c82a | 58 | void attitudeControl::GenWheelVelocities(float* w_L, float* w_R, int flag, float tc, |
akashvibhute | 11:49344285c82a | 59 | float beta, float dbeta, float gamma, float dgamma, |
akashvibhute | 11:49344285c82a | 60 | float theta_L, float theta_R, float dtheta_L, float dtheta_R, |
akashvibhute | 11:49344285c82a | 61 | float ref_ddbeta, float ref_dbeta, float ref_beta, |
akashvibhute | 11:49344285c82a | 62 | float ref_ddtheta, float ref_dtheta, float ref_theta, |
akashvibhute | 11:49344285c82a | 63 | float ref_ddgamma, float ref_dgamma, float ref_gamma) |
akashvibhute | 11:49344285c82a | 64 | { |
akashvibhute | 11:49344285c82a | 65 | // Theta and Beta Control |
akashvibhute | 11:49344285c82a | 66 | if (!flag) ref_theta = 0.5*(theta_L + theta_R); |
akashvibhute | 11:49344285c82a | 67 | |
akashvibhute | 11:49344285c82a | 68 | float theta = 0.5*(theta_L + theta_R); |
akashvibhute | 11:49344285c82a | 69 | float dtheta = 0.5*(dtheta_L + dtheta_R); |
akashvibhute | 11:49344285c82a | 70 | float M11 = (ms+mc)*rs*rs + Is + mc*rc*rc + Ic_x - 2*mc*rc*rs*cos(beta); |
akashvibhute | 11:49344285c82a | 71 | float M12 = ((ms+mc)*rs*rs + Is)*rw/ri + 2*Iw - mc*rc*rs*cos(beta)*rw/ri; |
akashvibhute | 11:49344285c82a | 72 | float fx1 = -(mc*rc*rs*sin(beta)*dbeta*dbeta + mc*rc*gravity*sin(beta))/M11; |
akashvibhute | 11:49344285c82a | 73 | float fx2 = 0; |
akashvibhute | 11:49344285c82a | 74 | float gx1 = -M12/M11; |
akashvibhute | 11:49344285c82a | 75 | float gx2 = 1; |
akashvibhute | 11:49344285c82a | 76 | |
akashvibhute | 11:49344285c82a | 77 | float s_beta = (ref_dbeta - dbeta) + kx_beta[flag]*(ref_beta - beta); |
akashvibhute | 11:49344285c82a | 78 | float s_theta = (ref_dtheta - dtheta) + kx_theta[flag]*(ref_theta - theta); |
akashvibhute | 11:49344285c82a | 79 | |
akashvibhute | 11:49344285c82a | 80 | float temp1 = ref_ddbeta - fx1 + kx_beta[flag]*(ref_dbeta - dbeta) + ks_beta[flag]*tanh(kt_beta[flag]*s_beta); |
akashvibhute | 11:49344285c82a | 81 | float temp2 = ref_ddtheta - fx2 + kx_theta[flag]*(ref_dtheta - dtheta) + ks_theta[flag]*tanh(kt_theta[flag]*s_theta); |
akashvibhute | 11:49344285c82a | 82 | float u1 = (gx1*temp1 + gx2*temp2)/(gx1*gx1 + gx2*gx2); |
akashvibhute | 11:49344285c82a | 83 | |
akashvibhute | 11:49344285c82a | 84 | // Gamma Control |
akashvibhute | 11:49344285c82a | 85 | float dphi = 0.5*(dtheta_R - dtheta_L); |
akashvibhute | 11:49344285c82a | 86 | float gamma_e = ref_gamma - gamma; |
akashvibhute | 11:49344285c82a | 87 | float dgamma_e = ref_dgamma - dgamma; |
akashvibhute | 11:49344285c82a | 88 | float A = -rw*Is/(h*(Is+Ic_z)); |
akashvibhute | 11:49344285c82a | 89 | float u2 = (ref_ddgamma + k1*dgamma_e + k2*gamma_e)/A; |
akashvibhute | 11:49344285c82a | 90 | |
akashvibhute | 11:49344285c82a | 91 | // Control Inputs for the drive wheels |
akashvibhute | 11:49344285c82a | 92 | float dtheta_input = dtheta + u1*tc; |
akashvibhute | 11:49344285c82a | 93 | //float dphi_input = dphi + u2*tc; |
akashvibhute | 11:49344285c82a | 94 | float dphi_input = 0.0; |
akashvibhute | 11:49344285c82a | 95 | *w_L = dtheta_input - dphi_input; |
akashvibhute | 11:49344285c82a | 96 | *w_R = dtheta_input + dphi_input; |
akashvibhute | 11:49344285c82a | 97 | } |