![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Code to drive Team 1's robot for the 2016 R5 robotics competition.
Dependencies: mbed navigation R5_StepperDrive LongRangeSensor DistanceSensor scanner Gripper ColorSensor
Diff: r5driver.cpp
- Revision:
- 8:90af8914ce03
- Parent:
- 3:72b652325643
- Child:
- 9:a956ba674cea
diff -r a541859bbff0 -r 90af8914ce03 r5driver.cpp --- 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