![](/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:
- 29:e77e8891d985
- Parent:
- 28:17c17157e728
- Child:
- 30:c07381708ffd
--- a/r5driver.cpp Tue Mar 22 18:11:56 2016 +0000 +++ b/r5driver.cpp Fri Mar 25 19:51:18 2016 +0000 @@ -1,4 +1,5 @@ #include "navigation.h" +#include "Gripper.h" #include "scanner.h" #include "StepperDrive.h" #include "VL6180x.h" @@ -41,9 +42,9 @@ Scanner scanner(pc, PTB0, PTB1, shortRangeL, shortRangeR, longRangeL, longRangeR, 0.075); // (Serial &, PinName servoL, PinName servoR, shortRangeL, shortRangeR, longRangeL, longRangeR, period) - Navigation r5nav(scanner, 11); + Navigation r5nav(scanner, 15); wait(0.1); - + /* r5nav.addGraphNode(0, 1, 10.0625, 0); r5nav.addGraphNode(1, 0, 10.0625, 180); r5nav.addGraphNode(1, 2, 10.0625, 270); @@ -52,8 +53,8 @@ r5nav.addGraphNode(3, 2, 10.0625, 0); r5nav.addGraphNode(3, 0, 10.0625, 90); r5nav.addGraphNode(0, 3, 10.0625, 270); - - /* +*/ + // loading r5 map r5nav.addGraphNode(0, 1, 18, 0); r5nav.addGraphNode(1, 0, 18, 180); @@ -89,8 +90,8 @@ const uint16_t RED_VICTIM = 9; const uint16_t YELLOW_DROP_ZONE = 4; const uint16_t RED_DROP_ZONE = 2; - */ - + +/* r5nav.getShortestPath(1); pc.printf("\n\nDistance from 0 to 1: %i\n", r5nav.getMinDist(1) ); pc.printf("Route:\n"); @@ -114,8 +115,8 @@ 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"); @@ -147,15 +148,15 @@ //r5nav.setVertex(RED_DROP_ZONE); r5nav.executeRoute(pc, drive); wait(0.1); - */ +/* 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; }