Diff: GrabTest.cpp

Revision:

0:345761c4cf73

diff -r 000000000000 -r 345761c4cf73 GrabTest.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GrabTest.cpp	Sat Apr 09 04:27:41 2016 +0000
@@ -0,0 +1,75 @@
+#include "mbed.h"
+#include "DistanceSensor.h"
+#include "Gripper.h"
+#include "R5_StepperDrive.h"
+#include <cmath>
+
+int main()
+{
+    Serial bluetooth(PTE16,PTE17);
+    DistanceSensor dsRight(PTC2,3);
+    DistanceSensor dsLeft(PTC1,1);
+    Gripper grip(PTE20,PTE21);
+    StepperDrive drive(bluetooth,PTE19,PTE18,1,PTE3,PTE2,PTE22,0,10.0625,8.1875,700);
+    double lDist, rDist;
+    const int readThresh = 10;
+    const double pi = 3.14159;
+    const double theta = pi/4; //scan servo angle in radians
+    const int k1 = x; //distance from plane of the sensors to center of rotation
+    const int k2 = x; //distance from sensors to center line
+    double a, b, c, A, B, adjust, dist;
+    char side;
+
+    bluetooth.printf("Beginning Search\n\r");
+    while(true)
+    {
+        lDist = dsLeft.getDistance();
+        rDist = dsRight.getDistance();
+        
+        if(lDist < readThresh || rDist < readThresh)
+            break;
+        
+        drive.move(2,0);
+    }
+    
+    bluetooth.printf("Range Found\n\r");
+    
+    if(lDist < rDist)
+    {
+        c = lDist;
+        side = 'l';
+        bluetooth.printf("Using Left Distance %d\n\r", c);
+    }
+    else
+    {
+        c = rDist;
+        side = 'r';
+        bluetooth.printf("Using Left Distance %d\n\r", c);
+    }
+        
+    a = c*sin(theta);
+    b = sqrt(pow(c,2) - pow(a,2)); //sqrt(c^2 - a^2)
+    A = a + k1;
+    
+    if(side == 'l')
+        B = b - k2;
+    else
+        B = k2 - b;
+
+    adjust =  pi/2 - atan(A/B);
+    dist = sqrt(pow(A,2) + pow(B,2));
+    
+    bluetooth.printf("Approach Move is %f at %f rads\n\r", dist, adjust);
+    
+    drive.move(dist,adjust);
+    while(!drive.isMoveDone())
+        wait_us(5);
+    
+    bluetooth.printf("Move Complete\n\r");
+    bluetooth.printf("Grabbing\n\r");
+    grip.grip();
+    grip.lift();
+    bluetooth.printf("Done!\n\r");
+    
+    return 0;
+}
\ No newline at end of file