NA
Dependencies: AX12 BMA180_2 L3G4200D_1 mbed
Fork of AX12-HelloWorld by
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?
User | Revision | Line number | New 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 | } |