Ted Parrott / Mbed 2 deprecated Frontal_controller

NA

Dependencies:   AX12 BMA180_2 L3G4200D_1 mbed

Fork of AX12-HelloWorld by Chris Styles

main.cpp@2:f93868975538, 2017-11-16 (annotated)

Committer:
tedparrott6
Date:
Thu Nov 16 15:00:17 2017 +0000
Revision:
2:f93868975538
Parent:
1:b12b06e2fc2d 
NA

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chris 0:f6f8cf11779f 1 #include "mbed.h"
tedparrott6 2:f93868975538 2 #include "BMA180.h"
tedparrott6 2:f93868975538 3 #include "math.h"
tedparrott6 2:f93868975538 4 #include "L3G4200D.h"
chris 0:f6f8cf11779f 5 #include "AX12.h"
chris 0:f6f8cf11779f 6
tedparrott6 2:f93868975538 7 Serial pc(USBTX, USBRX);
tedparrott6 2:f93868975538 8 L3G4200D gyro(p9,p10);
tedparrott6 2:f93868975538 9 BMA180 my_BMA180(p5,p6,p7,p15,p16);
tedparrott6 2:f93868975538 10
tedparrott6 2:f93868975538 11 AX12 m1(p28,p27,1);
tedparrott6 2:f93868975538 12 AX12 m2(p28,p27,2);
tedparrott6 2:f93868975538 13 AX12 m3(p28,p27,3);
tedparrott6 2:f93868975538 14 AX12 m4(p28,p27,4);
tedparrott6 2:f93868975538 15 AX12 m5(p28,p27,5);
tedparrott6 2:f93868975538 16 AX12 m6(p28,p27,6);
tedparrott6 2:f93868975538 17 AX12 m7(p28,p27,7);
tedparrott6 2:f93868975538 18 AX12 m8(p28,p27,8);
tedparrott6 2:f93868975538 19 AX12 m9(p28,p27,9);
tedparrott6 2:f93868975538 20 AX12 m10(p28,p27,10);
chris 0:f6f8cf11779f 21
tedparrott6 2:f93868975538 22 float l_theta_k;
tedparrott6 2:f93868975538 23 float l_theta_h;
tedparrott6 2:f93868975538 24 float l_theta_a_x;
tedparrott6 2:f93868975538 25 float l_theta_a_x_rad;
tedparrott6 2:f93868975538 26 float l_theta_7_x;
tedparrott6 2:f93868975538 27 float l_theta_8_x;
tedparrott6 2:f93868975538 28 float l_theta_a_y;
tedparrott6 2:f93868975538 29 float l_theta_a_y_rad;
tedparrott6 2:f93868975538 30 float l_theta_7_y;
tedparrott6 2:f93868975538 31 float l_theta_8_y;
tedparrott6 2:f93868975538 32 float l_theta_8_r_y;
tedparrott6 2:f93868975538 33 float l_theta_5;
tedparrott6 2:f93868975538 34 float l_theta_7_m;
tedparrott6 2:f93868975538 35 float l_theta_8_m;
tedparrott6 2:f93868975538 36 float l_theta_6;
tedparrott6 2:f93868975538 37 float h_l;
tedparrott6 2:f93868975538 38 float r_theta_k;
tedparrott6 2:f93868975538 39 float r_theta_h;
tedparrott6 2:f93868975538 40 float r_theta_a_x;
tedparrott6 2:f93868975538 41 float r_theta_a_x_rad;
tedparrott6 2:f93868975538 42 float r_theta_2_x;
tedparrott6 2:f93868975538 43 float r_theta_1_x;
tedparrott6 2:f93868975538 44 float r_theta_a_y;
tedparrott6 2:f93868975538 45 float r_theta_a_y_rad;
tedparrott6 2:f93868975538 46 float r_theta_2_y;
tedparrott6 2:f93868975538 47 float r_theta_1_y;
tedparrott6 2:f93868975538 48 float r_theta_1_r_y;
tedparrott6 2:f93868975538 49 float r_theta_4;
tedparrott6 2:f93868975538 50 float r_theta_2_m;
tedparrott6 2:f93868975538 51 float r_theta_1_m;
tedparrott6 2:f93868975538 52 float r_theta_3;
tedparrott6 2:f93868975538 53 float h_r;
tedparrott6 2:f93868975538 54 float angle_read;
tedparrott6 2:f93868975538 55 float angle_read_rad;
tedparrott6 2:f93868975538 56 float speed_setpoint;
tedparrott6 2:f93868975538 57 float speed_error;
tedparrott6 2:f93868975538 58 float pos_setpoint;
tedparrott6 2:f93868975538 59 float pos_error;
tedparrott6 2:f93868975538 60 float kp;
tedparrott6 2:f93868975538 61 float kd;
tedparrott6 2:f93868975538 62 float angle_x;
tedparrott6 2:f93868975538 63 float angle_x_a;
tedparrott6 2:f93868975538 64 float angle_y;
tedparrott6 2:f93868975538 65 float angle_y_a;
tedparrott6 2:f93868975538 66 float r1;
tedparrott6 2:f93868975538 67 float r2;
tedparrott6 2:f93868975538 68 float pi = 3.14159;
tedparrott6 2:f93868975538 69 float speed_x;
tedparrott6 2:f93868975538 70 float speed_y;
tedparrott6 2:f93868975538 71 float g[3];
tedparrott6 2:f93868975538 72 float output_angle_x;
tedparrott6 2:f93868975538 73 float output_angle_y;
tedparrott6 2:f93868975538 74 int output_angle_d_x;
tedparrott6 2:f93868975538 75 int output_angle_d_y;
tedparrott6 2:f93868975538 76 float force_mag;
tedparrott6 2:f93868975538 77 float control_action;
tedparrott6 2:f93868975538 78
tedparrott6 2:f93868975538 79
tedparrott6 2:f93868975538 80
tedparrott6 2:f93868975538 81 int main()
tedparrott6 2:f93868975538 82 {
tedparrott6 2:f93868975538 83 m5.SetGoal(150);
tedparrott6 2:f93868975538 84 wait(1);
tedparrott6 2:f93868975538 85 m6.SetGoal(150);
tedparrott6 2:f93868975538 86 wait(1);
tedparrott6 2:f93868975538 87 m7.SetGoal(150);
tedparrott6 2:f93868975538 88 wait(1);
tedparrott6 2:f93868975538 89 m8.SetGoal(150);
tedparrott6 2:f93868975538 90 wait(1);
tedparrott6 2:f93868975538 91 m1.SetGoal(150);
tedparrott6 2:f93868975538 92 wait(1);
tedparrott6 2:f93868975538 93 m2.SetGoal(150);
tedparrott6 2:f93868975538 94 wait(1);
tedparrott6 2:f93868975538 95 m3.SetGoal(150);
tedparrott6 2:f93868975538 96 wait(1);
tedparrott6 2:f93868975538 97 m4.SetGoal(150);
tedparrott6 2:f93868975538 98 wait(1);
tedparrott6 2:f93868975538 99 m9.SetGoal(150);
tedparrott6 2:f93868975538 100 wait(1);
tedparrott6 2:f93868975538 101 m10.SetGoal(150);
tedparrott6 2:f93868975538 102 wait(1);
tedparrott6 2:f93868975538 103 h_l = 0;
tedparrott6 2:f93868975538 104 h_r = 0;
tedparrott6 2:f93868975538 105
tedparrott6 2:f93868975538 106 angle_x = 0;
tedparrott6 2:f93868975538 107 angle_y = 0;
tedparrott6 2:f93868975538 108 speed_setpoint = 0;
tedparrott6 2:f93868975538 109 pos_setpoint = -3;
tedparrott6 2:f93868975538 110
tedparrott6 2:f93868975538 111 kd = 0;
tedparrott6 2:f93868975538 112 kp = 2;
tedparrott6 2:f93868975538 113
tedparrott6 2:f93868975538 114 while(1) {
tedparrott6 2:f93868975538 115 gyro.read(g); // Read and Interpret Gyro Data
tedparrott6 2:f93868975538 116 speed_x = ((g[1])*0.07) - 1.12;
tedparrott6 2:f93868975538 117 speed_y = ((g[0])*0.07) - 0.595;
chris 1:b12b06e2fc2d 118
tedparrott6 2:f93868975538 119 int x_msb, y_msb, z_msb;
tedparrott6 2:f93868975538 120 char x_lsb, y_lsb, z_lsb;
tedparrott6 2:f93868975538 121 short ax, ay, az;
tedparrott6 2:f93868975538 122 float afx, afy, afz;
tedparrott6 2:f93868975538 123
tedparrott6 2:f93868975538 124 x_lsb = my_BMA180.readReg(ACCXLSB); // Read X LSB register
tedparrott6 2:f93868975538 125 x_msb = my_BMA180.readReg(ACCXMSB); // Read X MSB register
tedparrott6 2:f93868975538 126 ax = (x_msb << 8) | x_lsb; // Concatinate X MSB and LSB
tedparrott6 2:f93868975538 127 ax = ax >> 2; // Remove unused first 2 LSB (16 bits to 14 bits)
tedparrott6 2:f93868975538 128 afx = (float)ax*3/16384;
tedparrott6 2:f93868975538 129
tedparrott6 2:f93868975538 130 y_lsb = my_BMA180.readReg(ACCYLSB); // Read Y LSB register
tedparrott6 2:f93868975538 131 y_msb = my_BMA180.readReg(ACCYMSB); // Read Y MSB register
tedparrott6 2:f93868975538 132 ay = (y_msb << 8) | y_lsb; // Concatinate Y MSB and LSB
tedparrott6 2:f93868975538 133 ay = ay >> 2; // Remove unused first 2 LSB
tedparrott6 2:f93868975538 134 afy = (float)ay*3/16384;
tedparrott6 2:f93868975538 135
tedparrott6 2:f93868975538 136 z_lsb = my_BMA180.readReg(ACCZLSB); // Read Z LSB register
tedparrott6 2:f93868975538 137 z_msb = my_BMA180.readReg(ACCZMSB); // Read Z MSB register
tedparrott6 2:f93868975538 138 az = (z_msb << 8) | z_lsb; // Concatinate Z MSB and LSB
tedparrott6 2:f93868975538 139 az = az >> 2; // Remove unused first 2 LSB
tedparrott6 2:f93868975538 140 afz = (float)az*3/16384;
tedparrott6 2:f93868975538 141
tedparrott6 2:f93868975538 142 r1 = afx / (sqrt(pow(afy,2) + pow(afz,2))); // Determine X component of gravity force
tedparrott6 2:f93868975538 143 r2 = afy / (sqrt(pow(afx,2) + pow(afz,2))); // Determine Y component of gravity force
tedparrott6 2:f93868975538 144 angle_x_a = atan(r1)*180/pi; // Determine X, Y angles
tedparrott6 2:f93868975538 145 angle_y_a = atan(r2)*180/pi;
tedparrott6 2:f93868975538 146
tedparrott6 2:f93868975538 147 force_mag = abs(az) + abs(ay) + abs(ax); // Check force magnitude to reduce effects of shocks/vibration
tedparrott6 2:f93868975538 148
tedparrott6 2:f93868975538 149
tedparrott6 2:f93868975538 150 angle_x = 0.98*(angle_x + speed_x*0.005) + 0.02*angle_x_a;
tedparrott6 2:f93868975538 151 output_angle_x = angle_x;
tedparrott6 2:f93868975538 152 output_angle_d_x = output_angle_x;
tedparrott6 2:f93868975538 153
tedparrott6 2:f93868975538 154 angle_y = 0.98*(angle_y + speed_y*0.005) + 0.02*angle_y_a;
tedparrott6 2:f93868975538 155 output_angle_y = angle_y;
tedparrott6 2:f93868975538 156 output_angle_d_y = output_angle_y;
tedparrott6 2:f93868975538 157
tedparrott6 2:f93868975538 158 pos_error = pos_setpoint - output_angle_x;
tedparrott6 2:f93868975538 159 speed_error = speed_setpoint - speed_x;
tedparrott6 2:f93868975538 160
tedparrott6 2:f93868975538 161 control_action = kp*pos_error + kd*speed_error;
tedparrott6 2:f93868975538 162 if(control_action > 8){
tedparrott6 2:f93868975538 163 control_action = 8;
tedparrott6 2:f93868975538 164 }else if(control_action < -8){
tedparrott6 2:f93868975538 165 control_action = -8;
tedparrott6 2:f93868975538 166 }else{
tedparrott6 2:f93868975538 167 control_action = control_action;
tedparrott6 2:f93868975538 168 }
tedparrott6 2:f93868975538 169 angle_read = control_action;
tedparrott6 2:f93868975538 170 angle_read_rad = angle_read*(3.14159/180);
tedparrott6 2:f93868975538 171
tedparrott6 2:f93868975538 172 if(angle_read == 0){
tedparrott6 2:f93868975538 173 h_l = 0;
tedparrott6 2:f93868975538 174 h_r = 0;
tedparrott6 2:f93868975538 175 }else if(angle_read > 0){
tedparrott6 2:f93868975538 176 h_l = 79*tan(angle_read_rad);
tedparrott6 2:f93868975538 177 h_r = 0;
tedparrott6 2:f93868975538 178 }else if(angle_read < 0){
tedparrott6 2:f93868975538 179 angle_read_rad = abs(angle_read_rad);
tedparrott6 2:f93868975538 180 h_l = 0;
tedparrott6 2:f93868975538 181 h_r = 79*tan(angle_read_rad);
tedparrott6 2:f93868975538 182 }
tedparrott6 2:f93868975538 183 l_theta_h = ((180/3.14159)*acos(1-(h_l/204)));
tedparrott6 2:f93868975538 184 l_theta_k = 2*l_theta_h;
tedparrott6 2:f93868975538 185 l_theta_5 = 150 + l_theta_h;
tedparrott6 2:f93868975538 186 l_theta_6 = 150 + l_theta_k;
tedparrott6 2:f93868975538 187
tedparrott6 2:f93868975538 188 r_theta_h = ((180/3.14159)*acos(1-(h_r/204)));
tedparrott6 2:f93868975538 189 r_theta_k = 2*r_theta_h;
tedparrott6 2:f93868975538 190 r_theta_4 = 150 - r_theta_h;
tedparrott6 2:f93868975538 191 r_theta_3 = 150 - r_theta_k;
tedparrott6 2:f93868975538 192
tedparrott6 2:f93868975538 193 l_theta_a_x = l_theta_h;
tedparrott6 2:f93868975538 194 l_theta_a_x_rad = l_theta_a_x*(3.14159/180);
tedparrott6 2:f93868975538 195 l_theta_8_x = (l_theta_a_x/0.34);
tedparrott6 2:f93868975538 196 l_theta_7_x = ((180/3.14159)*(asin((11.4/8)*l_theta_a_x_rad)));
tedparrott6 2:f93868975538 197
tedparrott6 2:f93868975538 198 l_theta_a_y = angle_read;
tedparrott6 2:f93868975538 199 l_theta_a_y_rad = l_theta_a_y*3.14159/180;
tedparrott6 2:f93868975538 200 l_theta_7_y = -1.43*l_theta_a_y;
tedparrott6 2:f93868975538 201 l_theta_8_r_y = -asin((20.94/8)*sin(l_theta_a_y_rad));
tedparrott6 2:f93868975538 202 l_theta_8_y= (l_theta_8_r_y*180/3.14159);
tedparrott6 2:f93868975538 203
tedparrott6 2:f93868975538 204 r_theta_a_x = r_theta_h;
tedparrott6 2:f93868975538 205 r_theta_a_x_rad = r_theta_a_x*(3.14159/180);
tedparrott6 2:f93868975538 206 r_theta_1_x = (r_theta_a_x/0.34);
tedparrott6 2:f93868975538 207 r_theta_2_x = ((180/3.14159)*(asin((11.4/8)*r_theta_a_x_rad)));
tedparrott6 2:f93868975538 208
tedparrott6 2:f93868975538 209 r_theta_a_y = angle_read;
tedparrott6 2:f93868975538 210 r_theta_a_y_rad = r_theta_a_y*3.14159/180;
tedparrott6 2:f93868975538 211 r_theta_2_y = -1.43*r_theta_a_y;
tedparrott6 2:f93868975538 212 r_theta_1_r_y = -asin((20.94/8)*sin(r_theta_a_y_rad));
tedparrott6 2:f93868975538 213 r_theta_1_y= (r_theta_1_r_y*180/3.14159);
tedparrott6 2:f93868975538 214
tedparrott6 2:f93868975538 215 l_theta_7_m = 150 + l_theta_7_x + l_theta_7_y;
tedparrott6 2:f93868975538 216 l_theta_8_m = 150 - l_theta_8_x + l_theta_8_y;
tedparrott6 2:f93868975538 217
tedparrott6 2:f93868975538 218 r_theta_2_m = 150 - r_theta_2_x - r_theta_2_y;
tedparrott6 2:f93868975538 219 r_theta_1_m = 150 + r_theta_1_x - r_theta_1_y;
tedparrott6 2:f93868975538 220
tedparrott6 2:f93868975538 221 m1.SetGoal(r_theta_1_m);
tedparrott6 2:f93868975538 222 m2.SetGoal(r_theta_2_m);
tedparrott6 2:f93868975538 223 m3.SetGoal(r_theta_3);
tedparrott6 2:f93868975538 224 m4.SetGoal(r_theta_4);
tedparrott6 2:f93868975538 225 m5.SetGoal(l_theta_5);
tedparrott6 2:f93868975538 226 m6.SetGoal(l_theta_6);
tedparrott6 2:f93868975538 227 m7.SetGoal(l_theta_7_m);
tedparrott6 2:f93868975538 228 m8.SetGoal(l_theta_8_m);
tedparrott6 2:f93868975538 229 pc.printf("\n\rAngle: %05f",output_angle_y);
tedparrott6 2:f93868975538 230
chris 0:f6f8cf11779f 231 }
chris 0:f6f8cf11779f 232 }