NA
Dependencies: AX12 BMA180_2 L3G4200D_1 mbed
Fork of AX12-HelloWorld by
main.cpp
- Committer:
- tedparrott6
- Date:
- 2017-11-16
- Revision:
- 2:f93868975538
- Parent:
- 1:b12b06e2fc2d
File content as of revision 2:f93868975538:
#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 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; 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); } }