Ted Parrott
/
Frontal_tilt
NA
Fork of AX12-HelloWorld by
main.cpp@2:8a752bc2f2f8, 2017-11-16 (annotated)
- Committer:
- tedparrott6
- Date:
- Thu Nov 16 15:02:21 2017 +0000
- Revision:
- 2:8a752bc2f2f8
- Parent:
- 1:b12b06e2fc2d
NA
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
chris | 0:f6f8cf11779f | 1 | #include "mbed.h" |
chris | 0:f6f8cf11779f | 2 | #include "AX12.h" |
chris | 0:f6f8cf11779f | 3 | |
chris | 0:f6f8cf11779f | 4 | int main() { |
chris | 0:f6f8cf11779f | 5 | |
tedparrott6 | 2:8a752bc2f2f8 | 6 | AX12 m1 (p28, p27, 1); |
tedparrott6 | 2:8a752bc2f2f8 | 7 | AX12 m2 (p28, p27, 2); |
tedparrott6 | 2:8a752bc2f2f8 | 8 | AX12 m3 (p28, p27, 3); |
tedparrott6 | 2:8a752bc2f2f8 | 9 | AX12 m4 (p28, p27, 4); |
tedparrott6 | 2:8a752bc2f2f8 | 10 | AX12 m6 (p28, p27, 6); |
tedparrott6 | 2:8a752bc2f2f8 | 11 | AX12 m5 (p28, p27, 5); |
tedparrott6 | 2:8a752bc2f2f8 | 12 | AX12 m7 (p28, p27, 7); |
tedparrott6 | 2:8a752bc2f2f8 | 13 | AX12 m8 (p28, p27, 8); |
tedparrott6 | 2:8a752bc2f2f8 | 14 | AX12 m9 (p28, p27, 9); |
tedparrott6 | 2:8a752bc2f2f8 | 15 | AX12 m10 (p28, p27, 10); |
tedparrott6 | 2:8a752bc2f2f8 | 16 | Serial pc(USBTX,USBRX); |
tedparrott6 | 2:8a752bc2f2f8 | 17 | Timer t; |
tedparrott6 | 2:8a752bc2f2f8 | 18 | float l_theta_k; |
tedparrott6 | 2:8a752bc2f2f8 | 19 | float l_theta_h; |
tedparrott6 | 2:8a752bc2f2f8 | 20 | float l_theta_a_x; |
tedparrott6 | 2:8a752bc2f2f8 | 21 | float l_theta_a_x_rad; |
tedparrott6 | 2:8a752bc2f2f8 | 22 | float l_theta_7_x; |
tedparrott6 | 2:8a752bc2f2f8 | 23 | float l_theta_8_x; |
tedparrott6 | 2:8a752bc2f2f8 | 24 | float l_theta_a_y; |
tedparrott6 | 2:8a752bc2f2f8 | 25 | float l_theta_a_y_rad; |
tedparrott6 | 2:8a752bc2f2f8 | 26 | float l_theta_7_y; |
tedparrott6 | 2:8a752bc2f2f8 | 27 | float l_theta_8_y; |
tedparrott6 | 2:8a752bc2f2f8 | 28 | float l_theta_8_r_y; |
tedparrott6 | 2:8a752bc2f2f8 | 29 | float l_theta_5; |
tedparrott6 | 2:8a752bc2f2f8 | 30 | float l_theta_7_m; |
tedparrott6 | 2:8a752bc2f2f8 | 31 | float l_theta_8_m; |
tedparrott6 | 2:8a752bc2f2f8 | 32 | float l_theta_6; |
tedparrott6 | 2:8a752bc2f2f8 | 33 | float h_l; |
tedparrott6 | 2:8a752bc2f2f8 | 34 | float r_theta_k; |
tedparrott6 | 2:8a752bc2f2f8 | 35 | float r_theta_h; |
tedparrott6 | 2:8a752bc2f2f8 | 36 | float r_theta_a_x; |
tedparrott6 | 2:8a752bc2f2f8 | 37 | float r_theta_a_x_rad; |
tedparrott6 | 2:8a752bc2f2f8 | 38 | float r_theta_2_x; |
tedparrott6 | 2:8a752bc2f2f8 | 39 | float r_theta_1_x; |
tedparrott6 | 2:8a752bc2f2f8 | 40 | float r_theta_a_y; |
tedparrott6 | 2:8a752bc2f2f8 | 41 | float r_theta_a_y_rad; |
tedparrott6 | 2:8a752bc2f2f8 | 42 | float r_theta_2_y; |
tedparrott6 | 2:8a752bc2f2f8 | 43 | float r_theta_1_y; |
tedparrott6 | 2:8a752bc2f2f8 | 44 | float r_theta_1_r_y; |
tedparrott6 | 2:8a752bc2f2f8 | 45 | float r_theta_4; |
tedparrott6 | 2:8a752bc2f2f8 | 46 | float r_theta_2_m; |
tedparrott6 | 2:8a752bc2f2f8 | 47 | float r_theta_1_m; |
tedparrott6 | 2:8a752bc2f2f8 | 48 | float r_theta_3; |
tedparrott6 | 2:8a752bc2f2f8 | 49 | float h_r; |
tedparrott6 | 2:8a752bc2f2f8 | 50 | float time; |
tedparrott6 | 2:8a752bc2f2f8 | 51 | float angle_read; |
tedparrott6 | 2:8a752bc2f2f8 | 52 | float angle_read_rad; |
tedparrott6 | 2:8a752bc2f2f8 | 53 | m5.SetGoal(150); |
tedparrott6 | 2:8a752bc2f2f8 | 54 | wait(1); |
tedparrott6 | 2:8a752bc2f2f8 | 55 | m6.SetGoal(150); |
tedparrott6 | 2:8a752bc2f2f8 | 56 | wait(1); |
tedparrott6 | 2:8a752bc2f2f8 | 57 | m7.SetGoal(150); |
tedparrott6 | 2:8a752bc2f2f8 | 58 | wait(1); |
tedparrott6 | 2:8a752bc2f2f8 | 59 | m8.SetGoal(150); |
tedparrott6 | 2:8a752bc2f2f8 | 60 | wait(1); |
tedparrott6 | 2:8a752bc2f2f8 | 61 | m1.SetGoal(150); |
tedparrott6 | 2:8a752bc2f2f8 | 62 | wait(1); |
tedparrott6 | 2:8a752bc2f2f8 | 63 | m2.SetGoal(150); |
tedparrott6 | 2:8a752bc2f2f8 | 64 | wait(1); |
tedparrott6 | 2:8a752bc2f2f8 | 65 | m3.SetGoal(150); |
tedparrott6 | 2:8a752bc2f2f8 | 66 | wait(1); |
tedparrott6 | 2:8a752bc2f2f8 | 67 | m4.SetGoal(150); |
tedparrott6 | 2:8a752bc2f2f8 | 68 | wait(1); |
tedparrott6 | 2:8a752bc2f2f8 | 69 | m9.SetGoal(150); |
tedparrott6 | 2:8a752bc2f2f8 | 70 | wait(1); |
tedparrott6 | 2:8a752bc2f2f8 | 71 | m10.SetGoal(150); |
tedparrott6 | 2:8a752bc2f2f8 | 72 | wait(1); |
tedparrott6 | 2:8a752bc2f2f8 | 73 | t.start(); |
tedparrott6 | 2:8a752bc2f2f8 | 74 | h_l = 0; |
tedparrott6 | 2:8a752bc2f2f8 | 75 | h_r = 0; |
tedparrott6 | 2:8a752bc2f2f8 | 76 | |
chris | 0:f6f8cf11779f | 77 | while (1) { |
tedparrott6 | 2:8a752bc2f2f8 | 78 | time = t.read(); |
tedparrott6 | 2:8a752bc2f2f8 | 79 | angle_read = 5*sin(time); |
tedparrott6 | 2:8a752bc2f2f8 | 80 | angle_read_rad = angle_read*(3.14159/180); |
tedparrott6 | 2:8a752bc2f2f8 | 81 | |
tedparrott6 | 2:8a752bc2f2f8 | 82 | if(angle_read == 0){ |
tedparrott6 | 2:8a752bc2f2f8 | 83 | h_l = 0; |
tedparrott6 | 2:8a752bc2f2f8 | 84 | h_r = 0; |
tedparrott6 | 2:8a752bc2f2f8 | 85 | }else if(angle_read > 0){ |
tedparrott6 | 2:8a752bc2f2f8 | 86 | h_l = 79*tan(angle_read_rad); |
tedparrott6 | 2:8a752bc2f2f8 | 87 | h_r = 0; |
tedparrott6 | 2:8a752bc2f2f8 | 88 | }else if(angle_read < 0){ |
tedparrott6 | 2:8a752bc2f2f8 | 89 | angle_read_rad = abs(angle_read_rad); |
tedparrott6 | 2:8a752bc2f2f8 | 90 | h_l = 0; |
tedparrott6 | 2:8a752bc2f2f8 | 91 | h_r = 79*tan(angle_read_rad); |
tedparrott6 | 2:8a752bc2f2f8 | 92 | } |
tedparrott6 | 2:8a752bc2f2f8 | 93 | l_theta_h = ((180/3.14159)*acos(1-(h_l/204))); |
tedparrott6 | 2:8a752bc2f2f8 | 94 | l_theta_k = 2*l_theta_h; |
tedparrott6 | 2:8a752bc2f2f8 | 95 | l_theta_5 = 150 + l_theta_h; |
tedparrott6 | 2:8a752bc2f2f8 | 96 | l_theta_6 = 150 + l_theta_k; |
tedparrott6 | 2:8a752bc2f2f8 | 97 | |
tedparrott6 | 2:8a752bc2f2f8 | 98 | r_theta_h = ((180/3.14159)*acos(1-(h_r/204))); |
tedparrott6 | 2:8a752bc2f2f8 | 99 | r_theta_k = 2*r_theta_h; |
tedparrott6 | 2:8a752bc2f2f8 | 100 | r_theta_4 = 150 - r_theta_h; |
tedparrott6 | 2:8a752bc2f2f8 | 101 | r_theta_3 = 150 - r_theta_k; |
tedparrott6 | 2:8a752bc2f2f8 | 102 | |
tedparrott6 | 2:8a752bc2f2f8 | 103 | l_theta_a_x = l_theta_h; |
tedparrott6 | 2:8a752bc2f2f8 | 104 | l_theta_a_x_rad = l_theta_a_x*(3.14159/180); |
tedparrott6 | 2:8a752bc2f2f8 | 105 | l_theta_8_x = (l_theta_a_x/0.34); |
tedparrott6 | 2:8a752bc2f2f8 | 106 | l_theta_7_x = ((180/3.14159)*(asin((11.4/8)*l_theta_a_x_rad))); |
tedparrott6 | 2:8a752bc2f2f8 | 107 | |
tedparrott6 | 2:8a752bc2f2f8 | 108 | l_theta_a_y = angle_read; |
tedparrott6 | 2:8a752bc2f2f8 | 109 | l_theta_a_y_rad = l_theta_a_y*3.14159/180; |
tedparrott6 | 2:8a752bc2f2f8 | 110 | l_theta_7_y = -1.43*l_theta_a_y; |
tedparrott6 | 2:8a752bc2f2f8 | 111 | l_theta_8_r_y = -asin((20.94/8)*sin(l_theta_a_y_rad)); |
tedparrott6 | 2:8a752bc2f2f8 | 112 | l_theta_8_y= (l_theta_8_r_y*180/3.14159); |
tedparrott6 | 2:8a752bc2f2f8 | 113 | |
tedparrott6 | 2:8a752bc2f2f8 | 114 | r_theta_a_x = r_theta_h; |
tedparrott6 | 2:8a752bc2f2f8 | 115 | r_theta_a_x_rad = r_theta_a_x*(3.14159/180); |
tedparrott6 | 2:8a752bc2f2f8 | 116 | r_theta_1_x = (r_theta_a_x/0.34); |
tedparrott6 | 2:8a752bc2f2f8 | 117 | r_theta_2_x = ((180/3.14159)*(asin((11.4/8)*r_theta_a_x_rad))); |
tedparrott6 | 2:8a752bc2f2f8 | 118 | |
tedparrott6 | 2:8a752bc2f2f8 | 119 | r_theta_a_y = angle_read; |
tedparrott6 | 2:8a752bc2f2f8 | 120 | r_theta_a_y_rad = r_theta_a_y*3.14159/180; |
tedparrott6 | 2:8a752bc2f2f8 | 121 | r_theta_2_y = -1.43*r_theta_a_y; |
tedparrott6 | 2:8a752bc2f2f8 | 122 | r_theta_1_r_y = -asin((20.94/8)*sin(r_theta_a_y_rad)); |
tedparrott6 | 2:8a752bc2f2f8 | 123 | r_theta_1_y= (r_theta_1_r_y*180/3.14159); |
tedparrott6 | 2:8a752bc2f2f8 | 124 | |
tedparrott6 | 2:8a752bc2f2f8 | 125 | l_theta_7_m = 150 + l_theta_7_x + l_theta_7_y; |
tedparrott6 | 2:8a752bc2f2f8 | 126 | l_theta_8_m = 150 - l_theta_8_x + l_theta_8_y; |
tedparrott6 | 2:8a752bc2f2f8 | 127 | |
tedparrott6 | 2:8a752bc2f2f8 | 128 | r_theta_2_m = 150 - r_theta_2_x - r_theta_2_y; |
tedparrott6 | 2:8a752bc2f2f8 | 129 | r_theta_1_m = 150 + r_theta_1_x - r_theta_1_y; |
tedparrott6 | 2:8a752bc2f2f8 | 130 | |
tedparrott6 | 2:8a752bc2f2f8 | 131 | m1.SetGoal(r_theta_1_m); |
tedparrott6 | 2:8a752bc2f2f8 | 132 | m2.SetGoal(r_theta_2_m); |
tedparrott6 | 2:8a752bc2f2f8 | 133 | m3.SetGoal(r_theta_3); |
tedparrott6 | 2:8a752bc2f2f8 | 134 | m4.SetGoal(r_theta_4); |
tedparrott6 | 2:8a752bc2f2f8 | 135 | m5.SetGoal(l_theta_5); |
tedparrott6 | 2:8a752bc2f2f8 | 136 | m6.SetGoal(l_theta_6); |
tedparrott6 | 2:8a752bc2f2f8 | 137 | m7.SetGoal(l_theta_7_m); |
tedparrott6 | 2:8a752bc2f2f8 | 138 | m8.SetGoal(l_theta_8_m); |
tedparrott6 | 2:8a752bc2f2f8 | 139 | pc.printf("\n\rAngle: %05f",angle_read); |
chris | 0:f6f8cf11779f | 140 | } |
chris | 0:f6f8cf11779f | 141 | } |