NA
Dependencies: AX12 BMA180_4 L3G4200D_3 mbed
Fork of AX12-HelloWorld by
main.cpp@2:056f36912bf0, 2017-11-16 (annotated)
- Committer:
- tedparrott6
- Date:
- Thu Nov 16 15:05:18 2017 +0000
- Revision:
- 2:056f36912bf0
- 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:056f36912bf0 | 2 | #include "BMA180.h" |
tedparrott6 | 2:056f36912bf0 | 3 | #include "math.h" |
tedparrott6 | 2:056f36912bf0 | 4 | #include "L3G4200D.h" |
chris | 0:f6f8cf11779f | 5 | #include "AX12.h" |
chris | 0:f6f8cf11779f | 6 | |
tedparrott6 | 2:056f36912bf0 | 7 | L3G4200D gyro(p9,p10); |
tedparrott6 | 2:056f36912bf0 | 8 | BMA180 my_BMA180(p5,p6,p7,p15,p16); |
tedparrott6 | 2:056f36912bf0 | 9 | |
tedparrott6 | 2:056f36912bf0 | 10 | AX12 m1 (p28, p27, 1); |
tedparrott6 | 2:056f36912bf0 | 11 | AX12 m2 (p28, p27, 2); |
tedparrott6 | 2:056f36912bf0 | 12 | AX12 m3 (p28, p27, 3); |
tedparrott6 | 2:056f36912bf0 | 13 | AX12 m4 (p28, p27, 4); |
tedparrott6 | 2:056f36912bf0 | 14 | AX12 m5 (p28, p27, 5); |
tedparrott6 | 2:056f36912bf0 | 15 | AX12 m6 (p28, p27, 6); |
tedparrott6 | 2:056f36912bf0 | 16 | AX12 m7 (p28, p27, 7); |
tedparrott6 | 2:056f36912bf0 | 17 | AX12 m8 (p28, p27, 8); |
tedparrott6 | 2:056f36912bf0 | 18 | AX12 m9 (p28, p27, 9); |
tedparrott6 | 2:056f36912bf0 | 19 | AX12 m10 (p28, p27, 10); |
tedparrott6 | 2:056f36912bf0 | 20 | |
tedparrott6 | 2:056f36912bf0 | 21 | Timer t_1; |
tedparrott6 | 2:056f36912bf0 | 22 | Timer t_2; |
chris | 0:f6f8cf11779f | 23 | |
tedparrott6 | 2:056f36912bf0 | 24 | float theta_1; |
tedparrott6 | 2:056f36912bf0 | 25 | float theta_1_r; |
tedparrott6 | 2:056f36912bf0 | 26 | float theta_2; |
tedparrott6 | 2:056f36912bf0 | 27 | float theta_8; |
tedparrott6 | 2:056f36912bf0 | 28 | float theta_8_r; |
tedparrott6 | 2:056f36912bf0 | 29 | float theta_7; |
tedparrott6 | 2:056f36912bf0 | 30 | float time_1; |
tedparrott6 | 2:056f36912bf0 | 31 | float time_2; |
tedparrott6 | 2:056f36912bf0 | 32 | float angle_x; |
tedparrott6 | 2:056f36912bf0 | 33 | float angle_x_a; |
tedparrott6 | 2:056f36912bf0 | 34 | float angle_y; |
tedparrott6 | 2:056f36912bf0 | 35 | float angle_y_a; |
tedparrott6 | 2:056f36912bf0 | 36 | float r1; |
tedparrott6 | 2:056f36912bf0 | 37 | float r2; |
tedparrott6 | 2:056f36912bf0 | 38 | float pi = 3.14159; |
tedparrott6 | 2:056f36912bf0 | 39 | float speed_x; |
tedparrott6 | 2:056f36912bf0 | 40 | float speed_y; |
tedparrott6 | 2:056f36912bf0 | 41 | float g[3]; |
tedparrott6 | 2:056f36912bf0 | 42 | float theta_roll; |
tedparrott6 | 2:056f36912bf0 | 43 | float theta_a_right; |
tedparrott6 | 2:056f36912bf0 | 44 | float theta_a_left; |
tedparrott6 | 2:056f36912bf0 | 45 | float theta_a_right_rad; |
tedparrott6 | 2:056f36912bf0 | 46 | float theta_a_left_rad; |
tedparrott6 | 2:056f36912bf0 | 47 | float time_sum; |
tedparrott6 | 2:056f36912bf0 | 48 | float period; |
tedparrott6 | 2:056f36912bf0 | 49 | int n; |
tedparrott6 | 2:056f36912bf0 | 50 | float A; |
tedparrott6 | 2:056f36912bf0 | 51 | float B; |
tedparrott6 | 2:056f36912bf0 | 52 | float C; |
tedparrott6 | 2:056f36912bf0 | 53 | float leg_control_signal; |
tedparrott6 | 2:056f36912bf0 | 54 | float theta_4; |
tedparrott6 | 2:056f36912bf0 | 55 | float theta_5; |
tedparrott6 | 2:056f36912bf0 | 56 | int dir_0; |
tedparrott6 | 2:056f36912bf0 | 57 | int dir_1; |
tedparrott6 | 2:056f36912bf0 | 58 | int x_msb, y_msb, z_msb; |
tedparrott6 | 2:056f36912bf0 | 59 | char x_lsb, y_lsb, z_lsb; |
tedparrott6 | 2:056f36912bf0 | 60 | short ax, ay, az; |
tedparrott6 | 2:056f36912bf0 | 61 | float afx, afy, afz; |
tedparrott6 | 2:056f36912bf0 | 62 | |
tedparrott6 | 2:056f36912bf0 | 63 | int main() |
tedparrott6 | 2:056f36912bf0 | 64 | { |
tedparrott6 | 2:056f36912bf0 | 65 | m1.SetGoal(150); |
tedparrott6 | 2:056f36912bf0 | 66 | wait(0.5); |
tedparrott6 | 2:056f36912bf0 | 67 | m2.SetGoal(150); |
tedparrott6 | 2:056f36912bf0 | 68 | wait(0.5); |
tedparrott6 | 2:056f36912bf0 | 69 | m3.SetGoal(150); |
tedparrott6 | 2:056f36912bf0 | 70 | wait(0.5); |
tedparrott6 | 2:056f36912bf0 | 71 | m4.SetGoal(150); |
tedparrott6 | 2:056f36912bf0 | 72 | wait(0.5); |
tedparrott6 | 2:056f36912bf0 | 73 | m5.SetGoal(150); |
tedparrott6 | 2:056f36912bf0 | 74 | wait(0.5); |
tedparrott6 | 2:056f36912bf0 | 75 | m6.SetGoal(150); |
tedparrott6 | 2:056f36912bf0 | 76 | wait(0.5); |
tedparrott6 | 2:056f36912bf0 | 77 | m7.SetGoal(150); |
tedparrott6 | 2:056f36912bf0 | 78 | wait(0.5); |
tedparrott6 | 2:056f36912bf0 | 79 | m8.SetGoal(150); |
tedparrott6 | 2:056f36912bf0 | 80 | wait(0.5); |
tedparrott6 | 2:056f36912bf0 | 81 | m9.SetGoal(150); |
tedparrott6 | 2:056f36912bf0 | 82 | wait(0.5); |
tedparrott6 | 2:056f36912bf0 | 83 | m10.SetGoal(150); |
tedparrott6 | 2:056f36912bf0 | 84 | wait(0.5); |
tedparrott6 | 2:056f36912bf0 | 85 | |
tedparrott6 | 2:056f36912bf0 | 86 | t_1.start(); |
tedparrott6 | 2:056f36912bf0 | 87 | t_2.start(); |
tedparrott6 | 2:056f36912bf0 | 88 | time_sum = 0; |
tedparrott6 | 2:056f36912bf0 | 89 | n = 0; |
tedparrott6 | 2:056f36912bf0 | 90 | angle_y = 0; |
tedparrott6 | 2:056f36912bf0 | 91 | dir_1 = 0; |
tedparrott6 | 2:056f36912bf0 | 92 | dir_0 = 0; |
chris | 1:b12b06e2fc2d | 93 | |
chris | 0:f6f8cf11779f | 94 | while (1) { |
tedparrott6 | 2:056f36912bf0 | 95 | gyro.read(g); // Read and Interpret Gyro Data |
tedparrott6 | 2:056f36912bf0 | 96 | speed_x = ((g[1])*0.07) - 1.12; |
tedparrott6 | 2:056f36912bf0 | 97 | speed_y = ((g[0])*0.07) - 0.3; |
tedparrott6 | 2:056f36912bf0 | 98 | |
tedparrott6 | 2:056f36912bf0 | 99 | x_lsb = my_BMA180.readReg(ACCXLSB); // Read X LSB register |
tedparrott6 | 2:056f36912bf0 | 100 | x_msb = my_BMA180.readReg(ACCXMSB); // Read X MSB register |
tedparrott6 | 2:056f36912bf0 | 101 | ax = (x_msb << 8) | x_lsb; // Concatinate X MSB and LSB |
tedparrott6 | 2:056f36912bf0 | 102 | ax = ax >> 2; // Remove unused first 2 LSB (16 bits to 14 bits) |
tedparrott6 | 2:056f36912bf0 | 103 | afx = (float)ax*3/16384; |
tedparrott6 | 2:056f36912bf0 | 104 | |
tedparrott6 | 2:056f36912bf0 | 105 | y_lsb = my_BMA180.readReg(ACCYLSB); // Read Y LSB register |
tedparrott6 | 2:056f36912bf0 | 106 | y_msb = my_BMA180.readReg(ACCYMSB); // Read Y MSB register |
tedparrott6 | 2:056f36912bf0 | 107 | ay = (y_msb << 8) | y_lsb; // Concatinate Y MSB and LSB |
tedparrott6 | 2:056f36912bf0 | 108 | ay = ay >> 2; // Remove unused first 2 LSB |
tedparrott6 | 2:056f36912bf0 | 109 | afy = (float)ay*3/16384; |
tedparrott6 | 2:056f36912bf0 | 110 | |
tedparrott6 | 2:056f36912bf0 | 111 | z_lsb = my_BMA180.readReg(ACCZLSB); // Read Z LSB register |
tedparrott6 | 2:056f36912bf0 | 112 | z_msb = my_BMA180.readReg(ACCZMSB); // Read Z MSB register |
tedparrott6 | 2:056f36912bf0 | 113 | az = (z_msb << 8) | z_lsb; // Concatinate Z MSB and LSB |
tedparrott6 | 2:056f36912bf0 | 114 | az = az >> 2; // Remove unused first 2 LSB |
tedparrott6 | 2:056f36912bf0 | 115 | afz = (float)az*3/16384; |
tedparrott6 | 2:056f36912bf0 | 116 | |
tedparrott6 | 2:056f36912bf0 | 117 | r1 = afx / (sqrt(pow(afy,2) + pow(afz,2))); // Determine X component of gravity force |
tedparrott6 | 2:056f36912bf0 | 118 | r2 = afy / (sqrt(pow(afx,2) + pow(afz,2))); // Determine Y component of gravity force |
tedparrott6 | 2:056f36912bf0 | 119 | angle_x_a = atan(r1)*180/pi; // Determine X, Y angles |
tedparrott6 | 2:056f36912bf0 | 120 | angle_y_a = atan(r2)*180/pi; |
tedparrott6 | 2:056f36912bf0 | 121 | |
tedparrott6 | 2:056f36912bf0 | 122 | angle_x = 0.98*(angle_x + speed_x*0.005) + 0.02*angle_x_a; |
tedparrott6 | 2:056f36912bf0 | 123 | |
tedparrott6 | 2:056f36912bf0 | 124 | angle_y = 0.98*(angle_y + speed_y*0.005) + 0.02*angle_y_a; |
tedparrott6 | 2:056f36912bf0 | 125 | theta_roll = angle_y + 4.5; |
tedparrott6 | 2:056f36912bf0 | 126 | |
tedparrott6 | 2:056f36912bf0 | 127 | dir_0 = dir_1; |
tedparrott6 | 2:056f36912bf0 | 128 | |
tedparrott6 | 2:056f36912bf0 | 129 | if(theta_roll >= 0) { |
tedparrott6 | 2:056f36912bf0 | 130 | dir_1 = 1; |
tedparrott6 | 2:056f36912bf0 | 131 | } else { |
tedparrott6 | 2:056f36912bf0 | 132 | dir_1 = 0; |
tedparrott6 | 2:056f36912bf0 | 133 | } |
tedparrott6 | 2:056f36912bf0 | 134 | |
tedparrott6 | 2:056f36912bf0 | 135 | if(dir_1 != dir_0) { |
tedparrott6 | 2:056f36912bf0 | 136 | time_2 = t_2.read(); |
tedparrott6 | 2:056f36912bf0 | 137 | t_2.reset(); |
tedparrott6 | 2:056f36912bf0 | 138 | time_sum = time_sum + time_2; |
tedparrott6 | 2:056f36912bf0 | 139 | n=n+1; |
tedparrott6 | 2:056f36912bf0 | 140 | } |
tedparrott6 | 2:056f36912bf0 | 141 | |
tedparrott6 | 2:056f36912bf0 | 142 | period = 2*(time_sum/n); |
tedparrott6 | 2:056f36912bf0 | 143 | B = 2*3.14159/period; |
tedparrott6 | 2:056f36912bf0 | 144 | C = -(period/4)*B; |
tedparrott6 | 2:056f36912bf0 | 145 | A = 5; |
tedparrott6 | 2:056f36912bf0 | 146 | |
tedparrott6 | 2:056f36912bf0 | 147 | if(theta_roll < -3) { |
tedparrott6 | 2:056f36912bf0 | 148 | theta_a_right = -7; |
tedparrott6 | 2:056f36912bf0 | 149 | } else { |
tedparrott6 | 2:056f36912bf0 | 150 | theta_a_right = 0; |
tedparrott6 | 2:056f36912bf0 | 151 | } |
tedparrott6 | 2:056f36912bf0 | 152 | if(theta_roll > 3) { |
tedparrott6 | 2:056f36912bf0 | 153 | theta_a_left = 7; |
tedparrott6 | 2:056f36912bf0 | 154 | } else { |
tedparrott6 | 2:056f36912bf0 | 155 | theta_a_left = 0; |
tedparrott6 | 2:056f36912bf0 | 156 | } |
tedparrott6 | 2:056f36912bf0 | 157 | |
tedparrott6 | 2:056f36912bf0 | 158 | time_1 = t_1.read(); |
tedparrott6 | 2:056f36912bf0 | 159 | leg_control_signal = A*sin(B*time_1 + C); |
tedparrott6 | 2:056f36912bf0 | 160 | |
tedparrott6 | 2:056f36912bf0 | 161 | theta_4 = 150 + leg_control_signal; |
tedparrott6 | 2:056f36912bf0 | 162 | theta_5 = 150 + leg_control_signal; |
tedparrott6 | 2:056f36912bf0 | 163 | |
tedparrott6 | 2:056f36912bf0 | 164 | theta_a_right_rad = theta_a_right*3.14159/180; |
tedparrott6 | 2:056f36912bf0 | 165 | theta_a_left_rad = theta_a_left*3.14159/180; |
tedparrott6 | 2:056f36912bf0 | 166 | theta_2 = -1.43*theta_a_right + 150; |
tedparrott6 | 2:056f36912bf0 | 167 | theta_7 = -1.43*theta_a_left + 150; |
tedparrott6 | 2:056f36912bf0 | 168 | theta_1_r = -asin((20.94/8)*sin(theta_a_right_rad)); |
tedparrott6 | 2:056f36912bf0 | 169 | theta_1 = (theta_1_r*180/3.14159)+150; |
tedparrott6 | 2:056f36912bf0 | 170 | theta_8_r = -asin((20.94/8)*sin(theta_a_left_rad)); |
tedparrott6 | 2:056f36912bf0 | 171 | theta_8 = (theta_8_r*180/3.14159)+150; |
tedparrott6 | 2:056f36912bf0 | 172 | m1.SetGoal(theta_1); |
tedparrott6 | 2:056f36912bf0 | 173 | m2.SetGoal(theta_2); |
tedparrott6 | 2:056f36912bf0 | 174 | m7.SetGoal(theta_7); |
tedparrott6 | 2:056f36912bf0 | 175 | m8.SetGoal(theta_8); |
tedparrott6 | 2:056f36912bf0 | 176 | m4.SetGoal(theta_4); |
tedparrott6 | 2:056f36912bf0 | 177 | m5.SetGoal(theta_5); |
chris | 0:f6f8cf11779f | 178 | } |
chris | 0:f6f8cf11779f | 179 | } |