Team Virgo v3 / Orion_newPCB_test_LV

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 Team Virgo v3

06_AttitudeControl/attitudeControl.cpp@30:44676e1b38f8, 2018-03-20 (annotated)

Committer:
ahmed_lv
Date:
Tue Mar 20 05:56:22 2018 +0000
Revision:
30:44676e1b38f8
Parent:
19:7345688260b2 
Editted Input Variables to PID

Who changed what in which revision?

UserRevisionLine numberNew 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 }