NA
Dependencies: AX12 BMA180_3 L3G4200D_2 mbed
Fork of AX12-HelloWorld by
main.cpp@2:ce44bdd590ed, 2017-11-16 (annotated)
- Committer:
- tedparrott6
- Date:
- Thu Nov 16 15:01:44 2017 +0000
- Revision:
- 2:ce44bdd590ed
- 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:ce44bdd590ed | 2 | #include "BMA180.h" |
tedparrott6 | 2:ce44bdd590ed | 3 | #include "math.h" |
tedparrott6 | 2:ce44bdd590ed | 4 | #include "L3G4200D.h" |
chris | 0:f6f8cf11779f | 5 | #include "AX12.h" |
chris | 0:f6f8cf11779f | 6 | |
tedparrott6 | 2:ce44bdd590ed | 7 | Serial pc(USBTX, USBRX); |
tedparrott6 | 2:ce44bdd590ed | 8 | L3G4200D gyro(p9,p10); |
tedparrott6 | 2:ce44bdd590ed | 9 | BMA180 my_BMA180(p5,p6,p7,p15,p16); |
tedparrott6 | 2:ce44bdd590ed | 10 | |
tedparrott6 | 2:ce44bdd590ed | 11 | AX12 m1 (p28, p27, 1); |
tedparrott6 | 2:ce44bdd590ed | 12 | AX12 m2 (p28, p27, 2); |
tedparrott6 | 2:ce44bdd590ed | 13 | AX12 m3 (p28, p27, 3); |
tedparrott6 | 2:ce44bdd590ed | 14 | AX12 m4 (p28, p27, 4); |
tedparrott6 | 2:ce44bdd590ed | 15 | AX12 m5 (p28, p27, 5); |
tedparrott6 | 2:ce44bdd590ed | 16 | AX12 m6 (p28, p27, 6); |
tedparrott6 | 2:ce44bdd590ed | 17 | AX12 m7 (p28, p27, 7); |
tedparrott6 | 2:ce44bdd590ed | 18 | AX12 m8 (p28, p27, 8); |
tedparrott6 | 2:ce44bdd590ed | 19 | AX12 m9 (p28, p27, 9); |
tedparrott6 | 2:ce44bdd590ed | 20 | AX12 m10 (p28, p27, 10); |
chris | 0:f6f8cf11779f | 21 | |
tedparrott6 | 2:ce44bdd590ed | 22 | float theta_1; |
tedparrott6 | 2:ce44bdd590ed | 23 | float theta_1_r; |
tedparrott6 | 2:ce44bdd590ed | 24 | float theta_2; |
tedparrott6 | 2:ce44bdd590ed | 25 | float theta_8; |
tedparrott6 | 2:ce44bdd590ed | 26 | float theta_8_r; |
tedparrott6 | 2:ce44bdd590ed | 27 | float theta_7; |
tedparrott6 | 2:ce44bdd590ed | 28 | float angle_x; |
tedparrott6 | 2:ce44bdd590ed | 29 | float angle_x_a; |
tedparrott6 | 2:ce44bdd590ed | 30 | float angle_y; |
tedparrott6 | 2:ce44bdd590ed | 31 | float angle_y_a; |
tedparrott6 | 2:ce44bdd590ed | 32 | float r1; |
tedparrott6 | 2:ce44bdd590ed | 33 | float r2; |
tedparrott6 | 2:ce44bdd590ed | 34 | float pi = 3.14159; |
tedparrott6 | 2:ce44bdd590ed | 35 | float speed_x; |
tedparrott6 | 2:ce44bdd590ed | 36 | float speed_y; |
tedparrott6 | 2:ce44bdd590ed | 37 | float g[3]; |
tedparrott6 | 2:ce44bdd590ed | 38 | float theta_roll; |
tedparrott6 | 2:ce44bdd590ed | 39 | float theta_a_right; |
tedparrott6 | 2:ce44bdd590ed | 40 | float theta_a_left; |
tedparrott6 | 2:ce44bdd590ed | 41 | float theta_a_right_rad; |
tedparrott6 | 2:ce44bdd590ed | 42 | float theta_a_left_rad; |
tedparrott6 | 2:ce44bdd590ed | 43 | int x_msb, y_msb, z_msb; |
tedparrott6 | 2:ce44bdd590ed | 44 | char x_lsb, y_lsb, z_lsb; |
tedparrott6 | 2:ce44bdd590ed | 45 | short ax, ay, az; |
tedparrott6 | 2:ce44bdd590ed | 46 | float afx, afy, afz; |
chris | 1:b12b06e2fc2d | 47 | |
tedparrott6 | 2:ce44bdd590ed | 48 | |
tedparrott6 | 2:ce44bdd590ed | 49 | int main() |
tedparrott6 | 2:ce44bdd590ed | 50 | { |
tedparrott6 | 2:ce44bdd590ed | 51 | m1.SetGoal(150); |
tedparrott6 | 2:ce44bdd590ed | 52 | wait(0.5); |
tedparrott6 | 2:ce44bdd590ed | 53 | m2.SetGoal(150); |
tedparrott6 | 2:ce44bdd590ed | 54 | wait(0.5); |
tedparrott6 | 2:ce44bdd590ed | 55 | m3.SetGoal(150); |
tedparrott6 | 2:ce44bdd590ed | 56 | wait(0.5); |
tedparrott6 | 2:ce44bdd590ed | 57 | m4.SetGoal(150); |
tedparrott6 | 2:ce44bdd590ed | 58 | wait(0.5); |
tedparrott6 | 2:ce44bdd590ed | 59 | m5.SetGoal(150); |
tedparrott6 | 2:ce44bdd590ed | 60 | wait(0.5); |
tedparrott6 | 2:ce44bdd590ed | 61 | m6.SetGoal(150); |
tedparrott6 | 2:ce44bdd590ed | 62 | wait(0.5); |
tedparrott6 | 2:ce44bdd590ed | 63 | m7.SetGoal(150); |
tedparrott6 | 2:ce44bdd590ed | 64 | wait(0.5); |
tedparrott6 | 2:ce44bdd590ed | 65 | m8.SetGoal(150); |
tedparrott6 | 2:ce44bdd590ed | 66 | wait(0.5); |
tedparrott6 | 2:ce44bdd590ed | 67 | m9.SetGoal(150); |
tedparrott6 | 2:ce44bdd590ed | 68 | wait(0.5); |
tedparrott6 | 2:ce44bdd590ed | 69 | m10.SetGoal(150); |
tedparrott6 | 2:ce44bdd590ed | 70 | wait(0.5); |
tedparrott6 | 2:ce44bdd590ed | 71 | |
tedparrott6 | 2:ce44bdd590ed | 72 | angle_y = 0; |
tedparrott6 | 2:ce44bdd590ed | 73 | angle_x = 0; |
chris | 0:f6f8cf11779f | 74 | while (1) { |
tedparrott6 | 2:ce44bdd590ed | 75 | gyro.read(g); // Read and Interpret Gyro Data |
tedparrott6 | 2:ce44bdd590ed | 76 | speed_x = ((g[1])*0.07) - 1.12; |
tedparrott6 | 2:ce44bdd590ed | 77 | speed_y = ((g[0])*0.07) - 0.3; |
tedparrott6 | 2:ce44bdd590ed | 78 | |
tedparrott6 | 2:ce44bdd590ed | 79 | x_lsb = my_BMA180.readReg(ACCXLSB); // Read X LSB register |
tedparrott6 | 2:ce44bdd590ed | 80 | x_msb = my_BMA180.readReg(ACCXMSB); // Read X MSB register |
tedparrott6 | 2:ce44bdd590ed | 81 | ax = (x_msb << 8) | x_lsb; // Concatinate X MSB and LSB |
tedparrott6 | 2:ce44bdd590ed | 82 | ax = ax >> 2; // Remove unused first 2 LSB (16 bits to 14 bits) |
tedparrott6 | 2:ce44bdd590ed | 83 | afx = (float)ax*3/16384; |
tedparrott6 | 2:ce44bdd590ed | 84 | |
tedparrott6 | 2:ce44bdd590ed | 85 | y_lsb = my_BMA180.readReg(ACCYLSB); // Read Y LSB register |
tedparrott6 | 2:ce44bdd590ed | 86 | y_msb = my_BMA180.readReg(ACCYMSB); // Read Y MSB register |
tedparrott6 | 2:ce44bdd590ed | 87 | ay = (y_msb << 8) | y_lsb; // Concatinate Y MSB and LSB |
tedparrott6 | 2:ce44bdd590ed | 88 | ay = ay >> 2; // Remove unused first 2 LSB |
tedparrott6 | 2:ce44bdd590ed | 89 | afy = (float)ay*3/16384; |
tedparrott6 | 2:ce44bdd590ed | 90 | |
tedparrott6 | 2:ce44bdd590ed | 91 | z_lsb = my_BMA180.readReg(ACCZLSB); // Read Z LSB register |
tedparrott6 | 2:ce44bdd590ed | 92 | z_msb = my_BMA180.readReg(ACCZMSB); // Read Z MSB register |
tedparrott6 | 2:ce44bdd590ed | 93 | az = (z_msb << 8) | z_lsb; // Concatinate Z MSB and LSB |
tedparrott6 | 2:ce44bdd590ed | 94 | az = az >> 2; // Remove unused first 2 LSB |
tedparrott6 | 2:ce44bdd590ed | 95 | afz = (float)az*3/16384; |
tedparrott6 | 2:ce44bdd590ed | 96 | |
tedparrott6 | 2:ce44bdd590ed | 97 | r1 = afx / (sqrt(pow(afy,2) + pow(afz,2))); // Determine X component of gravity force |
tedparrott6 | 2:ce44bdd590ed | 98 | r2 = afy / (sqrt(pow(afx,2) + pow(afz,2))); // Determine Y component of gravity force |
tedparrott6 | 2:ce44bdd590ed | 99 | angle_x_a = atan(r1)*180/pi; // Determine X, Y angles |
tedparrott6 | 2:ce44bdd590ed | 100 | angle_y_a = atan(r2)*180/pi; |
tedparrott6 | 2:ce44bdd590ed | 101 | |
tedparrott6 | 2:ce44bdd590ed | 102 | angle_x = 0.98*(angle_x + speed_x*0.005) + 0.02*angle_x_a; |
tedparrott6 | 2:ce44bdd590ed | 103 | |
tedparrott6 | 2:ce44bdd590ed | 104 | angle_y = 0.98*(angle_y + speed_y*0.005) + 0.02*angle_y_a; |
tedparrott6 | 2:ce44bdd590ed | 105 | theta_roll = angle_y + 4.5; |
tedparrott6 | 2:ce44bdd590ed | 106 | |
tedparrott6 | 2:ce44bdd590ed | 107 | if(theta_roll < -3) { |
tedparrott6 | 2:ce44bdd590ed | 108 | theta_a_right = -7; |
tedparrott6 | 2:ce44bdd590ed | 109 | } else { |
tedparrott6 | 2:ce44bdd590ed | 110 | theta_a_right = 0; |
tedparrott6 | 2:ce44bdd590ed | 111 | } |
tedparrott6 | 2:ce44bdd590ed | 112 | if(theta_roll > 3) { |
tedparrott6 | 2:ce44bdd590ed | 113 | theta_a_left = 7; |
tedparrott6 | 2:ce44bdd590ed | 114 | } else { |
tedparrott6 | 2:ce44bdd590ed | 115 | theta_a_left = 0; |
tedparrott6 | 2:ce44bdd590ed | 116 | } |
tedparrott6 | 2:ce44bdd590ed | 117 | |
tedparrott6 | 2:ce44bdd590ed | 118 | theta_a_right_rad = theta_a_right*3.14159/180; |
tedparrott6 | 2:ce44bdd590ed | 119 | theta_a_left_rad = theta_a_left*3.14159/180; |
tedparrott6 | 2:ce44bdd590ed | 120 | theta_2 = -1.43*theta_a_right + 150; |
tedparrott6 | 2:ce44bdd590ed | 121 | theta_7 = -1.43*theta_a_left + 150; |
tedparrott6 | 2:ce44bdd590ed | 122 | theta_1_r = -asin((20.94/8)*sin(theta_a_right_rad)); |
tedparrott6 | 2:ce44bdd590ed | 123 | theta_1 = (theta_1_r*180/3.14159)+150; |
tedparrott6 | 2:ce44bdd590ed | 124 | theta_8_r = -asin((20.94/8)*sin(theta_a_left_rad)); |
tedparrott6 | 2:ce44bdd590ed | 125 | theta_8 = (theta_8_r*180/3.14159)+150; |
tedparrott6 | 2:ce44bdd590ed | 126 | m1.SetGoal(theta_1); |
tedparrott6 | 2:ce44bdd590ed | 127 | m2.SetGoal(theta_2); |
tedparrott6 | 2:ce44bdd590ed | 128 | m7.SetGoal(theta_7); |
tedparrott6 | 2:ce44bdd590ed | 129 | m8.SetGoal(theta_8); |
chris | 0:f6f8cf11779f | 130 | } |
chris | 0:f6f8cf11779f | 131 | } |