![](/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:
- 25:670a59096bf8
- Parent:
- 20:53ab414d7b33
- Child:
- 26:d99d078921a0
--- a/r5driver.cpp Sun Mar 20 23:31:33 2016 +0000 +++ b/r5driver.cpp Mon Mar 21 01:32:17 2016 +0000 @@ -22,18 +22,34 @@ led_red = 0; led_green = 1; - Navigation r5nav(10); - pc.printf("Navigation Object Created\n\n"); + pc.printf("\nWaiting for START BUTTON\n"); + while(!active) // wait for start_button + { + wait(1e-6); + } + + StepperDrive drive(pc, PTE19, PTE18, 0, PTE3, PTE2, 1, 10.0625, 8.3750, 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 scanner(pc, PTB0, PTB1, shortRangeL, shortRangeR, longRangeL, longRangeR, 0.075); + // (Serial &, PinName servoL, PinName servoR, shortRangeL, shortRangeR, longRangeL, longRangeR, period) + + Navigation r5nav(scanner, 10); 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); + r5nav.addGraphNode(0, 1, 10.0625, 0); + r5nav.addGraphNode(1, 0, 10.0625, 180); + r5nav.addGraphNode(1, 2, 10.0625, 270); + r5nav.addGraphNode(2, 1, 10.0625, 90); + r5nav.addGraphNode(2, 3, 10.0625, 180); + r5nav.addGraphNode(3, 2, 10.0625, 0); + r5nav.addGraphNode(3, 0, 10.0625, 90); + r5nav.addGraphNode(0, 3, 10.0625, 270); /* // loading r5 map @@ -61,25 +77,6 @@ const uint16_t YELLOW_DROP_ZONE = 4; const uint16_t RED_DROP_ZONE = 2; */ - 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) ); @@ -155,4 +152,4 @@ led_red = 1; led_green = 0; active = true; -} \ No newline at end of file +}