Diff: r5driver.cpp

Revision:

8:90af8914ce03

Parent:

3:72b652325643

Child:

9:a956ba674cea

--- a/r5driver.cpp	Thu Oct 29 16:29:40 2015 +0000
+++ b/r5driver.cpp	Fri Oct 30 15:52:06 2015 +0000
@@ -1,4 +1,5 @@
-#include "weighted_graph.h"
+#include "navigation.h"
+#include "stdint.h"
 #include "mbed.h"
 
 Serial pc(USBTX, USBRX);
@@ -7,21 +8,65 @@
 {
     pc.baud(115200);
     
-    WeightedGraph testMap(6);
-    pc.printf("testMap created\n\n");
+    Navigation r5nav(10);
+    pc.printf("Navigation Object Created\n\n");
+    wait(0.1);
 
-    int result;
-    result = testMap.loadMap("testmap.txt");
+    // 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(2, 1, 72, 180);
+    r5nav.addGraphNode(3, 1, 13, 270);
+    r5nav.addGraphNode(3, 4, 12, 180);
+    r5nav.addGraphNode(3, 5, 23, 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(6, 7, 12, 0);
+    r5nav.addGraphNode(7, 6, 12, 180);
+    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;
+    const uint16_t RED_DROP_ZONE = 2;
 
-    if (result == 1)
-        pc.printf("testmap.txt did not open properly\n\n");
+    pc.printf("Size of graph is %i\n", r5nav.graphSize() );
+    wait(0.1);
+
+    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);
+    wait(0.1);
 
-    else
-        pc.printf("testmap.txt loaded sucessfully\n\n");
+    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);
+    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);
+    wait(0.1);
 
-    pc.printf("testMap has %i vertices\n", testMap.getVerts() );
+    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);
+    wait(0.1);
 
     return 0;
 }
\ No newline at end of file