![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
NA
Dependencies: AX12 BMA180_2 L3G4200D_1 mbed
Fork of AX12-HelloWorld by
Diff: main.cpp
- Revision:
- 2:f93868975538
- Parent:
- 1:b12b06e2fc2d
--- a/main.cpp Thu Mar 31 12:03:04 2011 +0000 +++ b/main.cpp Thu Nov 16 15:00:17 2017 +0000 @@ -1,14 +1,232 @@ #include "mbed.h" +#include "BMA180.h" +#include "math.h" +#include "L3G4200D.h" #include "AX12.h" -int main() { +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); - AX12 myax12 (p9, p10, 1); +float l_theta_k; +float l_theta_h; +float l_theta_a_x; +float l_theta_a_x_rad; +float l_theta_7_x; +float l_theta_8_x; +float l_theta_a_y; +float l_theta_a_y_rad; +float l_theta_7_y; +float l_theta_8_y; +float l_theta_8_r_y; +float l_theta_5; +float l_theta_7_m; +float l_theta_8_m; +float l_theta_6; +float h_l; +float r_theta_k; +float r_theta_h; +float r_theta_a_x; +float r_theta_a_x_rad; +float r_theta_2_x; +float r_theta_1_x; +float r_theta_a_y; +float r_theta_a_y_rad; +float r_theta_2_y; +float r_theta_1_y; +float r_theta_1_r_y; +float r_theta_4; +float r_theta_2_m; +float r_theta_1_m; +float r_theta_3; +float h_r; +float angle_read; +float angle_read_rad; +float speed_setpoint; +float speed_error; +float pos_setpoint; +float pos_error; +float kp; +float kd; +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 output_angle_x; +float output_angle_y; +int output_angle_d_x; +int output_angle_d_y; +float force_mag; +float control_action; + + + +int main() +{ + m5.SetGoal(150); + wait(1); + m6.SetGoal(150); + wait(1); + m7.SetGoal(150); + wait(1); + m8.SetGoal(150); + wait(1); + m1.SetGoal(150); + wait(1); + m2.SetGoal(150); + wait(1); + m3.SetGoal(150); + wait(1); + m4.SetGoal(150); + wait(1); + m9.SetGoal(150); + wait(1); + m10.SetGoal(150); + wait(1); + h_l = 0; + h_r = 0; + + angle_x = 0; + angle_y = 0; + speed_setpoint = 0; + pos_setpoint = -3; + + kd = 0; + kp = 2; + + 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.595; - while (1) { - myax12.SetGoal(0); // go to 0 degrees - wait (2.0); - myax12.SetGoal(300); // go to 300 degrees - wait (2.0); + int x_msb, y_msb, z_msb; + char x_lsb, y_lsb, z_lsb; + short ax, ay, az; + float afx, afy, afz; + + 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; + + force_mag = abs(az) + abs(ay) + abs(ax); // Check force magnitude to reduce effects of shocks/vibration + + + angle_x = 0.98*(angle_x + speed_x*0.005) + 0.02*angle_x_a; + output_angle_x = angle_x; + output_angle_d_x = output_angle_x; + + angle_y = 0.98*(angle_y + speed_y*0.005) + 0.02*angle_y_a; + output_angle_y = angle_y; + output_angle_d_y = output_angle_y; + + pos_error = pos_setpoint - output_angle_x; + speed_error = speed_setpoint - speed_x; + + control_action = kp*pos_error + kd*speed_error; + if(control_action > 8){ + control_action = 8; + }else if(control_action < -8){ + control_action = -8; + }else{ + control_action = control_action; + } + angle_read = control_action; + angle_read_rad = angle_read*(3.14159/180); + + if(angle_read == 0){ + h_l = 0; + h_r = 0; + }else if(angle_read > 0){ + h_l = 79*tan(angle_read_rad); + h_r = 0; + }else if(angle_read < 0){ + angle_read_rad = abs(angle_read_rad); + h_l = 0; + h_r = 79*tan(angle_read_rad); + } + l_theta_h = ((180/3.14159)*acos(1-(h_l/204))); + l_theta_k = 2*l_theta_h; + l_theta_5 = 150 + l_theta_h; + l_theta_6 = 150 + l_theta_k; + + r_theta_h = ((180/3.14159)*acos(1-(h_r/204))); + r_theta_k = 2*r_theta_h; + r_theta_4 = 150 - r_theta_h; + r_theta_3 = 150 - r_theta_k; + + l_theta_a_x = l_theta_h; + l_theta_a_x_rad = l_theta_a_x*(3.14159/180); + l_theta_8_x = (l_theta_a_x/0.34); + l_theta_7_x = ((180/3.14159)*(asin((11.4/8)*l_theta_a_x_rad))); + + l_theta_a_y = angle_read; + l_theta_a_y_rad = l_theta_a_y*3.14159/180; + l_theta_7_y = -1.43*l_theta_a_y; + l_theta_8_r_y = -asin((20.94/8)*sin(l_theta_a_y_rad)); + l_theta_8_y= (l_theta_8_r_y*180/3.14159); + + r_theta_a_x = r_theta_h; + r_theta_a_x_rad = r_theta_a_x*(3.14159/180); + r_theta_1_x = (r_theta_a_x/0.34); + r_theta_2_x = ((180/3.14159)*(asin((11.4/8)*r_theta_a_x_rad))); + + r_theta_a_y = angle_read; + r_theta_a_y_rad = r_theta_a_y*3.14159/180; + r_theta_2_y = -1.43*r_theta_a_y; + r_theta_1_r_y = -asin((20.94/8)*sin(r_theta_a_y_rad)); + r_theta_1_y= (r_theta_1_r_y*180/3.14159); + + l_theta_7_m = 150 + l_theta_7_x + l_theta_7_y; + l_theta_8_m = 150 - l_theta_8_x + l_theta_8_y; + + r_theta_2_m = 150 - r_theta_2_x - r_theta_2_y; + r_theta_1_m = 150 + r_theta_1_x - r_theta_1_y; + + m1.SetGoal(r_theta_1_m); + m2.SetGoal(r_theta_2_m); + m3.SetGoal(r_theta_3); + m4.SetGoal(r_theta_4); + m5.SetGoal(l_theta_5); + m6.SetGoal(l_theta_6); + m7.SetGoal(l_theta_7_m); + m8.SetGoal(l_theta_8_m); + pc.printf("\n\rAngle: %05f",output_angle_y); + } } \ No newline at end of file