![](/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:
- 15:d8940756d5d5
- Parent:
- 12:e9f878ced6e7
- Child:
- 19:4abc62759078
- Child:
- 20:53ab414d7b33
diff -r 24bfe928d958 -r d8940756d5d5 r5driver.cpp --- a/r5driver.cpp Wed Jan 27 20:55:41 2016 +0000 +++ b/r5driver.cpp Wed Feb 17 16:48:15 2016 +0000 @@ -1,5 +1,7 @@ #include "navigation.h" +#include "scanner.h" #include "StepperDrive.h" +#include "VL6180x.h" #include "stdint.h" #include "mbed.h" @@ -23,7 +25,17 @@ Navigation r5nav(10); pc.printf("Navigation Object Created\n\n"); wait(0.1); + + r5nav.addGraphNode(0, 1, 9.875, 0); + r5nav.addGraphNode(1, 0, 9.875, 180); + r5nav.addGraphNode(1, 2, 9.875, 270); + r5nav.addGraphNode(2, 1, 9.875, 90); + r5nav.addGraphNode(2, 3, 9.875, 180); + r5nav.addGraphNode(3, 2, 9.875, 0); + r5nav.addGraphNode(3, 0, 9.875, 90); + r5nav.addGraphNode(0, 3, 9.875, 270); + /* // loading map r5nav.addGraphNode(0, 1, 18, 0); r5nav.addGraphNode(1, 0, 18, 180); @@ -48,12 +60,14 @@ 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) + */ + StepperDrive drive(pc, PTE19, PTE18, 0, PTE3, PTE2, 1, 9.875, 6.4375, 500); + //(serial &, 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 @@ -61,40 +75,72 @@ wait(1e-6); } - r5nav.getShortestPath(YELLOW_VICTIM); - pc.printf("\nDistance from 0 to YELLOW_VICTIM: %i\n", r5nav.getMinDist(YELLOW_VICTIM) ); + r5nav.getShortestPath(1); + pc.printf("\n\nDistance from 0 to 1: %i\n", r5nav.getMinDist(1) ); + pc.printf("Route:\n"); + r5nav.executeRoute(pc, drive); + wait(0.1); + + r5nav.getShortestPath(2); + pc.printf("\n\nDistance from 1 to 2: %i\n", r5nav.getMinDist(2) ); + pc.printf("Route:\n"); + r5nav.executeRoute(pc, drive); + wait(0.1); + + r5nav.getShortestPath(3); + pc.printf("\n\nDistance from 2 to 3: %i\n", r5nav.getMinDist(3) ); pc.printf("Route:\n"); - /*r5nav.printRoute(pc); - r5nav.setVertex(YELLOW_VICTIM);*/ + r5nav.executeRoute(pc, drive); + wait(0.1); + + r5nav.getShortestPath(0); + pc.printf("\n\nDistance from 3 to 0: %i\n", r5nav.getMinDist(0) ); + pc.printf("Route:\n"); + r5nav.executeRoute(pc, drive); + wait(0.1); + + /* + r5nav.getShortestPath(YELLOW_VICTIM); + pc.printf("\n\nDistance from 0 to YELLOW_VICTIM: %i\n", r5nav.getMinDist(YELLOW_VICTIM) ); + pc.printf("Route:\n"); + //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("\n\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("\n\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("\n\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); - + drive.move(0, ((90.0)*(3.14159 / 180.0))); // return to initial angle + // wait for move to complete + while(!drive.isMoveDone()) + { + wait(1e-6); + } + + pc.printf("\n\nExercise Complete"); return 0; }