NA
Dependencies: AX12 BMA180_4 L3G4200D_3 mbed
Fork of AX12-HelloWorld by
main.cpp
- Committer:
- tedparrott6
- Date:
- 2017-11-16
- Revision:
- 2:056f36912bf0
- Parent:
- 1:b12b06e2fc2d
File content as of revision 2:056f36912bf0:
#include "mbed.h" #include "BMA180.h" #include "math.h" #include "L3G4200D.h" #include "AX12.h" 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); Timer t_1; Timer t_2; float theta_1; float theta_1_r; float theta_2; float theta_8; float theta_8_r; float theta_7; float time_1; float time_2; 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; float time_sum; float period; int n; float A; float B; float C; float leg_control_signal; float theta_4; float theta_5; int dir_0; int dir_1; 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); t_1.start(); t_2.start(); time_sum = 0; n = 0; angle_y = 0; dir_1 = 0; dir_0 = 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; dir_0 = dir_1; if(theta_roll >= 0) { dir_1 = 1; } else { dir_1 = 0; } if(dir_1 != dir_0) { time_2 = t_2.read(); t_2.reset(); time_sum = time_sum + time_2; n=n+1; } period = 2*(time_sum/n); B = 2*3.14159/period; C = -(period/4)*B; A = 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; } time_1 = t_1.read(); leg_control_signal = A*sin(B*time_1 + C); theta_4 = 150 + leg_control_signal; theta_5 = 150 + leg_control_signal; 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); m4.SetGoal(theta_4); m5.SetGoal(theta_5); } }