NA
Dependencies: AX12 BMA180_3 L3G4200D_2 mbed
Fork of AX12-HelloWorld by
main.cpp
- Committer:
- tedparrott6
- Date:
- 2017-11-16
- Revision:
- 2:ce44bdd590ed
- Parent:
- 1:b12b06e2fc2d
File content as of revision 2:ce44bdd590ed:
#include "mbed.h" #include "BMA180.h" #include "math.h" #include "L3G4200D.h" #include "AX12.h" Serial pc(USBTX, USBRX); L3G4200D gyro(p9,p10); BMA180 my_BMA180(p5,p6,p7,p15,p16); AX12 m1 (p28, p27, 1); AX12 m2 (p28, p27, 2); AX12 m3 (p28, p27, 3); AX12 m4 (p28, p27, 4); AX12 m5 (p28, p27, 5); AX12 m6 (p28, p27, 6); AX12 m7 (p28, p27, 7); AX12 m8 (p28, p27, 8); AX12 m9 (p28, p27, 9); AX12 m10 (p28, p27, 10); float theta_1; float theta_1_r; float theta_2; float theta_8; float theta_8_r; float theta_7; float angle_x; float angle_x_a; float angle_y; float angle_y_a; float r1; float r2; float pi = 3.14159; float speed_x; float speed_y; float g[3]; float theta_roll; float theta_a_right; float theta_a_left; float theta_a_right_rad; float theta_a_left_rad; int x_msb, y_msb, z_msb; char x_lsb, y_lsb, z_lsb; short ax, ay, az; float afx, afy, afz; int main() { m1.SetGoal(150); wait(0.5); m2.SetGoal(150); wait(0.5); m3.SetGoal(150); wait(0.5); m4.SetGoal(150); wait(0.5); m5.SetGoal(150); wait(0.5); m6.SetGoal(150); wait(0.5); m7.SetGoal(150); wait(0.5); m8.SetGoal(150); wait(0.5); m9.SetGoal(150); wait(0.5); m10.SetGoal(150); wait(0.5); angle_y = 0; angle_x = 0; while (1) { gyro.read(g); // Read and Interpret Gyro Data speed_x = ((g[1])*0.07) - 1.12; speed_y = ((g[0])*0.07) - 0.3; x_lsb = my_BMA180.readReg(ACCXLSB); // Read X LSB register x_msb = my_BMA180.readReg(ACCXMSB); // Read X MSB register ax = (x_msb << 8) | x_lsb; // Concatinate X MSB and LSB ax = ax >> 2; // Remove unused first 2 LSB (16 bits to 14 bits) afx = (float)ax*3/16384; y_lsb = my_BMA180.readReg(ACCYLSB); // Read Y LSB register y_msb = my_BMA180.readReg(ACCYMSB); // Read Y MSB register ay = (y_msb << 8) | y_lsb; // Concatinate Y MSB and LSB ay = ay >> 2; // Remove unused first 2 LSB afy = (float)ay*3/16384; z_lsb = my_BMA180.readReg(ACCZLSB); // Read Z LSB register z_msb = my_BMA180.readReg(ACCZMSB); // Read Z MSB register az = (z_msb << 8) | z_lsb; // Concatinate Z MSB and LSB az = az >> 2; // Remove unused first 2 LSB afz = (float)az*3/16384; r1 = afx / (sqrt(pow(afy,2) + pow(afz,2))); // Determine X component of gravity force r2 = afy / (sqrt(pow(afx,2) + pow(afz,2))); // Determine Y component of gravity force angle_x_a = atan(r1)*180/pi; // Determine X, Y angles angle_y_a = atan(r2)*180/pi; angle_x = 0.98*(angle_x + speed_x*0.005) + 0.02*angle_x_a; angle_y = 0.98*(angle_y + speed_y*0.005) + 0.02*angle_y_a; theta_roll = angle_y + 4.5; if(theta_roll < -3) { theta_a_right = -7; } else { theta_a_right = 0; } if(theta_roll > 3) { theta_a_left = 7; } else { theta_a_left = 0; } theta_a_right_rad = theta_a_right*3.14159/180; theta_a_left_rad = theta_a_left*3.14159/180; theta_2 = -1.43*theta_a_right + 150; theta_7 = -1.43*theta_a_left + 150; theta_1_r = -asin((20.94/8)*sin(theta_a_right_rad)); theta_1 = (theta_1_r*180/3.14159)+150; theta_8_r = -asin((20.94/8)*sin(theta_a_left_rad)); theta_8 = (theta_8_r*180/3.14159)+150; m1.SetGoal(theta_1); m2.SetGoal(theta_2); m7.SetGoal(theta_7); m8.SetGoal(theta_8); } }