Code to drive Team 1's robot for the 2016 R5 robotics competition.
Dependencies: mbed navigation R5_StepperDrive LongRangeSensor DistanceSensor scanner Gripper ColorSensor
r5driver.cpp
- Committer:
- j_j205
- Date:
- 2015-10-30
- Revision:
- 7:056f78006270
- Parent:
- 3:72b652325643
- Child:
- 9:a956ba674cea
File content as of revision 7:056f78006270:
#include "navigation.h" #include "stdint.h" #include "mbed.h" Serial pc(USBTX, USBRX); int main() { pc.baud(115200); Navigation r5nav(10); pc.printf("Navigation Object Created\n\n"); wait(0.1); // 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; 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); 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); 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; }