Diff: r5driver.cpp

Revision:

43:048c307bf8ac

Parent:

42:1284491771ff

Child:

44:d4207182bfc2

--- a/r5driver.cpp	Fri Apr 08 05:00:33 2016 +0000
+++ b/r5driver.cpp	Sat Apr 09 00:37:08 2016 +0000
@@ -1,7 +1,6 @@
 #include "mbed.h"
 #include "StepperDrive.h"
 #include "LongRangeSensor.h"
-#include "DistanceSensor.h"
 #include "navigation.h"
 #include "Gripper.h"
 
@@ -33,11 +32,6 @@
     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");
@@ -118,7 +112,7 @@
     r5map.addGraphNode(28, 29, 4, 180);
     r5map.addGraphNode(29, 28, 4, 0);
     r5map.addGraphNode(29, 30, 7, 180);
-    r5map.addGraphNode(30, 17, 11, 270);
+    r5map.addGraphNode(30, 17, 9, 270);
     r5map.addGraphNode(30, 29, 6, 0);
     r5map.addGraphNode(30, 31, 6, 180);
     r5map.addGraphNode(30, 36, 12, 90);
@@ -212,7 +206,7 @@
     // 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, 5.0, 0);
+    reverseMove(drive, 4.5, 0);
     while(!drive.isMoveDone())
     {
         wait(1e-6);
@@ -270,7 +264,7 @@
     /* 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.newLocalizeLeftReverse();
+    r5map.newLocalizeRightReverse();
     reverseMove(drive, 2.0, 0);
     // wait for move to complete
     while(!drive.isMoveDone())
@@ -278,7 +272,7 @@
         wait(1e-6);
     } 
     r5map.setVertex(34);
-    r5map.newLocalizeLeftReverse();
+    r5map.newLocalizeRightReverse();
     reverseMove(drive, 7.0, 0);
     // wait for move to complete
     while(!drive.isMoveDone())
@@ -286,7 +280,7 @@
         wait(1e-6);
     }  
     r5map.setVertex(33);
-    r5map.newLocalizeLeftReverse();
+    r5map.newLocalizeRightReverse();
     reverseMove(drive, 7.0, 0);
     // wait for move to complete
     while(!drive.isMoveDone())
@@ -294,7 +288,7 @@
         wait(1e-6);
     }  
     r5map.setVertex(32);
-    r5map.newLocalizeLeftReverse();
+    r5map.newLocalizeRightReverse();
     reverseMove(drive, 7.0, 0);
     // wait for move to complete
     while(!drive.isMoveDone())
@@ -302,7 +296,7 @@
         wait(1e-6);
     } 
     r5map.setVertex(31);
-    r5map.newLocalizeLeftReverse();
+    r5map.newLocalizeRightReverse();
     reverseMove(drive, 6.0, 0);
     // wait for move to complete
     while(!drive.isMoveDone())
@@ -320,74 +314,6 @@
     r5map.executeRoute();
     wait(0.1);
     
-    /*// 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();
-    reverseMove(drive, 4.0, 0);
-    while(!drive.isMoveDone())
-    {
-        wait(1e-6);
-    }
-    r5map.setVertex(10);
-    r5map.localizeLeftReverse();
-    reverseMove(drive, 8.0, 0);
-    while(!drive.isMoveDone())
-    {
-        wait(1e-6);
-    }
-    r5map.setVertex(9);
-    r5map.localizeLeftReverse();
-    reverseMove(drive, 8.0, 0);
-    while(!drive.isMoveDone())
-    {
-        wait(1e-6);
-    }
-    r5map.setVertex(8);
-    r5map.localizeLeftReverse();
-    reverseMove(drive, 8.0, 0);
-    while(!drive.isMoveDone())
-    {
-        wait(1e-6);
-    }
-    r5map.setVertex(7);
-    r5map.localizeLeftReverse();
-    reverseMove(drive, 8.0, 0);
-    while(!drive.isMoveDone())
-    {
-        wait(1e-6);
-    }
-    r5map.setVertex(6);
-    r5map.localizeLeftReverse();
-    reverseMove(drive, 8.0, 0);
-    while(!drive.isMoveDone())
-    {
-        wait(1e-6);
-    }
-    r5map.setVertex(5);
-    r5map.localizeLeftReverse();
-    reverseMove(drive, 8.0, 0);
-    while(!drive.isMoveDone())
-    {
-        wait(1e-6);
-    }
-    r5map.setVertex(4);
-    r5map.localizeLeftReverse();
-    reverseMove(drive, 8.0, 0);
-    while(!drive.isMoveDone())
-    {
-        wait(1e-6);
-    }
-    r5map.setVertex(3);
-    r5map.localizeLeftReverse();
-    reverseMove(drive, 8.0, 0);
-    while(!drive.isMoveDone())
-    {
-        wait(1e-6);
-    }
-    r5map.setVertex(2);
-    r5map.localizeLeftReverse();*/
-    
     target = V3;
     r5map.getShortestPath(target);
     pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V3) );
@@ -452,4 +378,4 @@
      // restore original values for invertLeft and invertRight
      drive.setInvertLeft(!drive.getInvertLeft());
      drive.setInvertRight(!drive.getInvertRight());
-} 
\ No newline at end of file
+}
\ No newline at end of file