![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
NA
Fork of AX12-HelloWorld by
Diff: main.cpp
- Revision:
- 2:8a752bc2f2f8
- Parent:
- 1:b12b06e2fc2d
--- a/main.cpp Thu Mar 31 12:03:04 2011 +0000 +++ b/main.cpp Thu Nov 16 15:02:21 2017 +0000 @@ -3,12 +3,139 @@ int main() { - AX12 myax12 (p9, p10, 1); - + AX12 m1 (p28, p27, 1); + AX12 m2 (p28, p27, 2); + AX12 m3 (p28, p27, 3); + AX12 m4 (p28, p27, 4); + AX12 m6 (p28, p27, 6); + AX12 m5 (p28, p27, 5); + AX12 m7 (p28, p27, 7); + AX12 m8 (p28, p27, 8); + AX12 m9 (p28, p27, 9); + AX12 m10 (p28, p27, 10); + Serial pc(USBTX,USBRX); + Timer t; + 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 time; + float angle_read; + float angle_read_rad; + 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); + t.start(); + h_l = 0; + h_r = 0; + while (1) { - myax12.SetGoal(0); // go to 0 degrees - wait (2.0); - myax12.SetGoal(300); // go to 300 degrees - wait (2.0); + time = t.read(); + angle_read = 5*sin(time); + 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",angle_read); } } \ No newline at end of file