Diff: r5driver.cpp

Revision:

20:53ab414d7b33

Parent:

15:d8940756d5d5

Child:

25:670a59096bf8

--- a/r5driver.cpp	Mon Feb 22 21:10:39 2016 +0000
+++ b/r5driver.cpp	Tue Feb 23 23:36:14 2016 +0000
@@ -18,14 +18,14 @@
     pc.baud(115200);
     start.mode(PullUp); /* Start/Stop button is active low needing PullUp */
     start.fall(&activate);
-    
+
     led_red = 0;
     led_green = 1;
-       
+
     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);
@@ -36,7 +36,7 @@
     r5nav.addGraphNode(0, 3, 9.875, 270);
 
     /*
-    // loading map
+    // loading r5 map
     r5nav.addGraphNode(0, 1, 18, 0);
     r5nav.addGraphNode(1, 0, 18, 180);
     r5nav.addGraphNode(1, 2, 72, 0);
@@ -55,7 +55,7 @@
     r5nav.addGraphNode(8, 5, 12, 270);
     r5nav.addGraphNode(8, 9, 35, 180);
     r5nav.addGraphNode(9, 8, 35, 0);
-    
+
     const uint16_t YELLOW_VICTIM = 7;
     const uint16_t RED_VICTIM = 9;
     const uint16_t YELLOW_DROP_ZONE = 4;
@@ -64,41 +64,47 @@
     StepperDrive drive(pc, PTE19, PTE18, 0, PTE3, PTE2, 1, 9.875, 6.4375, 500);
               //(serial &, stepPinLeft, dirPinLeft, invertLeft, stepPinRight, dirPinRight, invertRight, wheelCircum, wheelSepar, periodUs)
 
+    VL6180x shortRangeL(PTE0, PTE1, 0x52); // I2C1, 0x29 left-shifted by 1 bit
+    VL6180x shortRangeR(PTC9, PTC8, 0x52); // I2C0, 0x29 left-shifted by 1 bit
+    Gp2x longRangeL(PTB2);
+    Gp2x longRangeR(PTB3);
+
+    Scanner (pc, PTB0, PTB1, shortRangeL, shortRangeR, longRangeL, longRangeR, 0.1);
+        // (Serial &, PinName servoL, PinName servoR, shortRangeL, shortRangeR, longRangeL, longRangeR, period)
+
     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
     {
         wait(1e-6);
     }
-    
+
     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.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) );
@@ -132,14 +138,14 @@
     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;
 }