Diff: r5driver.cpp

Revision:

32:82bfece56f8f

Parent:

30:c07381708ffd

Child:

35:4e09da30bda2

--- a/r5driver.cpp	Sat Mar 26 05:11:53 2016 +0000
+++ b/r5driver.cpp	Sat Mar 26 05:26:23 2016 +0000
@@ -2,7 +2,7 @@
 #include "Gripper.h"
 #include "scanner.h"
 #include "StepperDrive.h"
-#include "VL6180x.h"
+#include "ShortRangeSensor.h"
 #include "stdint.h"
 #include "mbed.h"
 
@@ -31,11 +31,11 @@
         wait(1e-6);
     }
 
-    StepperDrive drive(pc, PTE19, PTE18, 0, PTE3, PTE2, 1, 10.0625, 8.3750, 500);
+    StepperDrive drive(pc, PTE19, PTE18, 0, PTE3, PTE2, 1, 10.0625, 8.4800, 500); // 8.375
               //(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
+    ShortRangeSensor shortRangeL(PTE0, PTE1); // verify i2c pins
+    ShortRangeSensor shortRangeR(PTC9, PTC8); // verify i2c pins
     LongRangeSensor longRangeL(pc, PTB2);
     LongRangeSensor longRangeR(pc, PTB3);
 
@@ -44,7 +44,7 @@
 
     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);
@@ -54,43 +54,41 @@
     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);
-    r5nav.addGraphNode(1, 2, 72, 0);
-    r5nav.addGraphNode(1, 3, 13.5, 90);
+    r5nav.addGraphNode(0, 1, 13, 0);
+    r5nav.addGraphNode(1, 0, 13, 180);
+    r5nav.addGraphNode(1, 2, 72, 356.5); // original angle = 0
+    r5nav.addGraphNode(1, 3, 15, 90);
     r5nav.addGraphNode(2, 1, 72, 180);
-    r5nav.addGraphNode(3, 1, 13.5, 270);
+    r5nav.addGraphNode(3, 1, 15, 270);
     r5nav.addGraphNode(3, 4, 12, 180);
-    r5nav.addGraphNode(3, 5, 23, 0);
+    r5nav.addGraphNode(3, 5, 23, 356.5); // (-5 degrees) original angle = 0
     r5nav.addGraphNode(4, 3, 12, 0);
     r5nav.addGraphNode(5, 3, 23, 180);
     r5nav.addGraphNode(5, 6, 37, 0);
-    r5nav.addGraphNode(5, 8, 12, 90);
-    r5nav.addGraphNode(6, 5, 37, 180);
+    r5nav.addGraphNode(5, 8, 12.5, 90);
+    r5nav.addGraphNode(6, 5, 37, 176.5); // original angle = 180
     r5nav.addGraphNode(6, 7, 12, 0);
+    r5nav.addGraphNode(6, 10, 12.5, 90);
     r5nav.addGraphNode(7, 6, 12, 180);
-    r5nav.addGraphNode(8, 5, 12, 270);
-    r5nav.addGraphNode(8, 9, 35, 180);
-    r5nav.addGraphNode(9, 8, 35, 0);
+    r5nav.addGraphNode(8, 5, 12.75, 270); // original distance = 12.5
+    r5nav.addGraphNode(8, 9, 35, 174.5); // original angle = 180
     r5nav.addGraphNode(8, 10, 37, 0);
-    r5nav.addGraphNode(6, 10, 12, 90);
-    r5nav.addGraphNode(10, 6, 12, 270);
     r5nav.addGraphNode(8, 11, 12, 90);
+    r5nav.addGraphNode(9, 8, 35, 0);
+    r5nav.addGraphNode(10, 6, 12.5, 270);
     r5nav.addGraphNode(11, 8, 12, 270);
     r5nav.addGraphNode(11, 12, 35, 180);
     r5nav.addGraphNode(12, 11, 35, 0);
-    r5nav.addGraphNode(11, 13, 37, 0);
-    r5nav.addGraphNode(13, 11, 37, 180);
     r5nav.addGraphNode(13, 14, 12, 0);
     r5nav.addGraphNode(14, 13, 12, 180);
-    
+
     const uint16_t YELLOW_VICTIM = 7;
     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) );
@@ -116,7 +114,7 @@
     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");