Diff: r5driver.cpp

Revision:

40:2d33bb4d6d6f

Parent:

39:1e26cc57c8b7

Child:

41:029135230ebf

--- a/r5driver.cpp	Wed Apr 06 22:07:38 2016 +0000
+++ b/r5driver.cpp	Fri Apr 08 01:36:13 2016 +0000
@@ -1,6 +1,7 @@
 #include "mbed.h"
 #include "StepperDrive.h"
 #include "LongRangeSensor.h"
+#include "DistanceSensor.h"
 #include "navigation.h"
 #include "Gripper.h"
 
@@ -32,13 +33,18 @@
     led_red = 0;
     led_green = 1;
     
+    DistanceSensor shortRangeL(PTC2, 2);
+    DistanceSensor shortRangeR(PTC1, 3);
+pc.printf("\nShort Range Sensors created");
+bluetooth.printf("\nShort Range Sensors created");
+    
     LongRangeSensor longRangeL(bluetooth, PTB2);
     LongRangeSensor longRangeR(bluetooth, PTB3);
 pc.printf("\nLong Range Sensors created");
 bluetooth.printf("\nLong Range Sensors created");
 
     StepperDrive drive(bluetooth, PTE19, PTE18, 1, PTE3, PTE2, 0, 10.0625, 
-        8.4800, 650); // 8.375
+        8.2, 700); // 8.4800
               /* (serial &, stepPinLeft, dirPinLeft, invertLeft, 
               stepPinRight, dirPinRight, invertRight, wheelCircum, 
               wheelSepar, periodUs) */
@@ -52,12 +58,12 @@
 bluetooth.printf("\nNavigation created");
     
     //loading r5 map...
-    r5map.addGraphNode(0, 1, 6, 0);
-    r5map.addGraphNode(1, 0, 6, 180);
+    r5map.addGraphNode(0, 1, 6.75, 0);
+    r5map.addGraphNode(1, 0, 6.75, 180);
     r5map.addGraphNode(1, 2, 7, 0);
     r5map.addGraphNode(2, 1, 7, 180);
     r5map.addGraphNode(2, 3, 8, 0);
-    r5map.addGraphNode(2, 12, 15, 90);
+    r5map.addGraphNode(2, 12, 14.75, 90);
     r5map.addGraphNode(3, 2, 8, 180);
     r5map.addGraphNode(3, 4, 8, 0);
     r5map.addGraphNode(4, 3, 8, 180);
@@ -75,7 +81,7 @@
     r5map.addGraphNode(10, 9, 8, 180);
     r5map.addGraphNode(10, 11, 4, 0);
     r5map.addGraphNode(11, 10, 4, 180);
-    r5map.addGraphNode(12, 2, 15, 270);
+    r5map.addGraphNode(12, 2, 14.75, 270);
     r5map.addGraphNode(12, 13, 8, 180);
     r5map.addGraphNode(12, 14, 6, 0);
     r5map.addGraphNode(13, 12, 8, 0);
@@ -126,7 +132,7 @@
     r5map.addGraphNode(34, 35, 2, 180);
     r5map.addGraphNode(35, 34, 2, 0);
     r5map.addGraphNode(36, 30, 12, 270);
-    r5map.addGraphNode(36, 37, 6, 180);
+    r5map.addGraphNode(36, 37, 7, 180);
     r5map.addGraphNode(37, 36, 7, 0);
     r5map.addGraphNode(37, 38, 7, 180);
     r5map.addGraphNode(38, 37, 7, 0);
@@ -193,21 +199,20 @@
         wait(1e-6);
     }
     
-    int target = V1;    
+    int target;
+    target = V1;    
     r5map.getShortestPath(target);
     pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V1) );
     bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V1) );
     r5map.executeRoute();
     wait(0.1);
     
-//    gripper.lower();
-//    gripper.grip();
-//    gripper.lift();
+//    call to hunt
 
-    /* back up to safe turn-around location */
+    // back up to safe turn-around location 
     pc.printf("\nBack up to safe turn around location");
     bluetooth.printf("\nBack up to safe turn around location");
-    reverseMove(drive, 6.0, 0);
+    reverseMove(drive, 5.0, 0);
     while(!drive.isMoveDone())
     {
         wait(1e-6);
@@ -221,7 +226,7 @@
     r5map.executeRoute();
     wait(0.1);
     
-    /* back up to safe turn-around location */
+    // back up to safe turn-around location 
     pc.printf("\nBack up to a safe turn around location");
     bluetooth.printf("\nBack up to a safe turn around location");
     r5map.localizeLeftReverse();
@@ -234,6 +239,27 @@
     r5map.setVertex(12);
     r5map.localizeLeftReverse();
     
+    // turn 180 degrees to left
+    pc.printf("\nTurn left 180 degrees");
+    bluetooth.printf("\nTurn left 180 degrees");    
+    drive.move(0, -180.0*(3.14159/180));
+    // wait for move to complete
+    while(!drive.isMoveDone())
+    {
+        wait(1e-6);
+    }
+    r5map.setAngle(0);
+    
+    // move to compensate for coming up short
+    pc.printf("\nCompensation move forward 2.0");
+    bluetooth.printf("\nCompensation move forward 2.0");    
+    drive.move(2.0, 0);
+    // wait for move to complete
+    while(!drive.isMoveDone())
+    {
+        wait(1e-6);
+    }
+    
     target = V2;
     r5map.getShortestPath(target);
     pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V2) );
@@ -294,7 +320,7 @@
     r5map.executeRoute();
     wait(0.1);
     
-    /* back up to safe turn around location 
+    /*// back up to safe turn around location 
     pc.printf("\nBack up to a safe turn around location");
     bluetooth.printf("\nBack up to a safe turn around location");
     r5map.localizeLeftReverse();