Texas State IEEE / Mbed 2 deprecated R5 2016 Robotics Team 1

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@38:ebf73b654553, 2016-04-01 (annotated)

Committer:
j_j205
Date:
Fri Apr 01 15:56:01 2016 +0000
Revision:
38:ebf73b654553
Parent:
36:5b0bf9153c3d
Child:
39:1e26cc57c8b7
4/1/16 JJ

Who changed what in which revision?

UserRevisionLine numberNew contents of line
j_j205 8:90af8914ce03 1 #include "navigation.h"
j_j205 29:e77e8891d985 2 #include "Gripper.h"
j_j205 15:d8940756d5d5 3 #include "scanner.h"
j_j205 12:e9f878ced6e7 4 #include "StepperDrive.h"
j_j205 32:82bfece56f8f 5 #include "ShortRangeSensor.h"
j_j205 8:90af8914ce03 6 #include "stdint.h"
j_j205 2:3b162e764c9d 7 #include "mbed.h"
j_j205 0:a3c39d3359ac 8
j_j205 0:a3c39d3359ac 9 Serial pc(USBTX, USBRX);
j_j205 28:17c17157e728 10 // Serial pc(PTE16,PTE17); // bluetooth
j_j205 12:e9f878ced6e7 11 InterruptIn start(SW1);
j_j205 12:e9f878ced6e7 12 DigitalOut led_red(LED_RED);
j_j205 12:e9f878ced6e7 13 DigitalOut led_green(LED_GREEN);
j_j205 12:e9f878ced6e7 14 bool active = false;
j_j205 38:ebf73b654553 15 bool v3Flag = 0;
j_j205 12:e9f878ced6e7 16
j_j205 12:e9f878ced6e7 17 void activate();
j_j205 0:a3c39d3359ac 18
j_j205 0:a3c39d3359ac 19 int main()
j_j205 0:a3c39d3359ac 20 {
j_j205 0:a3c39d3359ac 21 pc.baud(115200);
j_j205 28:17c17157e728 22 // pc.baud(9600); /* interface via Bluetooth at 9600 */
j_j205 12:e9f878ced6e7 23 start.mode(PullUp); /* Start/Stop button is active low needing PullUp */
j_j205 12:e9f878ced6e7 24 start.fall(&activate);
j_j205 20:53ab414d7b33 25
j_j205 12:e9f878ced6e7 26 led_red = 0;
j_j205 12:e9f878ced6e7 27 led_green = 1;
j_j205 20:53ab414d7b33 28
j_j205 25:670a59096bf8 29 pc.printf("\nWaiting for START BUTTON\n");
j_j205 25:670a59096bf8 30 while(!active) // wait for start_button
j_j205 25:670a59096bf8 31 {
j_j205 25:670a59096bf8 32 wait(1e-6);
j_j205 25:670a59096bf8 33 }
j_j205 25:670a59096bf8 34
j_j205 38:ebf73b654553 35 StepperDrive drive(pc, PTE19, PTE18, 0, PTE3, PTE2, 1, 10.0625, 8.4800, 650); // 8.375
j_j205 25:670a59096bf8 36 //(serial &, stepPinLeft, dirPinLeft, invertLeft, stepPinRight, dirPinRight, invertRight, wheelCircum, wheelSepar, periodUs)
j_j205 25:670a59096bf8 37
j_j205 32:82bfece56f8f 38 ShortRangeSensor shortRangeL(PTE0, PTE1); // verify i2c pins
j_j205 32:82bfece56f8f 39 ShortRangeSensor shortRangeR(PTC9, PTC8); // verify i2c pins
j_j205 26:d99d078921a0 40 LongRangeSensor longRangeL(pc, PTB2);
j_j205 26:d99d078921a0 41 LongRangeSensor longRangeR(pc, PTB3);
j_j205 36:5b0bf9153c3d 42 Gripper gripper(PTB2, PTB3); // grip pin, wrist pin
j_j205 25:670a59096bf8 43
j_j205 38:ebf73b654553 44 Scanner scanner(pc, drive, PTB0, PTB1, shortRangeL, shortRangeR, longRangeL, longRangeR, gripper, 1.0); // original period = 0.075
j_j205 25:670a59096bf8 45 // (Serial &, PinName servoL, PinName servoR, shortRangeL, shortRangeR, longRangeL, longRangeR, period)
j_j205 25:670a59096bf8 46
j_j205 35:4e09da30bda2 47 Navigation r5nav(scanner, 25);
j_j205 8:90af8914ce03 48 wait(0.1);
j_j205 32:82bfece56f8f 49 /*
j_j205 25:670a59096bf8 50 r5nav.addGraphNode(0, 1, 10.0625, 0);
j_j205 25:670a59096bf8 51 r5nav.addGraphNode(1, 0, 10.0625, 180);
j_j205 25:670a59096bf8 52 r5nav.addGraphNode(1, 2, 10.0625, 270);
j_j205 25:670a59096bf8 53 r5nav.addGraphNode(2, 1, 10.0625, 90);
j_j205 25:670a59096bf8 54 r5nav.addGraphNode(2, 3, 10.0625, 180);
j_j205 25:670a59096bf8 55 r5nav.addGraphNode(3, 2, 10.0625, 0);
j_j205 25:670a59096bf8 56 r5nav.addGraphNode(3, 0, 10.0625, 90);
j_j205 25:670a59096bf8 57 r5nav.addGraphNode(0, 3, 10.0625, 270);
j_j205 29:e77e8891d985 58 */
j_j205 32:82bfece56f8f 59
j_j205 20:53ab414d7b33 60 // loading r5 map
j_j205 32:82bfece56f8f 61 r5nav.addGraphNode(0, 1, 13, 0);
j_j205 32:82bfece56f8f 62 r5nav.addGraphNode(1, 0, 13, 180);
j_j205 35:4e09da30bda2 63 r5nav.addGraphNode(1, 2, 68, 356.5); // original angle = 0
j_j205 32:82bfece56f8f 64 r5nav.addGraphNode(1, 3, 15, 90);
j_j205 35:4e09da30bda2 65 r5nav.addGraphNode(2, 1, 68, 180);
j_j205 32:82bfece56f8f 66 r5nav.addGraphNode(3, 1, 15, 270);
j_j205 35:4e09da30bda2 67 r5nav.addGraphNode(3, 4, 8, 180);
j_j205 35:4e09da30bda2 68 // r5nav.addGraphNode(3, 5, 23, 356.5); // (-5 degrees) original angle = 0
j_j205 35:4e09da30bda2 69 r5nav.addGraphNode(3, 15, 6, 0);
j_j205 35:4e09da30bda2 70 r5nav.addGraphNode(4, 3, 8, 0);
j_j205 35:4e09da30bda2 71 // r5nav.addGraphNode(5, 3, 23, 180);
j_j205 38:ebf73b654553 72 r5nav.addGraphNode(5, 6, 37, 358); // (-2 degrees) original angle = 0
j_j205 32:82bfece56f8f 73 r5nav.addGraphNode(5, 8, 12.5, 90);
j_j205 35:4e09da30bda2 74 r5nav.addGraphNode(5, 15, 17, 180);
j_j205 32:82bfece56f8f 75 r5nav.addGraphNode(6, 5, 37, 176.5); // original angle = 180
j_j205 35:4e09da30bda2 76 r5nav.addGraphNode(6, 7, 6, 0);
j_j205 32:82bfece56f8f 77 r5nav.addGraphNode(6, 10, 12.5, 90);
j_j205 35:4e09da30bda2 78 r5nav.addGraphNode(7, 6, 6, 180);
j_j205 32:82bfece56f8f 79 r5nav.addGraphNode(8, 5, 12.75, 270); // original distance = 12.5
j_j205 35:4e09da30bda2 80 // r5nav.addGraphNode(8, 9, 35, 174.5); // original angle = 180
j_j205 35:4e09da30bda2 81 // r5nav.addGraphNode(8, 10, 37, 0);
j_j205 28:17c17157e728 82 r5nav.addGraphNode(8, 11, 12, 90);
j_j205 35:4e09da30bda2 83 r5nav.addGraphNode(8, 16, 6, 180);
j_j205 35:4e09da30bda2 84 r5nav.addGraphNode(8, 17, 6, 0);
j_j205 35:4e09da30bda2 85 // r5nav.addGraphNode(9, 8, 35, 0);
j_j205 35:4e09da30bda2 86 r5nav.addGraphNode(9, 16, 23, 0);
j_j205 32:82bfece56f8f 87 r5nav.addGraphNode(10, 6, 12.5, 270);
j_j205 35:4e09da30bda2 88 r5nav.addGraphNode(10, 13, 12, 90);
j_j205 35:4e09da30bda2 89 r5nav.addGraphNode(10, 18, 6, 180);
j_j205 28:17c17157e728 90 r5nav.addGraphNode(11, 8, 12, 270);
j_j205 28:17c17157e728 91 r5nav.addGraphNode(11, 12, 35, 180);
j_j205 35:4e09da30bda2 92 r5nav.addGraphNode(11, 19, 6, 180);
j_j205 28:17c17157e728 93 r5nav.addGraphNode(12, 11, 35, 0);
j_j205 35:4e09da30bda2 94 r5nav.addGraphNode(12, 19, 23, 0);
j_j205 35:4e09da30bda2 95 r5nav.addGraphNode(13, 10, 12, 270);
j_j205 28:17c17157e728 96 r5nav.addGraphNode(13, 14, 12, 0);
j_j205 28:17c17157e728 97 r5nav.addGraphNode(14, 13, 12, 180);
j_j205 35:4e09da30bda2 98 r5nav.addGraphNode(14, 20, 21, 90);
j_j205 35:4e09da30bda2 99 r5nav.addGraphNode(15, 3, 6, 180);
j_j205 35:4e09da30bda2 100 r5nav.addGraphNode(15, 5, 17, 0);
j_j205 35:4e09da30bda2 101 r5nav.addGraphNode(16, 8, 6, 0);
j_j205 35:4e09da30bda2 102 r5nav.addGraphNode(16, 9, 23, 180);
j_j205 35:4e09da30bda2 103 r5nav.addGraphNode(17, 8, 6, 180);
j_j205 35:4e09da30bda2 104 r5nav.addGraphNode(17, 18, 25, 0);
j_j205 35:4e09da30bda2 105 r5nav.addGraphNode(18, 10, 6, 0);
j_j205 35:4e09da30bda2 106 r5nav.addGraphNode(18, 17, 25, 180);
j_j205 35:4e09da30bda2 107 r5nav.addGraphNode(19, 11, 6, 0);
j_j205 35:4e09da30bda2 108 r5nav.addGraphNode(19, 12, 23, 180);
j_j205 35:4e09da30bda2 109 r5nav.addGraphNode(20, 14, 21, 270);
j_j205 35:4e09da30bda2 110 r5nav.addGraphNode(20, 21, 24, 90);
j_j205 35:4e09da30bda2 111 r5nav.addGraphNode(21, 20, 24, 270);
j_j205 35:4e09da30bda2 112 r5nav.addGraphNode(21, 22, 4, 180);
j_j205 35:4e09da30bda2 113 r5nav.addGraphNode(22, 21, 4, 0);
j_j205 35:4e09da30bda2 114 r5nav.addGraphNode(22, 23, 80, 180);
j_j205 35:4e09da30bda2 115 r5nav.addGraphNode(23, 22, 80, 0);
j_j205 35:4e09da30bda2 116 r5nav.addGraphNode(23, 24, 2, 270);
j_j205 35:4e09da30bda2 117 r5nav.addGraphNode(24, 23, 2, 90);
j_j205 32:82bfece56f8f 118
j_j205 35:4e09da30bda2 119 const uint16_t V1 = 7;
j_j205 35:4e09da30bda2 120 const uint16_t V2 = 9;
j_j205 35:4e09da30bda2 121 const uint16_t V3 = 12;
j_j205 35:4e09da30bda2 122 const uint16_t V4 = 20;
j_j205 35:4e09da30bda2 123 const uint16_t V5 = 22;
j_j205 35:4e09da30bda2 124 const uint16_t V6 = 24;
j_j205 8:90af8914ce03 125 const uint16_t YELLOW_DROP_ZONE = 4;
j_j205 8:90af8914ce03 126 const uint16_t RED_DROP_ZONE = 2;
j_j205 32:82bfece56f8f 127
j_j205 29:e77e8891d985 128 /*
j_j205 15:d8940756d5d5 129 r5nav.getShortestPath(1);
j_j205 15:d8940756d5d5 130 pc.printf("\n\nDistance from 0 to 1: %i\n", r5nav.getMinDist(1) );
j_j205 15:d8940756d5d5 131 pc.printf("Route:\n");
j_j205 15:d8940756d5d5 132 r5nav.executeRoute(pc, drive);
j_j205 15:d8940756d5d5 133 wait(0.1);
j_j205 20:53ab414d7b33 134
j_j205 15:d8940756d5d5 135 r5nav.getShortestPath(2);
j_j205 15:d8940756d5d5 136 pc.printf("\n\nDistance from 1 to 2: %i\n", r5nav.getMinDist(2) );
j_j205 15:d8940756d5d5 137 pc.printf("Route:\n");
j_j205 15:d8940756d5d5 138 r5nav.executeRoute(pc, drive);
j_j205 15:d8940756d5d5 139 wait(0.1);
j_j205 20:53ab414d7b33 140
j_j205 15:d8940756d5d5 141 r5nav.getShortestPath(3);
j_j205 15:d8940756d5d5 142 pc.printf("\n\nDistance from 2 to 3: %i\n", r5nav.getMinDist(3) );
j_j205 8:90af8914ce03 143 pc.printf("Route:\n");
j_j205 15:d8940756d5d5 144 r5nav.executeRoute(pc, drive);
j_j205 15:d8940756d5d5 145 wait(0.1);
j_j205 20:53ab414d7b33 146
j_j205 15:d8940756d5d5 147 r5nav.getShortestPath(0);
j_j205 15:d8940756d5d5 148 pc.printf("\n\nDistance from 3 to 0: %i\n", r5nav.getMinDist(0) );
j_j205 15:d8940756d5d5 149 pc.printf("Route:\n");
j_j205 15:d8940756d5d5 150 r5nav.executeRoute(pc, drive);
j_j205 15:d8940756d5d5 151 wait(0.1);
j_j205 29:e77e8891d985 152 */
j_j205 32:82bfece56f8f 153
j_j205 35:4e09da30bda2 154 /* Begin 1st peg retrieval */
j_j205 35:4e09da30bda2 155
j_j205 35:4e09da30bda2 156 r5nav.getShortestPath(1);
j_j205 35:4e09da30bda2 157 pc.printf("\n\nDistance from 0 to 1: %i\n", r5nav.getMinDist(1) );
j_j205 15:d8940756d5d5 158 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 159 scanner.setLocalizeRightFlag(1);
j_j205 35:4e09da30bda2 160 scanner.setLocalizeLeftFlag(0);
j_j205 12:e9f878ced6e7 161 r5nav.executeRoute(pc, drive);
j_j205 8:90af8914ce03 162 wait(0.1);
j_j205 1:1958e6db41fc 163
j_j205 35:4e09da30bda2 164 r5nav.getShortestPath(3);
j_j205 35:4e09da30bda2 165 pc.printf("\n\nDistance from 1 to 3: %i\n", r5nav.getMinDist(3) );
j_j205 35:4e09da30bda2 166 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 167 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 168 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 169 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 170 wait(0.1);
j_j205 35:4e09da30bda2 171
j_j205 35:4e09da30bda2 172 r5nav.getShortestPath(15);
j_j205 35:4e09da30bda2 173 pc.printf("\n\nDistance from 3 to 15: %i\n", r5nav.getMinDist(15) );
j_j205 35:4e09da30bda2 174 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 175 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 176 scanner.setLocalizeLeftFlag(1);
j_j205 35:4e09da30bda2 177 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 178 wait(0.1);
j_j205 35:4e09da30bda2 179
j_j205 35:4e09da30bda2 180 r5nav.getShortestPath(V1);
j_j205 35:4e09da30bda2 181 pc.printf("\n\nDistance from 15 to V1: %i\n", r5nav.getMinDist(V1) );
j_j205 35:4e09da30bda2 182 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 183 scanner.setLocalizeRightFlag(1);
j_j205 35:4e09da30bda2 184 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 185 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 186 wait(0.1);
j_j205 35:4e09da30bda2 187
j_j205 35:4e09da30bda2 188 /* add hunt method call */
j_j205 35:4e09da30bda2 189
j_j205 35:4e09da30bda2 190 r5nav.getShortestPath(15);
j_j205 35:4e09da30bda2 191 pc.printf("\n\nDistance from V1 to 15: %i\n", r5nav.getMinDist(15) );
j_j205 35:4e09da30bda2 192 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 193 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 194 scanner.setLocalizeLeftFlag(1);
j_j205 35:4e09da30bda2 195 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 196 wait(0.1);
j_j205 35:4e09da30bda2 197
j_j205 35:4e09da30bda2 198 r5nav.getShortestPath(3);
j_j205 35:4e09da30bda2 199 pc.printf("\n\nDistance from 15 to 3: %i\n", r5nav.getMinDist(3) );
j_j205 8:90af8914ce03 200 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 201 scanner.setLocalizeRightFlag(1);
j_j205 35:4e09da30bda2 202 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 203 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 204 wait(0.1);
j_j205 35:4e09da30bda2 205
j_j205 35:4e09da30bda2 206 if (scanner.getYellowFlag() == 1)
j_j205 35:4e09da30bda2 207 {
j_j205 35:4e09da30bda2 208 r5nav.getShortestPath(YELLOW_DROP_ZONE);
j_j205 35:4e09da30bda2 209 pc.printf("\n\nDistance from %i to YELLOW_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(YELLOW_DROP_ZONE) );
j_j205 35:4e09da30bda2 210 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 211 //r5nav.printRoute(pc);
j_j205 35:4e09da30bda2 212 scanner.setLocalizeRightFlag(1);
j_j205 35:4e09da30bda2 213 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 214 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 215 wait(0.1);
j_j205 35:4e09da30bda2 216 }
j_j205 35:4e09da30bda2 217
j_j205 35:4e09da30bda2 218 else
j_j205 35:4e09da30bda2 219 {
j_j205 35:4e09da30bda2 220 r5nav.getShortestPath(1);
j_j205 35:4e09da30bda2 221 pc.printf("\n\nDistance from 3 to 1: %i\n", r5nav.getMinDist(1) );
j_j205 35:4e09da30bda2 222 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 223 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 224 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 225 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 226 wait(0.1);
j_j205 35:4e09da30bda2 227
j_j205 35:4e09da30bda2 228 r5nav.getShortestPath(RED_DROP_ZONE);
j_j205 35:4e09da30bda2 229 pc.printf("\n\nDistance from %i to RED_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(RED_DROP_ZONE) );
j_j205 35:4e09da30bda2 230 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 231 //r5nav.printRoute(pc);
j_j205 35:4e09da30bda2 232 scanner.setLocalizeRightFlag(1);
j_j205 35:4e09da30bda2 233 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 234 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 235 wait(0.1);
j_j205 35:4e09da30bda2 236 }
j_j205 35:4e09da30bda2 237
j_j205 35:4e09da30bda2 238 /* 1st peg retrieval complete
j_j205 35:4e09da30bda2 239 Begin 2nd peg retrieval */
j_j205 35:4e09da30bda2 240
j_j205 35:4e09da30bda2 241 if (r5nav.getVertex() == YELLOW_DROP_ZONE)
j_j205 35:4e09da30bda2 242 {
j_j205 35:4e09da30bda2 243 r5nav.getShortestPath(3);
j_j205 35:4e09da30bda2 244 pc.printf("\n\nDistance from YELLOW_DROP_ZONE to 3: %i\n", r5nav.getMinDist(3) );
j_j205 35:4e09da30bda2 245 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 246 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 247 scanner.setLocalizeLeftFlag(1);
j_j205 35:4e09da30bda2 248 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 249 wait(0.1);
j_j205 35:4e09da30bda2 250 }
j_j205 35:4e09da30bda2 251
j_j205 35:4e09da30bda2 252 else
j_j205 35:4e09da30bda2 253 {
j_j205 35:4e09da30bda2 254 r5nav.getShortestPath(1);
j_j205 35:4e09da30bda2 255 pc.printf("\n\nDistance from RED_DROP_ZONE to 1: %i\n", r5nav.getMinDist(1) );
j_j205 35:4e09da30bda2 256 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 257 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 258 scanner.setLocalizeLeftFlag(1);
j_j205 35:4e09da30bda2 259 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 260 wait(0.1);
j_j205 35:4e09da30bda2 261
j_j205 35:4e09da30bda2 262 r5nav.getShortestPath(3);
j_j205 35:4e09da30bda2 263 pc.printf("\n\nDistance from 1 to 3: %i\n", r5nav.getMinDist(3) );
j_j205 35:4e09da30bda2 264 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 265 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 266 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 267 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 268 wait(0.1);
j_j205 35:4e09da30bda2 269 }
j_j205 35:4e09da30bda2 270
j_j205 35:4e09da30bda2 271 r5nav.getShortestPath(15);
j_j205 35:4e09da30bda2 272 pc.printf("\n\nDistance from 3 to 15: %i\n", r5nav.getMinDist(15) );
j_j205 35:4e09da30bda2 273 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 274 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 275 scanner.setLocalizeLeftFlag(1);
j_j205 35:4e09da30bda2 276 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 277 wait(0.1);
j_j205 35:4e09da30bda2 278
j_j205 35:4e09da30bda2 279 r5nav.getShortestPath(5);
j_j205 35:4e09da30bda2 280 pc.printf("\n\nDistance from 15 to 5: %i\n", r5nav.getMinDist(5) );
j_j205 35:4e09da30bda2 281 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 282 scanner.setLocalizeRightFlag(1);
j_j205 35:4e09da30bda2 283 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 284 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 285 wait(0.1);
j_j205 35:4e09da30bda2 286
j_j205 35:4e09da30bda2 287 r5nav.getShortestPath(16);
j_j205 35:4e09da30bda2 288 pc.printf("\n\nDistance from 5 to 16: %i\n", r5nav.getMinDist(16) );
j_j205 35:4e09da30bda2 289 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 290 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 291 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 292 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 293 wait(0.1);
j_j205 35:4e09da30bda2 294
j_j205 35:4e09da30bda2 295 r5nav.getShortestPath(V2);
j_j205 35:4e09da30bda2 296 pc.printf("\n\nDistance from 16 to V2: %i\n", r5nav.getMinDist(V2) );
j_j205 35:4e09da30bda2 297 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 298 scanner.setLocalizeRightFlag(1);
j_j205 35:4e09da30bda2 299 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 300 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 301 wait(0.1);
j_j205 35:4e09da30bda2 302
j_j205 35:4e09da30bda2 303 /* add hunt method call */
j_j205 35:4e09da30bda2 304
j_j205 35:4e09da30bda2 305 r5nav.getShortestPath(16);
j_j205 35:4e09da30bda2 306 pc.printf("\n\nDistance from V2 to 16: %i\n", r5nav.getMinDist(16) );
j_j205 35:4e09da30bda2 307 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 308 scanner.setLocalizeRightFlag(1);
j_j205 35:4e09da30bda2 309 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 310 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 311 wait(0.1);
j_j205 35:4e09da30bda2 312
j_j205 35:4e09da30bda2 313 r5nav.getShortestPath(5);
j_j205 35:4e09da30bda2 314 pc.printf("\n\nDistance from 16 to 5: %i\n", r5nav.getMinDist(5) );
j_j205 35:4e09da30bda2 315 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 316 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 317 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 318 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 319 wait(0.1);
j_j205 35:4e09da30bda2 320
j_j205 35:4e09da30bda2 321 r5nav.getShortestPath(15);
j_j205 35:4e09da30bda2 322 pc.printf("\n\nDistance from 5 to 15: %i\n", r5nav.getMinDist(15) );
j_j205 35:4e09da30bda2 323 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 324 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 325 scanner.setLocalizeLeftFlag(1);
j_j205 35:4e09da30bda2 326 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 327 wait(0.1);
j_j205 35:4e09da30bda2 328
j_j205 35:4e09da30bda2 329 r5nav.getShortestPath(3);
j_j205 35:4e09da30bda2 330 pc.printf("\n\nDistance from 15 to 3: %i\n", r5nav.getMinDist(3) );
j_j205 35:4e09da30bda2 331 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 332 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 333 scanner.setLocalizeLeftFlag(1);
j_j205 12:e9f878ced6e7 334 r5nav.executeRoute(pc, drive);
j_j205 8:90af8914ce03 335 wait(0.1);
j_j205 35:4e09da30bda2 336
j_j205 35:4e09da30bda2 337 if (scanner.getYellowFlag() == 1)
j_j205 35:4e09da30bda2 338 {
j_j205 35:4e09da30bda2 339 r5nav.getShortestPath(YELLOW_DROP_ZONE);
j_j205 35:4e09da30bda2 340 pc.printf("\n\nDistance from %i to YELLOW_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(YELLOW_DROP_ZONE) );
j_j205 35:4e09da30bda2 341 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 342 //r5nav.printRoute(pc);
j_j205 35:4e09da30bda2 343 scanner.setLocalizeRightFlag(1);
j_j205 35:4e09da30bda2 344 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 345 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 346 wait(0.1);
j_j205 35:4e09da30bda2 347 }
j_j205 35:4e09da30bda2 348
j_j205 35:4e09da30bda2 349 else
j_j205 35:4e09da30bda2 350 {
j_j205 35:4e09da30bda2 351 r5nav.getShortestPath(1);
j_j205 35:4e09da30bda2 352 pc.printf("\n\nDistance from 3 to 1: %i\n", r5nav.getMinDist(1) );
j_j205 35:4e09da30bda2 353 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 354 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 355 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 356 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 357 wait(0.1);
j_j205 35:4e09da30bda2 358
j_j205 35:4e09da30bda2 359 r5nav.getShortestPath(RED_DROP_ZONE);
j_j205 35:4e09da30bda2 360 pc.printf("\n\nDistance from %i to RED_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(RED_DROP_ZONE) );
j_j205 35:4e09da30bda2 361 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 362 //r5nav.printRoute(pc);
j_j205 35:4e09da30bda2 363 scanner.setLocalizeRightFlag(1);
j_j205 35:4e09da30bda2 364 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 365 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 366 wait(0.1);
j_j205 35:4e09da30bda2 367 }
j_j205 35:4e09da30bda2 368
j_j205 35:4e09da30bda2 369 /* 2nd peg retrieval complete
j_j205 35:4e09da30bda2 370 Begin 3nd peg retrieval */
j_j205 35:4e09da30bda2 371
j_j205 35:4e09da30bda2 372
j_j205 35:4e09da30bda2 373 if (r5nav.getVertex() == YELLOW_DROP_ZONE)
j_j205 35:4e09da30bda2 374 {
j_j205 35:4e09da30bda2 375 r5nav.getShortestPath(3);
j_j205 35:4e09da30bda2 376 pc.printf("\n\nDistance from YELLOW_DROP_ZONE to 3: %i\n", r5nav.getMinDist(3) );
j_j205 35:4e09da30bda2 377 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 378 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 379 scanner.setLocalizeLeftFlag(1);
j_j205 35:4e09da30bda2 380 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 381 wait(0.1);
j_j205 35:4e09da30bda2 382 }
j_j205 1:1958e6db41fc 383
j_j205 35:4e09da30bda2 384 else
j_j205 35:4e09da30bda2 385 {
j_j205 35:4e09da30bda2 386 r5nav.getShortestPath(1);
j_j205 35:4e09da30bda2 387 pc.printf("\n\nDistance from RED_DROP_ZONE to 1: %i\n", r5nav.getMinDist(1) );
j_j205 35:4e09da30bda2 388 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 389 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 390 scanner.setLocalizeLeftFlag(1);
j_j205 35:4e09da30bda2 391 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 392 wait(0.1);
j_j205 35:4e09da30bda2 393
j_j205 35:4e09da30bda2 394 r5nav.getShortestPath(3);
j_j205 35:4e09da30bda2 395 pc.printf("\n\nDistance from 1 to 3: %i\n", r5nav.getMinDist(3) );
j_j205 35:4e09da30bda2 396 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 397 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 398 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 399 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 400 wait(0.1);
j_j205 35:4e09da30bda2 401 }
j_j205 35:4e09da30bda2 402
j_j205 35:4e09da30bda2 403 r5nav.getShortestPath(15);
j_j205 35:4e09da30bda2 404 pc.printf("\n\nDistance from 3 to 15: %i\n", r5nav.getMinDist(15) );
j_j205 8:90af8914ce03 405 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 406 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 407 scanner.setLocalizeLeftFlag(1);
j_j205 35:4e09da30bda2 408 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 409 wait(0.1);
j_j205 35:4e09da30bda2 410
j_j205 35:4e09da30bda2 411 r5nav.getShortestPath(5);
j_j205 35:4e09da30bda2 412 pc.printf("\n\nDistance from 15 to 5: %i\n", r5nav.getMinDist(5) );
j_j205 35:4e09da30bda2 413 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 414 scanner.setLocalizeRightFlag(1);
j_j205 35:4e09da30bda2 415 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 416 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 417 wait(0.1);
j_j205 35:4e09da30bda2 418
j_j205 35:4e09da30bda2 419 r5nav.getShortestPath(19);
j_j205 35:4e09da30bda2 420 pc.printf("\n\nDistance from 5 to 19: %i\n", r5nav.getMinDist(19) );
j_j205 35:4e09da30bda2 421 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 422 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 423 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 424 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 425 wait(0.1);
j_j205 35:4e09da30bda2 426
j_j205 35:4e09da30bda2 427 r5nav.getShortestPath(V3);
j_j205 35:4e09da30bda2 428 pc.printf("\n\nDistance from 19 to V3: %i\n", r5nav.getMinDist(V3) );
j_j205 35:4e09da30bda2 429 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 430 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 431 scanner.setLocalizeLeftFlag(1);
j_j205 12:e9f878ced6e7 432 r5nav.executeRoute(pc, drive);
j_j205 8:90af8914ce03 433 wait(0.1);
j_j205 35:4e09da30bda2 434
j_j205 35:4e09da30bda2 435 /* add hunt method call */
j_j205 35:4e09da30bda2 436
j_j205 38:ebf73b654553 437 r5nav.getShortestPath(19);
j_j205 38:ebf73b654553 438 pc.printf("\n\nDistance from V3 to 19: %i\n", r5nav.getMinDist(19) );
j_j205 38:ebf73b654553 439 pc.printf("Route:\n");
j_j205 38:ebf73b654553 440 scanner.setLocalizeRightFlag(1);
j_j205 38:ebf73b654553 441 scanner.setLocalizeLeftFlag(0);
j_j205 38:ebf73b654553 442 r5nav.executeRoute(pc, drive);
j_j205 38:ebf73b654553 443 wait(0.1);
j_j205 38:ebf73b654553 444
j_j205 38:ebf73b654553 445 r5nav.getShortestPath(5);
j_j205 38:ebf73b654553 446 pc.printf("\n\nDistance from 19 to 5: %i\n", r5nav.getMinDist(5) );
j_j205 38:ebf73b654553 447 pc.printf("Route:\n");
j_j205 38:ebf73b654553 448 scanner.setLocalizeRightFlag(0);
j_j205 38:ebf73b654553 449 scanner.setLocalizeLeftFlag(0);
j_j205 38:ebf73b654553 450 r5nav.executeRoute(pc, drive);
j_j205 38:ebf73b654553 451 wait(0.1);
j_j205 38:ebf73b654553 452 // at node 5
j_j205 35:4e09da30bda2 453
j_j205 38:ebf73b654553 454 if (scanner.getObjectFound() == 1)
j_j205 35:4e09da30bda2 455 {
j_j205 38:ebf73b654553 456 v3Flag = 1;
j_j205 38:ebf73b654553 457
j_j205 38:ebf73b654553 458 r5nav.getShortestPath(15);
j_j205 38:ebf73b654553 459 pc.printf("\n\nDistance from 5 to 15: %i\n", r5nav.getMinDist(15) );
j_j205 35:4e09da30bda2 460 pc.printf("Route:\n");
j_j205 38:ebf73b654553 461 scanner.setLocalizeRightFlag(0);
j_j205 38:ebf73b654553 462 scanner.setLocalizeLeftFlag(1);
j_j205 38:ebf73b654553 463 r5nav.executeRoute(pc, drive);
j_j205 38:ebf73b654553 464 wait(0.1);
j_j205 38:ebf73b654553 465
j_j205 38:ebf73b654553 466 r5nav.getShortestPath(3);
j_j205 38:ebf73b654553 467 pc.printf("\n\nDistance from 15 to 3: %i\n", r5nav.getMinDist(3) );
j_j205 38:ebf73b654553 468 pc.printf("Route:\n");
j_j205 38:ebf73b654553 469 scanner.setLocalizeRightFlag(0);
j_j205 38:ebf73b654553 470 scanner.setLocalizeLeftFlag(1);
j_j205 35:4e09da30bda2 471 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 472 wait(0.1);
j_j205 38:ebf73b654553 473
j_j205 38:ebf73b654553 474 if (scanner.getYellowFlag() == 1)
j_j205 38:ebf73b654553 475 {
j_j205 38:ebf73b654553 476 r5nav.getShortestPath(YELLOW_DROP_ZONE);
j_j205 38:ebf73b654553 477 pc.printf("\n\nDistance from %i to YELLOW_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(YELLOW_DROP_ZONE) );
j_j205 38:ebf73b654553 478 pc.printf("Route:\n");
j_j205 38:ebf73b654553 479 //r5nav.printRoute(pc);
j_j205 38:ebf73b654553 480 scanner.setLocalizeRightFlag(1);
j_j205 38:ebf73b654553 481 scanner.setLocalizeLeftFlag(0);
j_j205 38:ebf73b654553 482 r5nav.executeRoute(pc, drive);
j_j205 38:ebf73b654553 483 wait(0.1);
j_j205 38:ebf73b654553 484
j_j205 38:ebf73b654553 485 /* add drop peg code */
j_j205 38:ebf73b654553 486
j_j205 38:ebf73b654553 487 r5nav.getShortestPath(15)
j_j205 38:ebf73b654553 488 pc.printf("\n\nDistance from %i to 15%i\n", r5nav.getVertex(), r5nav.getMinDist(15);
j_j205 38:ebf73b654553 489 pc.printf("Route:\n");
j_j205 38:ebf73b654553 490 //r5nav.printRoute(pc);
j_j205 38:ebf73b654553 491 scanner.setLocalizeLeftFlag(1);
j_j205 38:ebf73b654553 492 scanner.setLocalizeRightFlag(0);
j_j205 38:ebf73b654553 493 r5nav.executeRoute(pc, drive);
j_j205 38:ebf73b654553 494 wait(0.1);
j_j205 38:ebf73b654553 495
j_j205 38:ebf73b654553 496 r5nav.getShortestPath(5);
j_j205 38:ebf73b654553 497 pc.printf("\n\nDistance from %i to 5: %i\n", r5nav.getVertex(), r5nav.getMinDist(5) );
j_j205 38:ebf73b654553 498 pc.printf("Route:\n");
j_j205 38:ebf73b654553 499 //r5nav.printRoute(pc);
j_j205 38:ebf73b654553 500 scanner.setLocalizeRightFlag(1);
j_j205 38:ebf73b654553 501 scanner.setLocalizeLeftFlag(0);
j_j205 38:ebf73b654553 502 r5nav.executeRoute(pc, drive);
j_j205 38:ebf73b654553 503 wait(0.1);
j_j205 38:ebf73b654553 504 }
j_j205 38:ebf73b654553 505
j_j205 38:ebf73b654553 506 else
j_j205 38:ebf73b654553 507 {
j_j205 38:ebf73b654553 508 r5nav.getShortestPath(1);
j_j205 38:ebf73b654553 509 pc.printf("\n\nDistance from 3 to 1: %i\n", r5nav.getMinDist(1) );
j_j205 38:ebf73b654553 510 pc.printf("Route:\n");
j_j205 38:ebf73b654553 511 scanner.setLocalizeRightFlag(0);
j_j205 38:ebf73b654553 512 scanner.setLocalizeLeftFlag(0);
j_j205 38:ebf73b654553 513 r5nav.executeRoute(pc, drive);
j_j205 38:ebf73b654553 514 wait(0.1);
j_j205 38:ebf73b654553 515
j_j205 38:ebf73b654553 516 r5nav.getShortestPath(RED_DROP_ZONE);
j_j205 38:ebf73b654553 517 pc.printf("\n\nDistance from %i to RED_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(RED_DROP_ZONE) );
j_j205 38:ebf73b654553 518 pc.printf("Route:\n");
j_j205 38:ebf73b654553 519 //r5nav.printRoute(pc);
j_j205 38:ebf73b654553 520 scanner.setLocalizeRightFlag(1);
j_j205 38:ebf73b654553 521 scanner.setLocalizeLeftFlag(0);
j_j205 38:ebf73b654553 522 r5nav.executeRoute(pc, drive);
j_j205 38:ebf73b654553 523 wait(0.1);
j_j205 38:ebf73b654553 524
j_j205 38:ebf73b654553 525 r5nav.getShortestPath(1);
j_j205 38:ebf73b654553 526 pc.printf("\n\nDistance from %i to 1: %i\n", r5nav.getVertex(), r5nav.getMinDist(1) );
j_j205 38:ebf73b654553 527 pc.printf("Route:\n");
j_j205 38:ebf73b654553 528 //r5nav.printRoute(pc);
j_j205 38:ebf73b654553 529 scanner.setLocalizeRightFlag(0);
j_j205 38:ebf73b654553 530 scanner.setLocalizeLeftFlag(1);
j_j205 38:ebf73b654553 531 r5nav.executeRoute(pc, drive);
j_j205 38:ebf73b654553 532 wait(0.1);
j_j205 38:ebf73b654553 533
j_j205 38:ebf73b654553 534 r5nav.getShortestPath(3);
j_j205 38:ebf73b654553 535 pc.printf("\n\nDistance from %i to 3: %i\n", r5nav.getVertex(), r5nav.getMinDist(3) );
j_j205 38:ebf73b654553 536 pc.printf("Route:\n");
j_j205 38:ebf73b654553 537 //r5nav.printRoute(pc);
j_j205 38:ebf73b654553 538 scanner.setLocalizeRightFlag(0);
j_j205 38:ebf73b654553 539 scanner.setLocalizeLeftFlag(0);
j_j205 38:ebf73b654553 540 r5nav.executeRoute(pc, drive);
j_j205 38:ebf73b654553 541 wait(0.1);
j_j205 38:ebf73b654553 542
j_j205 38:ebf73b654553 543 r5nav.getShortestPath(15);
j_j205 38:ebf73b654553 544 pc.printf("\n\nDistance from %i to 15: %i\n", r5nav.getVertex(), r5nav.getMinDist(15) );
j_j205 38:ebf73b654553 545 pc.printf("Route:\n");
j_j205 38:ebf73b654553 546 //r5nav.printRoute(pc);
j_j205 38:ebf73b654553 547 scanner.setLocalizeRightFlag(0);
j_j205 38:ebf73b654553 548 scanner.setLocalizeLeftFlag(1);
j_j205 38:ebf73b654553 549 r5nav.executeRoute(pc, drive);
j_j205 38:ebf73b654553 550 wait(0.1);
j_j205 38:ebf73b654553 551
j_j205 38:ebf73b654553 552 r5nav.getShortestPath(5);
j_j205 38:ebf73b654553 553 pc.printf("\n\nDistance from %i to 5: %i\n", r5nav.getVertex(), r5nav.getMinDist(5) );
j_j205 38:ebf73b654553 554 pc.printf("Route:\n");
j_j205 38:ebf73b654553 555 //r5nav.printRoute(pc);
j_j205 38:ebf73b654553 556 scanner.setLocalizeRightFlag(1);
j_j205 38:ebf73b654553 557 scanner.setLocalizeLeftFlag(0);
j_j205 38:ebf73b654553 558 r5nav.executeRoute(pc, drive);
j_j205 38:ebf73b654553 559 wait(0.1);
j_j205 38:ebf73b654553 560
j_j205 38:ebf73b654553 561 /* 3rd peg retrieval complete
j_j205 38:ebf73b654553 562 Begin 4th peg retrieval */
j_j205 38:ebf73b654553 563 }
j_j205 35:4e09da30bda2 564 }
j_j205 38:ebf73b654553 565
j_j205 38:ebf73b654553 566 // peg V3 not found and at node 5
j_j205 38:ebf73b654553 567
j_j205 38:ebf73b654553 568 r5nav.getShortestPath(6);
j_j205 38:ebf73b654553 569 pc.printf("\n\nDistance from %i to 6: %i\n", r5nav.getVertex(), r5nav.getMinDist(6) );
j_j205 38:ebf73b654553 570 pc.printf("Route:\n");
j_j205 38:ebf73b654553 571 //r5nav.printRoute(pc);
j_j205 38:ebf73b654553 572 scanner.setLocalizeRightFlag(1);
j_j205 38:ebf73b654553 573 scanner.setLocalizeLeftFlag(0);
j_j205 38:ebf73b654553 574 r5nav.executeRoute(pc, drive);
j_j205 38:ebf73b654553 575 wait(0.1);
j_j205 38:ebf73b654553 576
j_j205 38:ebf73b654553 577 r5nav.getShortestPath(14);
j_j205 38:ebf73b654553 578 pc.printf("\n\nDistance from %i to 14: %i\n", r5nav.getVertex(), r5nav.getMinDist(14) );
j_j205 38:ebf73b654553 579 pc.printf("Route:\n");
j_j205 38:ebf73b654553 580 //r5nav.printRoute(pc);
j_j205 38:ebf73b654553 581 scanner.setLocalizeRightFlag(0);
j_j205 38:ebf73b654553 582 scanner.setLocalizeLeftFlag(0);
j_j205 38:ebf73b654553 583 r5nav.executeRoute(pc, drive);
j_j205 38:ebf73b654553 584 wait(0.1);
j_j205 38:ebf73b654553 585
j_j205 38:ebf73b654553 586 r5nav.getShortestPath(20);
j_j205 38:ebf73b654553 587 pc.printf("\n\nDistance from %i to 20: %i\n", r5nav.getVertex(), r5nav.getMinDist(20) );
j_j205 38:ebf73b654553 588 pc.printf("Route:\n");
j_j205 38:ebf73b654553 589 //r5nav.printRoute(pc);
j_j205 38:ebf73b654553 590 scanner.setLocalizeRightFlag(1);
j_j205 38:ebf73b654553 591 scanner.setLocalizeLeftFlag(0);
j_j205 38:ebf73b654553 592 r5nav.executeRoute(pc, drive);
j_j205 38:ebf73b654553 593 wait(0.1);
j_j205 38:ebf73b654553 594
j_j205 38:ebf73b654553 595 /* add hunt code */
j_j205 38:ebf73b654553 596
j_j205 38:ebf73b654553 597 if (scanner.getObjectFound() == 1)
j_j205 38:ebf73b654553 598 {
j_j205 38:ebf73b654553 599 r5nav.getShortestPath(15);
j_j205 38:ebf73b654553 600 pc.printf("\n\nDistance from 5 to 15: %i\n", r5nav.getMinDist(15) );
j_j205 35:4e09da30bda2 601 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 602 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 603 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 604 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 605 wait(0.1);
j_j205 38:ebf73b654553 606 // may need to break up above route
j_j205 35:4e09da30bda2 607
j_j205 38:ebf73b654553 608 r5nav.getShortestPath(3);
j_j205 38:ebf73b654553 609 pc.printf("\n\nDistance from 15 to 3: %i\n", r5nav.getMinDist(3) );
j_j205 35:4e09da30bda2 610 pc.printf("Route:\n");
j_j205 38:ebf73b654553 611 scanner.setLocalizeRightFlag(0);
j_j205 38:ebf73b654553 612 scanner.setLocalizeLeftFlag(1);
j_j205 35:4e09da30bda2 613 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 614 wait(0.1);
j_j205 38:ebf73b654553 615
j_j205 38:ebf73b654553 616 if (scanner.getYellowFlag() == 1)
j_j205 38:ebf73b654553 617 {
j_j205 38:ebf73b654553 618 r5nav.getShortestPath(YELLOW_DROP_ZONE);
j_j205 38:ebf73b654553 619 pc.printf("\n\nDistance from %i to YELLOW_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(YELLOW_DROP_ZONE) );
j_j205 38:ebf73b654553 620 pc.printf("Route:\n");
j_j205 38:ebf73b654553 621 //r5nav.printRoute(pc);
j_j205 38:ebf73b654553 622 scanner.setLocalizeRightFlag(1);
j_j205 38:ebf73b654553 623 scanner.setLocalizeLeftFlag(0);
j_j205 38:ebf73b654553 624 r5nav.executeRoute(pc, drive);
j_j205 38:ebf73b654553 625 wait(0.1);
j_j205 38:ebf73b654553 626
j_j205 38:ebf73b654553 627 /* add drop peg code */
j_j205 38:ebf73b654553 628
j_j205 38:ebf73b654553 629 r5nav.getShortestPath(15)
j_j205 38:ebf73b654553 630 pc.printf("\n\nDistance from %i to 15%i\n", r5nav.getVertex(), r5nav.getMinDist(15);
j_j205 38:ebf73b654553 631 pc.printf("Route:\n");
j_j205 38:ebf73b654553 632 //r5nav.printRoute(pc);
j_j205 38:ebf73b654553 633 scanner.setLocalizeLeftFlag(1);
j_j205 38:ebf73b654553 634 scanner.setLocalizeRightFlag(0);
j_j205 38:ebf73b654553 635 r5nav.executeRoute(pc, drive);
j_j205 38:ebf73b654553 636 wait(0.1);
j_j205 38:ebf73b654553 637
j_j205 38:ebf73b654553 638 r5nav.getShortestPath(5);
j_j205 38:ebf73b654553 639 pc.printf("\n\nDistance from %i to 5: %i\n", r5nav.getVertex(), r5nav.getMinDist(5) );
j_j205 38:ebf73b654553 640 pc.printf("Route:\n");
j_j205 38:ebf73b654553 641 //r5nav.printRoute(pc);
j_j205 38:ebf73b654553 642 scanner.setLocalizeRightFlag(1);
j_j205 38:ebf73b654553 643 scanner.setLocalizeLeftFlag(0);
j_j205 38:ebf73b654553 644 r5nav.executeRoute(pc, drive);
j_j205 38:ebf73b654553 645 wait(0.1);
j_j205 38:ebf73b654553 646
j_j205 38:ebf73b654553 647 /* may need to get back to 20 */
j_j205 38:ebf73b654553 648 }
j_j205 38:ebf73b654553 649
j_j205 38:ebf73b654553 650 else
j_j205 38:ebf73b654553 651 {
j_j205 38:ebf73b654553 652 r5nav.getShortestPath(1);
j_j205 38:ebf73b654553 653 pc.printf("\n\nDistance from 3 to 1: %i\n", r5nav.getMinDist(1) );
j_j205 38:ebf73b654553 654 pc.printf("Route:\n");
j_j205 38:ebf73b654553 655 scanner.setLocalizeRightFlag(0);
j_j205 38:ebf73b654553 656 scanner.setLocalizeLeftFlag(0);
j_j205 38:ebf73b654553 657 r5nav.executeRoute(pc, drive);
j_j205 38:ebf73b654553 658 wait(0.1);
j_j205 38:ebf73b654553 659
j_j205 38:ebf73b654553 660 r5nav.getShortestPath(RED_DROP_ZONE);
j_j205 38:ebf73b654553 661 pc.printf("\n\nDistance from %i to RED_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(RED_DROP_ZONE) );
j_j205 38:ebf73b654553 662 pc.printf("Route:\n");
j_j205 38:ebf73b654553 663 //r5nav.printRoute(pc);
j_j205 38:ebf73b654553 664 scanner.setLocalizeRightFlag(1);
j_j205 38:ebf73b654553 665 scanner.setLocalizeLeftFlag(0);
j_j205 38:ebf73b654553 666 r5nav.executeRoute(pc, drive);
j_j205 38:ebf73b654553 667 wait(0.1);
j_j205 38:ebf73b654553 668
j_j205 38:ebf73b654553 669 r5nav.getShortestPath(1);
j_j205 38:ebf73b654553 670 pc.printf("\n\nDistance from %i to 1: %i\n", r5nav.getVertex(), r5nav.getMinDist(1) );
j_j205 38:ebf73b654553 671 pc.printf("Route:\n");
j_j205 38:ebf73b654553 672 //r5nav.printRoute(pc);
j_j205 38:ebf73b654553 673 scanner.setLocalizeRightFlag(0);
j_j205 38:ebf73b654553 674 scanner.setLocalizeLeftFlag(1);
j_j205 38:ebf73b654553 675 r5nav.executeRoute(pc, drive);
j_j205 38:ebf73b654553 676 wait(0.1);
j_j205 38:ebf73b654553 677
j_j205 38:ebf73b654553 678 r5nav.getShortestPath(3);
j_j205 38:ebf73b654553 679 pc.printf("\n\nDistance from %i to 3: %i\n", r5nav.getVertex(), r5nav.getMinDist(3) );
j_j205 38:ebf73b654553 680 pc.printf("Route:\n");
j_j205 38:ebf73b654553 681 //r5nav.printRoute(pc);
j_j205 38:ebf73b654553 682 scanner.setLocalizeRightFlag(0);
j_j205 38:ebf73b654553 683 scanner.setLocalizeLeftFlag(0);
j_j205 38:ebf73b654553 684 r5nav.executeRoute(pc, drive);
j_j205 38:ebf73b654553 685 wait(0.1);
j_j205 38:ebf73b654553 686
j_j205 38:ebf73b654553 687 r5nav.getShortestPath(15);
j_j205 38:ebf73b654553 688 pc.printf("\n\nDistance from %i to 15: %i\n", r5nav.getVertex(), r5nav.getMinDist(15) );
j_j205 38:ebf73b654553 689 pc.printf("Route:\n");
j_j205 38:ebf73b654553 690 //r5nav.printRoute(pc);
j_j205 38:ebf73b654553 691 scanner.setLocalizeRightFlag(0);
j_j205 38:ebf73b654553 692 scanner.setLocalizeLeftFlag(1);
j_j205 38:ebf73b654553 693 r5nav.executeRoute(pc, drive);
j_j205 38:ebf73b654553 694 wait(0.1);
j_j205 38:ebf73b654553 695
j_j205 38:ebf73b654553 696 r5nav.getShortestPath(5);
j_j205 38:ebf73b654553 697 pc.printf("\n\nDistance from %i to 5: %i\n", r5nav.getVertex(), r5nav.getMinDist(5) );
j_j205 38:ebf73b654553 698 pc.printf("Route:\n");
j_j205 38:ebf73b654553 699 //r5nav.printRoute(pc);
j_j205 38:ebf73b654553 700 scanner.setLocalizeRightFlag(1);
j_j205 38:ebf73b654553 701 scanner.setLocalizeLeftFlag(0);
j_j205 38:ebf73b654553 702 r5nav.executeRoute(pc, drive);
j_j205 38:ebf73b654553 703 wait(0.1);
j_j205 38:ebf73b654553 704
j_j205 38:ebf73b654553 705 /* may need to get back to 20 */
j_j205 38:ebf73b654553 706
j_j205 38:ebf73b654553 707 /* V4 peg retrieval complete
j_j205 38:ebf73b654553 708 Begin V5 peg retrieval */
j_j205 38:ebf73b654553 709 }
j_j205 35:4e09da30bda2 710 }
j_j205 35:4e09da30bda2 711
j_j205 38:ebf73b654553 712 /* add more code for for V5 and V6 */
j_j205 20:53ab414d7b33 713
j_j205 29:e77e8891d985 714 /*
j_j205 15:d8940756d5d5 715 drive.move(0, ((90.0)*(3.14159 / 180.0))); // return to initial angle
j_j205 15:d8940756d5d5 716 // wait for move to complete
j_j205 15:d8940756d5d5 717 while(!drive.isMoveDone())
j_j205 15:d8940756d5d5 718 {
j_j205 15:d8940756d5d5 719 wait(1e-6);
j_j205 15:d8940756d5d5 720 }
j_j205 29:e77e8891d985 721 */
j_j205 15:d8940756d5d5 722 pc.printf("\n\nExercise Complete");
j_j205 0:a3c39d3359ac 723 return 0;
j_j205 12:e9f878ced6e7 724 }
j_j205 12:e9f878ced6e7 725
j_j205 12:e9f878ced6e7 726 void activate()
j_j205 12:e9f878ced6e7 727 {
j_j205 12:e9f878ced6e7 728 led_red = 1;
j_j205 12:e9f878ced6e7 729 led_green = 0;
j_j205 12:e9f878ced6e7 730 active = true;
j_j205 25:670a59096bf8 731 }