Diff: r5driver.cpp

Revision:

28:17c17157e728

Parent:

26:d99d078921a0

Child:

29:e77e8891d985

--- a/r5driver.cpp	Tue Mar 22 03:01:56 2016 +0000
+++ b/r5driver.cpp	Tue Mar 22 18:11:56 2016 +0000
@@ -6,6 +6,7 @@
 #include "mbed.h"
 
 Serial pc(USBTX, USBRX);
+// Serial pc(PTE16,PTE17); // bluetooth
 InterruptIn start(SW1);
 DigitalOut led_red(LED_RED);
 DigitalOut led_green(LED_GREEN);
@@ -16,6 +17,7 @@
 int main()
 {
     pc.baud(115200);
+    // pc.baud(9600);  /* interface via Bluetooth at 9600 */
     start.mode(PullUp); /* Start/Stop button is active low needing PullUp */
     start.fall(&activate);
 
@@ -39,7 +41,7 @@
     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);
+    Navigation r5nav(scanner, 11);
     wait(0.1);
 
     r5nav.addGraphNode(0, 1, 10.0625, 0);
@@ -71,7 +73,18 @@
     r5nav.addGraphNode(8, 5, 12, 270);
     r5nav.addGraphNode(8, 9, 35, 180);
     r5nav.addGraphNode(9, 8, 35, 0);
-
+    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(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;