Texas State IEEE / Mbed 2 deprecated GrabTest

that thing for testing

Dependencies:   DistanceSensor Gripper R5_StepperDrive mbed

GrabTest.cpp@0:345761c4cf73, 2016-04-09 (annotated)

Committer:
Hypna
Date:
Sat Apr 09 04:27:41 2016 +0000
Revision:
0:345761c4cf73
Initial publish

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Hypna 0:345761c4cf73 1 #include "mbed.h"
Hypna 0:345761c4cf73 2 #include "DistanceSensor.h"
Hypna 0:345761c4cf73 3 #include "Gripper.h"
Hypna 0:345761c4cf73 4 #include "R5_StepperDrive.h"
Hypna 0:345761c4cf73 5 #include <cmath>
Hypna 0:345761c4cf73 6
Hypna 0:345761c4cf73 7 int main()
Hypna 0:345761c4cf73 8 {
Hypna 0:345761c4cf73 9 Serial bluetooth(PTE16,PTE17);
Hypna 0:345761c4cf73 10 DistanceSensor dsRight(PTC2,3);
Hypna 0:345761c4cf73 11 DistanceSensor dsLeft(PTC1,1);
Hypna 0:345761c4cf73 12 Gripper grip(PTE20,PTE21);
Hypna 0:345761c4cf73 13 StepperDrive drive(bluetooth,PTE19,PTE18,1,PTE3,PTE2,PTE22,0,10.0625,8.1875,700);
Hypna 0:345761c4cf73 14 double lDist, rDist;
Hypna 0:345761c4cf73 15 const int readThresh = 10;
Hypna 0:345761c4cf73 16 const double pi = 3.14159;
Hypna 0:345761c4cf73 17 const double theta = pi/4; //scan servo angle in radians
Hypna 0:345761c4cf73 18 const int k1 = x; //distance from plane of the sensors to center of rotation
Hypna 0:345761c4cf73 19 const int k2 = x; //distance from sensors to center line
Hypna 0:345761c4cf73 20 double a, b, c, A, B, adjust, dist;
Hypna 0:345761c4cf73 21 char side;
Hypna 0:345761c4cf73 22
Hypna 0:345761c4cf73 23 bluetooth.printf("Beginning Search\n\r");
Hypna 0:345761c4cf73 24 while(true)
Hypna 0:345761c4cf73 25 {
Hypna 0:345761c4cf73 26 lDist = dsLeft.getDistance();
Hypna 0:345761c4cf73 27 rDist = dsRight.getDistance();
Hypna 0:345761c4cf73 28
Hypna 0:345761c4cf73 29 if(lDist < readThresh || rDist < readThresh)
Hypna 0:345761c4cf73 30 break;
Hypna 0:345761c4cf73 31
Hypna 0:345761c4cf73 32 drive.move(2,0);
Hypna 0:345761c4cf73 33 }
Hypna 0:345761c4cf73 34
Hypna 0:345761c4cf73 35 bluetooth.printf("Range Found\n\r");
Hypna 0:345761c4cf73 36
Hypna 0:345761c4cf73 37 if(lDist < rDist)
Hypna 0:345761c4cf73 38 {
Hypna 0:345761c4cf73 39 c = lDist;
Hypna 0:345761c4cf73 40 side = 'l';
Hypna 0:345761c4cf73 41 bluetooth.printf("Using Left Distance %d\n\r", c);
Hypna 0:345761c4cf73 42 }
Hypna 0:345761c4cf73 43 else
Hypna 0:345761c4cf73 44 {
Hypna 0:345761c4cf73 45 c = rDist;
Hypna 0:345761c4cf73 46 side = 'r';
Hypna 0:345761c4cf73 47 bluetooth.printf("Using Left Distance %d\n\r", c);
Hypna 0:345761c4cf73 48 }
Hypna 0:345761c4cf73 49
Hypna 0:345761c4cf73 50 a = c*sin(theta);
Hypna 0:345761c4cf73 51 b = sqrt(pow(c,2) - pow(a,2)); //sqrt(c^2 - a^2)
Hypna 0:345761c4cf73 52 A = a + k1;
Hypna 0:345761c4cf73 53
Hypna 0:345761c4cf73 54 if(side == 'l')
Hypna 0:345761c4cf73 55 B = b - k2;
Hypna 0:345761c4cf73 56 else
Hypna 0:345761c4cf73 57 B = k2 - b;
Hypna 0:345761c4cf73 58
Hypna 0:345761c4cf73 59 adjust = pi/2 - atan(A/B);
Hypna 0:345761c4cf73 60 dist = sqrt(pow(A,2) + pow(B,2));
Hypna 0:345761c4cf73 61
Hypna 0:345761c4cf73 62 bluetooth.printf("Approach Move is %f at %f rads\n\r", dist, adjust);
Hypna 0:345761c4cf73 63
Hypna 0:345761c4cf73 64 drive.move(dist,adjust);
Hypna 0:345761c4cf73 65 while(!drive.isMoveDone())
Hypna 0:345761c4cf73 66 wait_us(5);
Hypna 0:345761c4cf73 67
Hypna 0:345761c4cf73 68 bluetooth.printf("Move Complete\n\r");
Hypna 0:345761c4cf73 69 bluetooth.printf("Grabbing\n\r");
Hypna 0:345761c4cf73 70 grip.grip();
Hypna 0:345761c4cf73 71 grip.lift();
Hypna 0:345761c4cf73 72 bluetooth.printf("Done!\n\r");
Hypna 0:345761c4cf73 73
Hypna 0:345761c4cf73 74 return 0;
Hypna 0:345761c4cf73 75 }