Ted Parrott
/
biaxial_ankle_tilt
NA
Fork of AX12-HelloWorld by
main.cpp@2:3d159bed27c1, 2017-11-16 (annotated)
- Committer:
- tedparrott6
- Date:
- Thu Nov 16 14:56:50 2017 +0000
- Revision:
- 2:3d159bed27c1
- 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:3d159bed27c1 | 6 | AX12 m7 (p28, p27, 7); |
tedparrott6 | 2:3d159bed27c1 | 7 | AX12 m8 (p28, p27, 8); |
tedparrott6 | 2:3d159bed27c1 | 8 | Serial pc(USBTX,USBRX); |
tedparrott6 | 2:3d159bed27c1 | 9 | float theta_a_x; |
tedparrott6 | 2:3d159bed27c1 | 10 | float theta_a_y; |
tedparrott6 | 2:3d159bed27c1 | 11 | float theta_a_x_rad; |
tedparrott6 | 2:3d159bed27c1 | 12 | float theta_8_x ; |
tedparrott6 | 2:3d159bed27c1 | 13 | float theta_7_x; |
tedparrott6 | 2:3d159bed27c1 | 14 | float theta_a_y_rad; |
tedparrott6 | 2:3d159bed27c1 | 15 | float theta_7_y; |
tedparrott6 | 2:3d159bed27c1 | 16 | float theta_8_r_y; |
tedparrott6 | 2:3d159bed27c1 | 17 | float theta_8_y; |
tedparrott6 | 2:3d159bed27c1 | 18 | float theta_7_m; |
tedparrott6 | 2:3d159bed27c1 | 19 | float theta_8_m; |
tedparrott6 | 2:3d159bed27c1 | 20 | |
tedparrott6 | 2:3d159bed27c1 | 21 | m7.SetGoal(150); |
tedparrott6 | 2:3d159bed27c1 | 22 | wait(1); |
tedparrott6 | 2:3d159bed27c1 | 23 | m8.SetGoal(150); |
tedparrott6 | 2:3d159bed27c1 | 24 | wait(1); |
tedparrott6 | 2:3d159bed27c1 | 25 | |
chris | 0:f6f8cf11779f | 26 | while (1) { |
tedparrott6 | 2:3d159bed27c1 | 27 | theta_a_x = 0; |
tedparrott6 | 2:3d159bed27c1 | 28 | theta_a_x_rad = theta_a_x*(3.14159/180); |
tedparrott6 | 2:3d159bed27c1 | 29 | theta_8_x = (theta_a_x/0.34); |
tedparrott6 | 2:3d159bed27c1 | 30 | theta_7_x = ((180/3.14159)*(asin((11.4/8)*theta_a_x_rad))); |
tedparrott6 | 2:3d159bed27c1 | 31 | |
tedparrott6 | 2:3d159bed27c1 | 32 | theta_a_y = 0; |
tedparrott6 | 2:3d159bed27c1 | 33 | theta_a_y_rad = theta_a_y*3.14159/180; |
tedparrott6 | 2:3d159bed27c1 | 34 | theta_7_y = -1.43*theta_a_y; |
tedparrott6 | 2:3d159bed27c1 | 35 | theta_8_r_y = -asin((20.94/8)*sin(theta_a_y_rad)); |
tedparrott6 | 2:3d159bed27c1 | 36 | theta_8_y= (theta_8_r_y*180/3.14159); |
tedparrott6 | 2:3d159bed27c1 | 37 | |
tedparrott6 | 2:3d159bed27c1 | 38 | theta_7_m = 150 + theta_7_x + theta_7_y; |
tedparrott6 | 2:3d159bed27c1 | 39 | theta_8_m = 150 - theta_8_x + theta_8_y; |
tedparrott6 | 2:3d159bed27c1 | 40 | m7.SetGoal(theta_7_m); |
tedparrott6 | 2:3d159bed27c1 | 41 | m8.SetGoal(theta_8_m); |
tedparrott6 | 2:3d159bed27c1 | 42 | pc.printf("\n\r7m: %05f 8m: %05f",theta_7_m,theta_8_m); |
chris | 0:f6f8cf11779f | 43 | } |
chris | 0:f6f8cf11779f | 44 | } |