Craig Evans
/
MeArm_Lab3_Task1
Lab 3 Task 1
Fork of MeArm_Lab2_Task1 by
main.cpp@3:13f56160c56e, 2017-07-04 (annotated)
- Committer:
- eencae
- Date:
- Tue Jul 04 18:11:30 2017 +0000
- Revision:
- 3:13f56160c56e
- Parent:
- 2:2494cdc6977a
Initial commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
eencae | 3:13f56160c56e | 1 | /* MeArm Lab 3 Task 1 |
eencae | 2:2494cdc6977a | 2 | |
eencae | 2:2494cdc6977a | 3 | (c) Dr Craig A. Evans, University of Leeds |
eencae | 2:2494cdc6977a | 4 | |
eencae | 2:2494cdc6977a | 5 | July 2017 |
eencae | 2:2494cdc6977a | 6 | |
eencae | 2:2494cdc6977a | 7 | */ |
eencae | 2:2494cdc6977a | 8 | |
eencae | 0:be56eef4c199 | 9 | #include "mbed.h" |
eencae | 0:be56eef4c199 | 10 | #include "MeArm.h" |
eencae | 0:be56eef4c199 | 11 | |
eencae | 2:2494cdc6977a | 12 | // create MeArm object - middle, left, right, claw |
eencae | 0:be56eef4c199 | 13 | MeArm arm(p21,p22,p23,p24); |
eencae | 0:be56eef4c199 | 14 | |
eencae | 0:be56eef4c199 | 15 | int main() |
eencae | 0:be56eef4c199 | 16 | { |
eencae | 1:ea13195efe98 | 17 | // open , closed |
eencae | 1:ea13195efe98 | 18 | arm.set_claw_calibration_values(1450,2160); |
eencae | 1:ea13195efe98 | 19 | // 0 , 90 |
eencae | 1:ea13195efe98 | 20 | arm.set_middle_calibration_values(1650,2700); |
eencae | 1:ea13195efe98 | 21 | arm.set_right_calibration_values(2000,1000); |
eencae | 1:ea13195efe98 | 22 | arm.set_left_calibration_values(1870,850); |
eencae | 0:be56eef4c199 | 23 | |
eencae | 3:13f56160c56e | 24 | // x stays constant |
eencae | 3:13f56160c56e | 25 | float x = 140.0; |
eencae | 3:13f56160c56e | 26 | |
eencae | 3:13f56160c56e | 27 | // arrays to store values in trajectory |
eencae | 3:13f56160c56e | 28 | float y[20]; |
eencae | 3:13f56160c56e | 29 | float z[20]; |
eencae | 3:13f56160c56e | 30 | |
eencae | 3:13f56160c56e | 31 | // loop through points in array |
eencae | 3:13f56160c56e | 32 | for(int i = 0; i < 20; i++) { |
eencae | 3:13f56160c56e | 33 | // this will create values in the range -100 to 100 |
eencae | 3:13f56160c56e | 34 | y[i] = -100.0 + (200.0*i)/(20-1); |
eencae | 3:13f56160c56e | 35 | // scaled parabola - z = (y/10)^2 + 50 |
eencae | 3:13f56160c56e | 36 | z[i] = pow(y[i]/10.0,2.0)+50; |
eencae | 3:13f56160c56e | 37 | // print to Cool Term to check - can plot in Excel |
eencae | 3:13f56160c56e | 38 | printf("%f , %f\n",y[i],z[i]); |
eencae | 3:13f56160c56e | 39 | } |
eencae | 3:13f56160c56e | 40 | |
eencae | 1:ea13195efe98 | 41 | while(1) { |
eencae | 3:13f56160c56e | 42 | |
eencae | 3:13f56160c56e | 43 | // loop through each point from y=-100 to 100 |
eencae | 3:13f56160c56e | 44 | // and move the arm to each point |
eencae | 3:13f56160c56e | 45 | for(int i = 0; i < 20; i++) { |
eencae | 3:13f56160c56e | 46 | arm.goto_xyz(x,y[i],z[i]); |
eencae | 3:13f56160c56e | 47 | // small delay before moving to next point |
eencae | 3:13f56160c56e | 48 | wait_ms(50); |
eencae | 3:13f56160c56e | 49 | } |
eencae | 1:ea13195efe98 | 50 | |
eencae | 3:13f56160c56e | 51 | // then loop back in the opposite direction |
eencae | 3:13f56160c56e | 52 | for(int i = 18; i > 0; i--) { |
eencae | 3:13f56160c56e | 53 | arm.goto_xyz(x,y[i],z[i]); |
eencae | 3:13f56160c56e | 54 | // small delay before moving to next point |
eencae | 3:13f56160c56e | 55 | wait_ms(50); |
eencae | 3:13f56160c56e | 56 | } |
eencae | 1:ea13195efe98 | 57 | |
eencae | 3:13f56160c56e | 58 | // the loop then restarts so the arm will go back and forth along the trajectory |
eencae | 1:ea13195efe98 | 59 | |
eencae | 1:ea13195efe98 | 60 | } |
eencae | 3:13f56160c56e | 61 | |
eencae | 1:ea13195efe98 | 62 | } |