Diff: r5driver.cpp

Revision:

12:e9f878ced6e7

Parent:

11:b5f75d25a4b3

Child:

15:d8940756d5d5

--- a/r5driver.cpp	Sun Nov 22 19:33:03 2015 +0000
+++ b/r5driver.cpp	Wed Jan 27 20:48:55 2016 +0000
@@ -1,14 +1,25 @@
-#include "StepperMotor.h"
 #include "navigation.h"
+#include "StepperDrive.h"
 #include "stdint.h"
 #include "mbed.h"
 
 Serial pc(USBTX, USBRX);
+InterruptIn start(SW1);
+DigitalOut led_red(LED_RED);
+DigitalOut led_green(LED_GREEN);
+bool active = false;
+
+void activate();
 
 int main()
 {
     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);
@@ -16,10 +27,10 @@
     // loading map
     r5nav.addGraphNode(0, 1, 18, 0);
     r5nav.addGraphNode(1, 0, 18, 180);
-    r5nav.addGraphNode(1, 2, 7, 0);
-    r5nav.addGraphNode(1, 3, 1, 90);
+    r5nav.addGraphNode(1, 2, 72, 0);
+    r5nav.addGraphNode(1, 3, 13.5, 90);
     r5nav.addGraphNode(2, 1, 72, 180);
-    r5nav.addGraphNode(3, 1, 13, 270);
+    r5nav.addGraphNode(3, 1, 13.5, 270);
     r5nav.addGraphNode(3, 4, 12, 180);
     r5nav.addGraphNode(3, 5, 23, 0);
     r5nav.addGraphNode(4, 3, 12, 0);
@@ -37,39 +48,59 @@
     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)
 
     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(YELLOW_VICTIM);
     pc.printf("\nDistance from 0 to YELLOW_VICTIM: %i\n", r5nav.getMinDist(YELLOW_VICTIM) );
     pc.printf("Route:\n");
-    r5nav.printRoute(pc);
-    r5nav.setVertex(YELLOW_VICTIM);
+    /*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("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("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("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);
+    //r5nav.printGraph(pc);
 
     return 0;
+}
+
+void activate()
+{
+    led_red = 1;
+    led_green = 0;
+    active = true;
 }
\ No newline at end of file