Diff: r5driver.cpp

Revision:

29:e77e8891d985

Parent:

28:17c17157e728

Child:

30:c07381708ffd

diff -r 17c17157e728 -r e77e8891d985 r5driver.cpp
--- a/r5driver.cpp	Tue Mar 22 18:11:56 2016 +0000
+++ b/r5driver.cpp	Fri Mar 25 19:51:18 2016 +0000
@@ -1,4 +1,5 @@
 #include "navigation.h"
+#include "Gripper.h"
 #include "scanner.h"
 #include "StepperDrive.h"
 #include "VL6180x.h"
@@ -41,9 +42,9 @@
     Scanner scanner(pc, PTB0, PTB1, shortRangeL, shortRangeR, longRangeL, longRangeR, 0.075);
         // (Serial &, PinName servoL, PinName servoR, shortRangeL, shortRangeR, longRangeL, longRangeR, period)
 
-    Navigation r5nav(scanner, 11);
+    Navigation r5nav(scanner, 15);
     wait(0.1);
-
+    /*
     r5nav.addGraphNode(0, 1, 10.0625, 0);
     r5nav.addGraphNode(1, 0, 10.0625, 180);
     r5nav.addGraphNode(1, 2, 10.0625, 270);
@@ -52,8 +53,8 @@
     r5nav.addGraphNode(3, 2, 10.0625, 0);
     r5nav.addGraphNode(3, 0, 10.0625, 90);
     r5nav.addGraphNode(0, 3, 10.0625, 270);
-
-    /*
+*/
+    
     // loading r5 map
     r5nav.addGraphNode(0, 1, 18, 0);
     r5nav.addGraphNode(1, 0, 18, 180);
@@ -89,8 +90,8 @@
     const uint16_t RED_VICTIM = 9;
     const uint16_t YELLOW_DROP_ZONE = 4;
     const uint16_t RED_DROP_ZONE = 2;
-    */
-
+    
+/*
     r5nav.getShortestPath(1);
     pc.printf("\n\nDistance from 0 to 1: %i\n", r5nav.getMinDist(1) );
     pc.printf("Route:\n");
@@ -114,8 +115,8 @@
     pc.printf("Route:\n");
     r5nav.executeRoute(pc, drive);
     wait(0.1);
-
-    /*
+*/
+    
     r5nav.getShortestPath(YELLOW_VICTIM);
     pc.printf("\n\nDistance from 0 to YELLOW_VICTIM: %i\n", r5nav.getMinDist(YELLOW_VICTIM) );
     pc.printf("Route:\n");
@@ -147,15 +148,15 @@
     //r5nav.setVertex(RED_DROP_ZONE);
     r5nav.executeRoute(pc, drive);
     wait(0.1);
-    */
 
+/*
     drive.move(0, ((90.0)*(3.14159 / 180.0))); // return to initial angle
     // wait for move to complete
     while(!drive.isMoveDone())
     {
         wait(1e-6);
     }
-
+*/
     pc.printf("\n\nExercise Complete");
     return 0;
 }