![](/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:
- 38:ebf73b654553
- Parent:
- 36:5b0bf9153c3d
- Child:
- 39:1e26cc57c8b7
--- a/r5driver.cpp Sat Mar 26 23:28:21 2016 +0000 +++ b/r5driver.cpp Fri Apr 01 15:56:01 2016 +0000 @@ -12,6 +12,7 @@ DigitalOut led_red(LED_RED); DigitalOut led_green(LED_GREEN); bool active = false; +bool v3Flag = 0; void activate(); @@ -31,7 +32,7 @@ wait(1e-6); } - StepperDrive drive(pc, PTE19, PTE18, 0, PTE3, PTE2, 1, 10.0625, 8.4800, 500); // 8.375 + StepperDrive drive(pc, PTE19, PTE18, 0, PTE3, PTE2, 1, 10.0625, 8.4800, 650); // 8.375 //(serial &, stepPinLeft, dirPinLeft, invertLeft, stepPinRight, dirPinRight, invertRight, wheelCircum, wheelSepar, periodUs) ShortRangeSensor shortRangeL(PTE0, PTE1); // verify i2c pins @@ -40,7 +41,7 @@ LongRangeSensor longRangeR(pc, PTB3); Gripper gripper(PTB2, PTB3); // grip pin, wrist pin - Scanner scanner(pc, drive, PTB0, PTB1, shortRangeL, shortRangeR, longRangeL, longRangeR, gripper, 0.075); + Scanner scanner(pc, drive, PTB0, PTB1, shortRangeL, shortRangeR, longRangeL, longRangeR, gripper, 1.0); // original period = 0.075 // (Serial &, PinName servoL, PinName servoR, shortRangeL, shortRangeR, longRangeL, longRangeR, period) Navigation r5nav(scanner, 25); @@ -68,7 +69,7 @@ r5nav.addGraphNode(3, 15, 6, 0); r5nav.addGraphNode(4, 3, 8, 0); // r5nav.addGraphNode(5, 3, 23, 180); - r5nav.addGraphNode(5, 6, 37, 0); + r5nav.addGraphNode(5, 6, 37, 358); // (-2 degrees) original angle = 0 r5nav.addGraphNode(5, 8, 12.5, 90); r5nav.addGraphNode(5, 15, 17, 180); r5nav.addGraphNode(6, 5, 37, 176.5); // original angle = 180 @@ -433,100 +434,282 @@ /* add hunt method call */ - if (scanner.getObjectFound() == 1) - { - r5nav.getShortestPath(19); - pc.printf("\n\nDistance from V3 to 19: %i\n", r5nav.getMinDist(19) ); - pc.printf("Route:\n"); - scanner.setLocalizeRightFlag(1); - scanner.setLocalizeLeftFlag(0); - r5nav.executeRoute(pc, drive); - wait(0.1); - - // at node 5 - r5nav.getShortestPath(5); - pc.printf("\n\nDistance from 19 to 5: %i\n", r5nav.getMinDist(5) ); - pc.printf("Route:\n"); - scanner.setLocalizeRightFlag(0); - scanner.setLocalizeLeftFlag(0); - r5nav.executeRoute(pc, drive); - wait(0.1); + r5nav.getShortestPath(19); + pc.printf("\n\nDistance from V3 to 19: %i\n", r5nav.getMinDist(19) ); + pc.printf("Route:\n"); + scanner.setLocalizeRightFlag(1); + scanner.setLocalizeLeftFlag(0); + r5nav.executeRoute(pc, drive); + wait(0.1); + + r5nav.getShortestPath(5); + pc.printf("\n\nDistance from 19 to 5: %i\n", r5nav.getMinDist(5) ); + pc.printf("Route:\n"); + scanner.setLocalizeRightFlag(0); + scanner.setLocalizeLeftFlag(0); + r5nav.executeRoute(pc, drive); + wait(0.1); + // at node 5 - r5nav.getShortestPath(15); - pc.printf("\n\nDistance from 5 to 15: %i\n", r5nav.getMinDist(15) ); - pc.printf("Route:\n"); - scanner.setLocalizeRightFlag(0); - scanner.setLocalizeLeftFlag(1); - r5nav.executeRoute(pc, drive); - wait(0.1); - - r5nav.getShortestPath(3); - pc.printf("\n\nDistance from 15 to 3: %i\n", r5nav.getMinDist(3) ); - pc.printf("Route:\n"); - scanner.setLocalizeRightFlag(0); - scanner.setLocalizeLeftFlag(1); - r5nav.executeRoute(pc, drive); - wait(0.1); - - if (scanner.getYellowFlag() == 1) + if (scanner.getObjectFound() == 1) { - r5nav.getShortestPath(YELLOW_DROP_ZONE); - pc.printf("\n\nDistance from %i to YELLOW_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(YELLOW_DROP_ZONE) ); + v3Flag = 1; + + r5nav.getShortestPath(15); + pc.printf("\n\nDistance from 5 to 15: %i\n", r5nav.getMinDist(15) ); pc.printf("Route:\n"); - //r5nav.printRoute(pc); - scanner.setLocalizeRightFlag(1); - scanner.setLocalizeLeftFlag(0); + scanner.setLocalizeRightFlag(0); + scanner.setLocalizeLeftFlag(1); + r5nav.executeRoute(pc, drive); + wait(0.1); + + r5nav.getShortestPath(3); + pc.printf("\n\nDistance from 15 to 3: %i\n", r5nav.getMinDist(3) ); + pc.printf("Route:\n"); + scanner.setLocalizeRightFlag(0); + scanner.setLocalizeLeftFlag(1); r5nav.executeRoute(pc, drive); wait(0.1); + + if (scanner.getYellowFlag() == 1) + { + r5nav.getShortestPath(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); + scanner.setLocalizeRightFlag(1); + scanner.setLocalizeLeftFlag(0); + r5nav.executeRoute(pc, drive); + wait(0.1); + + /* add drop peg code */ + + r5nav.getShortestPath(15) + pc.printf("\n\nDistance from %i to 15%i\n", r5nav.getVertex(), r5nav.getMinDist(15); + pc.printf("Route:\n"); + //r5nav.printRoute(pc); + scanner.setLocalizeLeftFlag(1); + scanner.setLocalizeRightFlag(0); + r5nav.executeRoute(pc, drive); + wait(0.1); + + r5nav.getShortestPath(5); + pc.printf("\n\nDistance from %i to 5: %i\n", r5nav.getVertex(), r5nav.getMinDist(5) ); + pc.printf("Route:\n"); + //r5nav.printRoute(pc); + scanner.setLocalizeRightFlag(1); + scanner.setLocalizeLeftFlag(0); + r5nav.executeRoute(pc, drive); + wait(0.1); + } + + else + { + r5nav.getShortestPath(1); + pc.printf("\n\nDistance from 3 to 1: %i\n", r5nav.getMinDist(1) ); + pc.printf("Route:\n"); + scanner.setLocalizeRightFlag(0); + scanner.setLocalizeLeftFlag(0); + r5nav.executeRoute(pc, drive); + wait(0.1); + + r5nav.getShortestPath(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); + scanner.setLocalizeRightFlag(1); + scanner.setLocalizeLeftFlag(0); + r5nav.executeRoute(pc, drive); + wait(0.1); + + r5nav.getShortestPath(1); + pc.printf("\n\nDistance from %i to 1: %i\n", r5nav.getVertex(), r5nav.getMinDist(1) ); + pc.printf("Route:\n"); + //r5nav.printRoute(pc); + scanner.setLocalizeRightFlag(0); + scanner.setLocalizeLeftFlag(1); + r5nav.executeRoute(pc, drive); + wait(0.1); + + r5nav.getShortestPath(3); + pc.printf("\n\nDistance from %i to 3: %i\n", r5nav.getVertex(), r5nav.getMinDist(3) ); + pc.printf("Route:\n"); + //r5nav.printRoute(pc); + scanner.setLocalizeRightFlag(0); + scanner.setLocalizeLeftFlag(0); + r5nav.executeRoute(pc, drive); + wait(0.1); + + r5nav.getShortestPath(15); + pc.printf("\n\nDistance from %i to 15: %i\n", r5nav.getVertex(), r5nav.getMinDist(15) ); + pc.printf("Route:\n"); + //r5nav.printRoute(pc); + scanner.setLocalizeRightFlag(0); + scanner.setLocalizeLeftFlag(1); + r5nav.executeRoute(pc, drive); + wait(0.1); + + r5nav.getShortestPath(5); + pc.printf("\n\nDistance from %i to 5: %i\n", r5nav.getVertex(), r5nav.getMinDist(5) ); + pc.printf("Route:\n"); + //r5nav.printRoute(pc); + scanner.setLocalizeRightFlag(1); + scanner.setLocalizeLeftFlag(0); + r5nav.executeRoute(pc, drive); + wait(0.1); + + /* 3rd peg retrieval complete + Begin 4th peg retrieval */ + } } - - else - { - r5nav.getShortestPath(1); - pc.printf("\n\nDistance from 3 to 1: %i\n", r5nav.getMinDist(1) ); + + // peg V3 not found and at node 5 + + r5nav.getShortestPath(6); + pc.printf("\n\nDistance from %i to 6: %i\n", r5nav.getVertex(), r5nav.getMinDist(6) ); + pc.printf("Route:\n"); + //r5nav.printRoute(pc); + scanner.setLocalizeRightFlag(1); + scanner.setLocalizeLeftFlag(0); + r5nav.executeRoute(pc, drive); + wait(0.1); + + r5nav.getShortestPath(14); + pc.printf("\n\nDistance from %i to 14: %i\n", r5nav.getVertex(), r5nav.getMinDist(14) ); + pc.printf("Route:\n"); + //r5nav.printRoute(pc); + scanner.setLocalizeRightFlag(0); + scanner.setLocalizeLeftFlag(0); + r5nav.executeRoute(pc, drive); + wait(0.1); + + r5nav.getShortestPath(20); + pc.printf("\n\nDistance from %i to 20: %i\n", r5nav.getVertex(), r5nav.getMinDist(20) ); + pc.printf("Route:\n"); + //r5nav.printRoute(pc); + scanner.setLocalizeRightFlag(1); + scanner.setLocalizeLeftFlag(0); + r5nav.executeRoute(pc, drive); + wait(0.1); + + /* add hunt code */ + + if (scanner.getObjectFound() == 1) + { + r5nav.getShortestPath(15); + pc.printf("\n\nDistance from 5 to 15: %i\n", r5nav.getMinDist(15) ); pc.printf("Route:\n"); scanner.setLocalizeRightFlag(0); scanner.setLocalizeLeftFlag(0); r5nav.executeRoute(pc, drive); wait(0.1); + // may need to break up above route - r5nav.getShortestPath(RED_DROP_ZONE); - pc.printf("\n\nDistance from %i to RED_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(RED_DROP_ZONE) ); + r5nav.getShortestPath(3); + pc.printf("\n\nDistance from 15 to 3: %i\n", r5nav.getMinDist(3) ); pc.printf("Route:\n"); - //r5nav.printRoute(pc); - scanner.setLocalizeRightFlag(1); - scanner.setLocalizeLeftFlag(0); + scanner.setLocalizeRightFlag(0); + scanner.setLocalizeLeftFlag(1); r5nav.executeRoute(pc, drive); wait(0.1); + + if (scanner.getYellowFlag() == 1) + { + r5nav.getShortestPath(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); + scanner.setLocalizeRightFlag(1); + scanner.setLocalizeLeftFlag(0); + r5nav.executeRoute(pc, drive); + wait(0.1); + + /* add drop peg code */ + + r5nav.getShortestPath(15) + pc.printf("\n\nDistance from %i to 15%i\n", r5nav.getVertex(), r5nav.getMinDist(15); + pc.printf("Route:\n"); + //r5nav.printRoute(pc); + scanner.setLocalizeLeftFlag(1); + scanner.setLocalizeRightFlag(0); + r5nav.executeRoute(pc, drive); + wait(0.1); + + r5nav.getShortestPath(5); + pc.printf("\n\nDistance from %i to 5: %i\n", r5nav.getVertex(), r5nav.getMinDist(5) ); + pc.printf("Route:\n"); + //r5nav.printRoute(pc); + scanner.setLocalizeRightFlag(1); + scanner.setLocalizeLeftFlag(0); + r5nav.executeRoute(pc, drive); + wait(0.1); + + /* may need to get back to 20 */ + } + + else + { + r5nav.getShortestPath(1); + pc.printf("\n\nDistance from 3 to 1: %i\n", r5nav.getMinDist(1) ); + pc.printf("Route:\n"); + scanner.setLocalizeRightFlag(0); + scanner.setLocalizeLeftFlag(0); + r5nav.executeRoute(pc, drive); + wait(0.1); + + r5nav.getShortestPath(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); + scanner.setLocalizeRightFlag(1); + scanner.setLocalizeLeftFlag(0); + r5nav.executeRoute(pc, drive); + wait(0.1); + + r5nav.getShortestPath(1); + pc.printf("\n\nDistance from %i to 1: %i\n", r5nav.getVertex(), r5nav.getMinDist(1) ); + pc.printf("Route:\n"); + //r5nav.printRoute(pc); + scanner.setLocalizeRightFlag(0); + scanner.setLocalizeLeftFlag(1); + r5nav.executeRoute(pc, drive); + wait(0.1); + + r5nav.getShortestPath(3); + pc.printf("\n\nDistance from %i to 3: %i\n", r5nav.getVertex(), r5nav.getMinDist(3) ); + pc.printf("Route:\n"); + //r5nav.printRoute(pc); + scanner.setLocalizeRightFlag(0); + scanner.setLocalizeLeftFlag(0); + r5nav.executeRoute(pc, drive); + wait(0.1); + + r5nav.getShortestPath(15); + pc.printf("\n\nDistance from %i to 15: %i\n", r5nav.getVertex(), r5nav.getMinDist(15) ); + pc.printf("Route:\n"); + //r5nav.printRoute(pc); + scanner.setLocalizeRightFlag(0); + scanner.setLocalizeLeftFlag(1); + r5nav.executeRoute(pc, drive); + wait(0.1); + + r5nav.getShortestPath(5); + pc.printf("\n\nDistance from %i to 5: %i\n", r5nav.getVertex(), r5nav.getMinDist(5) ); + pc.printf("Route:\n"); + //r5nav.printRoute(pc); + scanner.setLocalizeRightFlag(1); + scanner.setLocalizeLeftFlag(0); + r5nav.executeRoute(pc, drive); + wait(0.1); + + /* may need to get back to 20 */ + + /* V4 peg retrieval complete + Begin V5 peg retrieval */ + } } - } - else // peg V3 not found - { - /* need to implement rest of moves - - r5nav.getShortestPath(19); - pc.printf("\n\nDistance from 12 to 19: %i\n", r5nav.getMinDist(19) ); - pc.printf("Route:\n"); - scanner.setLocalizeRightFlag(1); - scanner.setLocalizeLeftFlag(0); - r5nav.executeRoute(pc, drive); - wait(0.1); - - // at node 5 - r5nav.getShortestPath(5); - pc.printf("\n\nDistance from 19 to 5: %i\n", r5nav.getMinDist(5) ); - pc.printf("Route:\n"); - scanner.setLocalizeRightFlag(0); - scanner.setLocalizeLeftFlag(0); - r5nav.executeRoute(pc, drive); - wait(0.1); */ - } - - /* 3rd peg retrieval complete - Begin 4th peg retrieval */ - - + /* add more code for for V5 and V6 */ /* drive.move(0, ((90.0)*(3.14159 / 180.0))); // return to initial angle