Diff: r5driver.cpp

Revision:

15:d8940756d5d5

Parent:

12:e9f878ced6e7

Child:

19:4abc62759078

Child:

20:53ab414d7b33

--- a/r5driver.cpp	Wed Jan 27 20:55:41 2016 +0000
+++ b/r5driver.cpp	Wed Feb 17 16:48:15 2016 +0000
@@ -1,5 +1,7 @@
 #include "navigation.h"
+#include "scanner.h"
 #include "StepperDrive.h"
+#include "VL6180x.h"
 #include "stdint.h"
 #include "mbed.h"
 
@@ -23,7 +25,17 @@
     Navigation r5nav(10);
     pc.printf("Navigation Object Created\n\n");
     wait(0.1);
+    
+    r5nav.addGraphNode(0, 1, 9.875, 0);
+    r5nav.addGraphNode(1, 0, 9.875, 180);
+    r5nav.addGraphNode(1, 2, 9.875, 270);
+    r5nav.addGraphNode(2, 1, 9.875, 90);
+    r5nav.addGraphNode(2, 3, 9.875, 180);
+    r5nav.addGraphNode(3, 2, 9.875, 0);
+    r5nav.addGraphNode(3, 0, 9.875, 90);
+    r5nav.addGraphNode(0, 3, 9.875, 270);
 
+    /*
     // loading map
     r5nav.addGraphNode(0, 1, 18, 0);
     r5nav.addGraphNode(1, 0, 18, 180);
@@ -48,12 +60,14 @@
     const uint16_t RED_VICTIM = 9;
     const uint16_t YELLOW_DROP_ZONE = 4;
     const uint16_t RED_DROP_ZONE = 2;
-    
-    StepperDrive drive(pc, PTE19, PTE18, 1,PTE3, PTE2, 0, 5.0, 5.0, 1000);
-              //(stepPinLeft, dirPinLeft, invertLeft, stepPinRight, dirPinRight, invertRight, wheelCircum, wheelSepar, periodUs)
+    */
+    StepperDrive drive(pc, PTE19, PTE18, 0, PTE3, PTE2, 1, 9.875, 6.4375, 500);
+              //(serial &, stepPinLeft, dirPinLeft, invertLeft, stepPinRight, dirPinRight, invertRight, wheelCircum, wheelSepar, periodUs)
 
     pc.printf("Size of graph is %i\n", r5nav.graphSize() );
     wait(0.1);
+    
+
 
     pc.printf("\nWaiting for START BUTTON\n");
     while(!active) // wait for start_button
@@ -61,40 +75,72 @@
         wait(1e-6);
     }
     
-    r5nav.getShortestPath(YELLOW_VICTIM);
-    pc.printf("\nDistance from 0 to YELLOW_VICTIM: %i\n", r5nav.getMinDist(YELLOW_VICTIM) );
+    r5nav.getShortestPath(1);
+    pc.printf("\n\nDistance from 0 to 1: %i\n", r5nav.getMinDist(1) );
+    pc.printf("Route:\n");
+    r5nav.executeRoute(pc, drive);
+    wait(0.1);
+    
+    r5nav.getShortestPath(2);
+    pc.printf("\n\nDistance from 1 to 2: %i\n", r5nav.getMinDist(2) );
+    pc.printf("Route:\n");
+    r5nav.executeRoute(pc, drive);
+    wait(0.1);
+    
+    r5nav.getShortestPath(3);
+    pc.printf("\n\nDistance from 2 to 3: %i\n", r5nav.getMinDist(3) );
     pc.printf("Route:\n");
-    /*r5nav.printRoute(pc);
-    r5nav.setVertex(YELLOW_VICTIM);*/
+    r5nav.executeRoute(pc, drive);
+    wait(0.1);
+    
+    r5nav.getShortestPath(0);
+    pc.printf("\n\nDistance from 3 to 0: %i\n", r5nav.getMinDist(0) );
+    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");
+    //r5nav.printRoute(pc);
+    //r5nav.setVertex(YELLOW_VICTIM)
     r5nav.executeRoute(pc, drive);
     wait(0.1);
 
     r5nav.getShortestPath(YELLOW_DROP_ZONE);
-    pc.printf("\nDistance from %i to YELLOW_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(YELLOW_DROP_ZONE) );
+    pc.printf("\n\nDistance from %i to YELLOW_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(YELLOW_DROP_ZONE) );
     pc.printf("Route:\n");
-    /*r5nav.printRoute(pc);
-    r5nav.setVertex(YELLOW_DROP_ZONE);*/
+    //r5nav.printRoute(pc);
+    //r5nav.setVertex(YELLOW_DROP_ZONE);
     r5nav.executeRoute(pc, drive);
     wait(0.1);
 
     r5nav.getShortestPath(RED_VICTIM);
-    pc.printf("\nDistance from %i to RED_VICTIM: %i\n", r5nav.getVertex(), r5nav.getMinDist(RED_VICTIM) );
+    pc.printf("\n\nDistance from %i to RED_VICTIM: %i\n", r5nav.getVertex(), r5nav.getMinDist(RED_VICTIM) );
     pc.printf("Route:\n");
-    /*r5nav.printRoute(pc);
-    r5nav.setVertex(RED_VICTIM);*/
+    //r5nav.printRoute(pc);
+    //r5nav.setVertex(RED_VICTIM);
     r5nav.executeRoute(pc, drive);
     wait(0.1);
 
     r5nav.getShortestPath(RED_DROP_ZONE);
-    pc.printf("\nDistance from %i to RED_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(RED_DROP_ZONE) );
+    pc.printf("\n\nDistance from %i to RED_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(RED_DROP_ZONE) );
     pc.printf("Route:\n");
-    /*r5nav.printRoute(pc);
-    r5nav.setVertex(RED_DROP_ZONE);*/
+    //r5nav.printRoute(pc);
+    //r5nav.setVertex(RED_DROP_ZONE);
     r5nav.executeRoute(pc, drive);
     wait(0.1);
+    */
     
-    //r5nav.printGraph(pc);
-
+    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;
 }