![](/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:
- 28:17c17157e728
- Parent:
- 26:d99d078921a0
- Child:
- 29:e77e8891d985
--- a/r5driver.cpp Tue Mar 22 03:01:56 2016 +0000 +++ b/r5driver.cpp Tue Mar 22 18:11:56 2016 +0000 @@ -6,6 +6,7 @@ #include "mbed.h" Serial pc(USBTX, USBRX); +// Serial pc(PTE16,PTE17); // bluetooth InterruptIn start(SW1); DigitalOut led_red(LED_RED); DigitalOut led_green(LED_GREEN); @@ -16,6 +17,7 @@ int main() { pc.baud(115200); + // pc.baud(9600); /* interface via Bluetooth at 9600 */ start.mode(PullUp); /* Start/Stop button is active low needing PullUp */ start.fall(&activate); @@ -39,7 +41,7 @@ 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); + Navigation r5nav(scanner, 11); wait(0.1); r5nav.addGraphNode(0, 1, 10.0625, 0); @@ -71,7 +73,18 @@ r5nav.addGraphNode(8, 5, 12, 270); r5nav.addGraphNode(8, 9, 35, 180); r5nav.addGraphNode(9, 8, 35, 0); - + r5nav.addGraphNode(8, 10, 37, 0); + r5nav.addGraphNode(6, 10, 12, 90); + r5nav.addGraphNode(10, 6, 12, 270); + r5nav.addGraphNode(8, 11, 12, 90); + r5nav.addGraphNode(11, 8, 12, 270); + r5nav.addGraphNode(11, 12, 35, 180); + r5nav.addGraphNode(12, 11, 35, 0); + r5nav.addGraphNode(11, 13, 37, 0); + r5nav.addGraphNode(13, 11, 37, 180); + r5nav.addGraphNode(13, 14, 12, 0); + r5nav.addGraphNode(14, 13, 12, 180); + const uint16_t YELLOW_VICTIM = 7; const uint16_t RED_VICTIM = 9; const uint16_t YELLOW_DROP_ZONE = 4;