Diff: r5driver.cpp

Revision:

25:670a59096bf8

Parent:

20:53ab414d7b33

Child:

26:d99d078921a0

--- a/r5driver.cpp	Sun Mar 20 23:31:33 2016 +0000
+++ b/r5driver.cpp	Mon Mar 21 01:32:17 2016 +0000
@@ -22,18 +22,34 @@
     led_red = 0;
     led_green = 1;
 
-    Navigation r5nav(10);
-    pc.printf("Navigation Object Created\n\n");
+    pc.printf("\nWaiting for START BUTTON\n");
+    while(!active) // wait for start_button
+    {
+        wait(1e-6);
+    }
+
+    StepperDrive drive(pc, PTE19, PTE18, 0, PTE3, PTE2, 1, 10.0625, 8.3750, 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 scanner(pc, PTB0, PTB1, shortRangeL, shortRangeR, longRangeL, longRangeR, 0.075);
+        // (Serial &, PinName servoL, PinName servoR, shortRangeL, shortRangeR, longRangeL, longRangeR, period)
+
+    Navigation r5nav(scanner, 10);
     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);
+    r5nav.addGraphNode(0, 1, 10.0625, 0);
+    r5nav.addGraphNode(1, 0, 10.0625, 180);
+    r5nav.addGraphNode(1, 2, 10.0625, 270);
+    r5nav.addGraphNode(2, 1, 10.0625, 90);
+    r5nav.addGraphNode(2, 3, 10.0625, 180);
+    r5nav.addGraphNode(3, 2, 10.0625, 0);
+    r5nav.addGraphNode(3, 0, 10.0625, 90);
+    r5nav.addGraphNode(0, 3, 10.0625, 270);
 
     /*
     // loading r5 map
@@ -61,25 +77,6 @@
     const uint16_t YELLOW_DROP_ZONE = 4;
     const uint16_t RED_DROP_ZONE = 2;
     */
-    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) );
@@ -155,4 +152,4 @@
     led_red = 1;
     led_green = 0;
     active = true;
-}
\ No newline at end of file
+}