Code to drive Team 1's robot for the 2016 R5 robotics competition.
Dependencies: mbed navigation R5_StepperDrive LongRangeSensor DistanceSensor scanner Gripper ColorSensor
r5driver.cpp
- Committer:
- j_j205
- Date:
- 2016-04-01
- Revision:
- 38:ebf73b654553
- Parent:
- 36:5b0bf9153c3d
- Child:
- 39:1e26cc57c8b7
File content as of revision 38:ebf73b654553:
#include "navigation.h" #include "Gripper.h" #include "scanner.h" #include "StepperDrive.h" #include "ShortRangeSensor.h" #include "stdint.h" #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); bool active = false; bool v3Flag = 0; void activate(); 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); led_red = 0; led_green = 1; 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.4800, 650); // 8.375 //(serial &, stepPinLeft, dirPinLeft, invertLeft, stepPinRight, dirPinRight, invertRight, wheelCircum, wheelSepar, periodUs) ShortRangeSensor shortRangeL(PTE0, PTE1); // verify i2c pins ShortRangeSensor shortRangeR(PTC9, PTC8); // verify i2c pins LongRangeSensor longRangeL(pc, PTB2); LongRangeSensor longRangeR(pc, PTB3); Gripper gripper(PTB2, PTB3); // grip pin, wrist pin 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); wait(0.1); /* 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 r5nav.addGraphNode(0, 1, 13, 0); r5nav.addGraphNode(1, 0, 13, 180); r5nav.addGraphNode(1, 2, 68, 356.5); // original angle = 0 r5nav.addGraphNode(1, 3, 15, 90); r5nav.addGraphNode(2, 1, 68, 180); r5nav.addGraphNode(3, 1, 15, 270); r5nav.addGraphNode(3, 4, 8, 180); // r5nav.addGraphNode(3, 5, 23, 356.5); // (-5 degrees) original angle = 0 r5nav.addGraphNode(3, 15, 6, 0); r5nav.addGraphNode(4, 3, 8, 0); // r5nav.addGraphNode(5, 3, 23, 180); 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 r5nav.addGraphNode(6, 7, 6, 0); r5nav.addGraphNode(6, 10, 12.5, 90); r5nav.addGraphNode(7, 6, 6, 180); r5nav.addGraphNode(8, 5, 12.75, 270); // original distance = 12.5 // r5nav.addGraphNode(8, 9, 35, 174.5); // original angle = 180 // r5nav.addGraphNode(8, 10, 37, 0); r5nav.addGraphNode(8, 11, 12, 90); r5nav.addGraphNode(8, 16, 6, 180); r5nav.addGraphNode(8, 17, 6, 0); // r5nav.addGraphNode(9, 8, 35, 0); r5nav.addGraphNode(9, 16, 23, 0); r5nav.addGraphNode(10, 6, 12.5, 270); r5nav.addGraphNode(10, 13, 12, 90); r5nav.addGraphNode(10, 18, 6, 180); r5nav.addGraphNode(11, 8, 12, 270); r5nav.addGraphNode(11, 12, 35, 180); r5nav.addGraphNode(11, 19, 6, 180); r5nav.addGraphNode(12, 11, 35, 0); r5nav.addGraphNode(12, 19, 23, 0); r5nav.addGraphNode(13, 10, 12, 270); r5nav.addGraphNode(13, 14, 12, 0); r5nav.addGraphNode(14, 13, 12, 180); r5nav.addGraphNode(14, 20, 21, 90); r5nav.addGraphNode(15, 3, 6, 180); r5nav.addGraphNode(15, 5, 17, 0); r5nav.addGraphNode(16, 8, 6, 0); r5nav.addGraphNode(16, 9, 23, 180); r5nav.addGraphNode(17, 8, 6, 180); r5nav.addGraphNode(17, 18, 25, 0); r5nav.addGraphNode(18, 10, 6, 0); r5nav.addGraphNode(18, 17, 25, 180); r5nav.addGraphNode(19, 11, 6, 0); r5nav.addGraphNode(19, 12, 23, 180); r5nav.addGraphNode(20, 14, 21, 270); r5nav.addGraphNode(20, 21, 24, 90); r5nav.addGraphNode(21, 20, 24, 270); r5nav.addGraphNode(21, 22, 4, 180); r5nav.addGraphNode(22, 21, 4, 0); r5nav.addGraphNode(22, 23, 80, 180); r5nav.addGraphNode(23, 22, 80, 0); r5nav.addGraphNode(23, 24, 2, 270); r5nav.addGraphNode(24, 23, 2, 90); const uint16_t V1 = 7; const uint16_t V2 = 9; const uint16_t V3 = 12; const uint16_t V4 = 20; const uint16_t V5 = 22; const uint16_t V6 = 24; 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"); 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); */ /* Begin 1st peg retrieval */ r5nav.getShortestPath(1); pc.printf("\n\nDistance from 0 to 1: %i\n", r5nav.getMinDist(1) ); pc.printf("Route:\n"); scanner.setLocalizeRightFlag(1); scanner.setLocalizeLeftFlag(0); r5nav.executeRoute(pc, drive); wait(0.1); r5nav.getShortestPath(3); pc.printf("\n\nDistance from 1 to 3: %i\n", r5nav.getMinDist(3) ); pc.printf("Route:\n"); scanner.setLocalizeRightFlag(0); scanner.setLocalizeLeftFlag(0); r5nav.executeRoute(pc, drive); wait(0.1); r5nav.getShortestPath(15); pc.printf("\n\nDistance from 3 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(V1); pc.printf("\n\nDistance from 15 to V1: %i\n", r5nav.getMinDist(V1) ); pc.printf("Route:\n"); scanner.setLocalizeRightFlag(1); scanner.setLocalizeLeftFlag(0); r5nav.executeRoute(pc, drive); wait(0.1); /* add hunt method call */ r5nav.getShortestPath(15); pc.printf("\n\nDistance from V1 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(1); scanner.setLocalizeLeftFlag(0); 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); } 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); } /* 1st peg retrieval complete Begin 2nd peg retrieval */ if (r5nav.getVertex() == YELLOW_DROP_ZONE) { r5nav.getShortestPath(3); pc.printf("\n\nDistance from YELLOW_DROP_ZONE to 3: %i\n", r5nav.getMinDist(3) ); pc.printf("Route:\n"); scanner.setLocalizeRightFlag(0); scanner.setLocalizeLeftFlag(1); r5nav.executeRoute(pc, drive); wait(0.1); } else { r5nav.getShortestPath(1); pc.printf("\n\nDistance from RED_DROP_ZONE to 1: %i\n", r5nav.getMinDist(1) ); 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 1 to 3: %i\n", r5nav.getMinDist(3) ); pc.printf("Route:\n"); scanner.setLocalizeRightFlag(0); scanner.setLocalizeLeftFlag(0); r5nav.executeRoute(pc, drive); wait(0.1); } r5nav.getShortestPath(15); pc.printf("\n\nDistance from 3 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(5); pc.printf("\n\nDistance from 15 to 5: %i\n", r5nav.getMinDist(5) ); pc.printf("Route:\n"); scanner.setLocalizeRightFlag(1); scanner.setLocalizeLeftFlag(0); r5nav.executeRoute(pc, drive); wait(0.1); r5nav.getShortestPath(16); pc.printf("\n\nDistance from 5 to 16: %i\n", r5nav.getMinDist(16) ); pc.printf("Route:\n"); scanner.setLocalizeRightFlag(0); scanner.setLocalizeLeftFlag(0); r5nav.executeRoute(pc, drive); wait(0.1); r5nav.getShortestPath(V2); pc.printf("\n\nDistance from 16 to V2: %i\n", r5nav.getMinDist(V2) ); pc.printf("Route:\n"); scanner.setLocalizeRightFlag(1); scanner.setLocalizeLeftFlag(0); r5nav.executeRoute(pc, drive); wait(0.1); /* add hunt method call */ r5nav.getShortestPath(16); pc.printf("\n\nDistance from V2 to 16: %i\n", r5nav.getMinDist(16) ); 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 16 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(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) { 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); } 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); } /* 2nd peg retrieval complete Begin 3nd peg retrieval */ if (r5nav.getVertex() == YELLOW_DROP_ZONE) { r5nav.getShortestPath(3); pc.printf("\n\nDistance from YELLOW_DROP_ZONE to 3: %i\n", r5nav.getMinDist(3) ); pc.printf("Route:\n"); scanner.setLocalizeRightFlag(0); scanner.setLocalizeLeftFlag(1); r5nav.executeRoute(pc, drive); wait(0.1); } else { r5nav.getShortestPath(1); pc.printf("\n\nDistance from RED_DROP_ZONE to 1: %i\n", r5nav.getMinDist(1) ); 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 1 to 3: %i\n", r5nav.getMinDist(3) ); pc.printf("Route:\n"); scanner.setLocalizeRightFlag(0); scanner.setLocalizeLeftFlag(0); r5nav.executeRoute(pc, drive); wait(0.1); } r5nav.getShortestPath(15); pc.printf("\n\nDistance from 3 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(5); pc.printf("\n\nDistance from 15 to 5: %i\n", r5nav.getMinDist(5) ); pc.printf("Route:\n"); scanner.setLocalizeRightFlag(1); scanner.setLocalizeLeftFlag(0); r5nav.executeRoute(pc, drive); wait(0.1); r5nav.getShortestPath(19); pc.printf("\n\nDistance from 5 to 19: %i\n", r5nav.getMinDist(19) ); pc.printf("Route:\n"); scanner.setLocalizeRightFlag(0); scanner.setLocalizeLeftFlag(0); r5nav.executeRoute(pc, drive); wait(0.1); r5nav.getShortestPath(V3); pc.printf("\n\nDistance from 19 to V3: %i\n", r5nav.getMinDist(V3) ); pc.printf("Route:\n"); scanner.setLocalizeRightFlag(0); scanner.setLocalizeLeftFlag(1); r5nav.executeRoute(pc, drive); wait(0.1); /* add hunt method call */ 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 if (scanner.getObjectFound() == 1) { v3Flag = 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(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 */ } } // 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(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); /* 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 */ } } /* add more code for for V5 and V6 */ /* 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; } void activate() { led_red = 1; led_green = 0; active = true; }