main.cpp

00001 /* MeArm Lab 3 Task 1
00002 
00003 (c) Dr Craig A. Evans, University of Leeds
00004 
00005 July 2017
00006 
00007 */
00008 
00009 #include "mbed.h"
00010 #include "MeArm.h"
00011 
00012 // create MeArm object - middle, left, right, claw
00013 MeArm arm(p21,p22,p23,p24);
00014 
00015 int main()
00016 {
00017     //                             open , closed
00018     arm.set_claw_calibration_values(1450,2160);
00019     //                                  0 , 90
00020     arm.set_middle_calibration_values(1650,2700);
00021     arm.set_right_calibration_values(2000,1000);
00022     arm.set_left_calibration_values(1870,850);
00023 
00024     // x stays constant
00025     float x = 140.0;
00026 
00027     // arrays to store values in trajectory
00028     float y[20];
00029     float z[20];
00030 
00031     // loop through points in array
00032     for(int i = 0; i < 20; i++) {
00033         // this will create values in the range -100 to 100
00034         y[i] = -100.0 + (200.0*i)/(20-1);
00035         // scaled parabola - z = (y/10)^2 + 50
00036         z[i] = pow(y[i]/10.0,2.0)+50;
00037         // print to Cool Term to check - can plot in Excel
00038         printf("%f , %f\n",y[i],z[i]);
00039     }
00040 
00041     while(1) {
00042 
00043         // loop through each point from y=-100 to 100
00044         // and move the arm to each point
00045         for(int i = 0; i < 20; i++) {
00046             arm.goto_xyz(x,y[i],z[i]);
00047             // small delay before moving to next point
00048             wait_ms(50);
00049         }
00050         
00051         // then loop back in the opposite direction
00052         for(int i = 18; i > 0; i--) {
00053             arm.goto_xyz(x,y[i],z[i]);
00054             // small delay before moving to next point
00055             wait_ms(50);
00056         }
00057         
00058         // the loop then restarts so the arm will go back and forth along the trajectory
00059         
00060     }
00061 
00062 }