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:
- 20:53ab414d7b33
- Parent:
- 15:d8940756d5d5
- Child:
- 25:670a59096bf8
diff -r 9b279e2b2e2b -r 53ab414d7b33 r5driver.cpp --- a/r5driver.cpp Mon Feb 22 21:10:39 2016 +0000 +++ b/r5driver.cpp Tue Feb 23 23:36:14 2016 +0000 @@ -18,14 +18,14 @@ pc.baud(115200); start.mode(PullUp); /* Start/Stop button is active low needing PullUp */ start.fall(&activate); - + led_red = 0; led_green = 1; - + 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); @@ -36,7 +36,7 @@ r5nav.addGraphNode(0, 3, 9.875, 270); /* - // loading map + // loading r5 map r5nav.addGraphNode(0, 1, 18, 0); r5nav.addGraphNode(1, 0, 18, 180); r5nav.addGraphNode(1, 2, 72, 0); @@ -55,7 +55,7 @@ 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; @@ -64,41 +64,47 @@ StepperDrive drive(pc, PTE19, PTE18, 0, PTE3, PTE2, 1, 9.875, 6.4375, 500); //(serial &, stepPinLeft, dirPinLeft, invertLeft, stepPinRight, dirPinRight, invertRight, wheelCircum, wheelSepar, periodUs) + VL6180x shortRangeL(PTE0, PTE1, 0x52); // I2C1, 0x29 left-shifted by 1 bit + VL6180x shortRangeR(PTC9, PTC8, 0x52); // I2C0, 0x29 left-shifted by 1 bit + Gp2x longRangeL(PTB2); + Gp2x longRangeR(PTB3); + + Scanner (pc, PTB0, PTB1, shortRangeL, shortRangeR, longRangeL, longRangeR, 0.1); + // (Serial &, PinName servoL, PinName servoR, shortRangeL, shortRangeR, longRangeL, longRangeR, period) + 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 { wait(1e-6); } - + 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.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) ); @@ -132,14 +138,14 @@ 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; }