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@36:5b0bf9153c3d, 2016-03-26 (annotated)

Committer:
j_j205
Date:
Sat Mar 26 23:21:21 2016 +0000
Revision:
36:5b0bf9153c3d
Parent:
35:4e09da30bda2
Child:
38:ebf73b654553
updated 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 12:e9f878ced6e7 15
j_j205 12:e9f878ced6e7 16 void activate();
j_j205 0:a3c39d3359ac 17
j_j205 0:a3c39d3359ac 18 int main()
j_j205 0:a3c39d3359ac 19 {
j_j205 0:a3c39d3359ac 20 pc.baud(115200);
j_j205 28:17c17157e728 21 // pc.baud(9600); /* interface via Bluetooth at 9600 */
j_j205 12:e9f878ced6e7 22 start.mode(PullUp); /* Start/Stop button is active low needing PullUp */
j_j205 12:e9f878ced6e7 23 start.fall(&activate);
j_j205 20:53ab414d7b33 24
j_j205 12:e9f878ced6e7 25 led_red = 0;
j_j205 12:e9f878ced6e7 26 led_green = 1;
j_j205 20:53ab414d7b33 27
j_j205 25:670a59096bf8 28 pc.printf("\nWaiting for START BUTTON\n");
j_j205 25:670a59096bf8 29 while(!active) // wait for start_button
j_j205 25:670a59096bf8 30 {
j_j205 25:670a59096bf8 31 wait(1e-6);
j_j205 25:670a59096bf8 32 }
j_j205 25:670a59096bf8 33
j_j205 32:82bfece56f8f 34 StepperDrive drive(pc, PTE19, PTE18, 0, PTE3, PTE2, 1, 10.0625, 8.4800, 500); // 8.375
j_j205 25:670a59096bf8 35 //(serial &, stepPinLeft, dirPinLeft, invertLeft, stepPinRight, dirPinRight, invertRight, wheelCircum, wheelSepar, periodUs)
j_j205 25:670a59096bf8 36
j_j205 32:82bfece56f8f 37 ShortRangeSensor shortRangeL(PTE0, PTE1); // verify i2c pins
j_j205 32:82bfece56f8f 38 ShortRangeSensor shortRangeR(PTC9, PTC8); // verify i2c pins
j_j205 26:d99d078921a0 39 LongRangeSensor longRangeL(pc, PTB2);
j_j205 26:d99d078921a0 40 LongRangeSensor longRangeR(pc, PTB3);
j_j205 36:5b0bf9153c3d 41 Gripper gripper(PTB2, PTB3); // grip pin, wrist pin
j_j205 25:670a59096bf8 42
j_j205 36:5b0bf9153c3d 43 Scanner scanner(pc, drive, PTB0, PTB1, shortRangeL, shortRangeR, longRangeL, longRangeR, gripper, 0.075);
j_j205 25:670a59096bf8 44 // (Serial &, PinName servoL, PinName servoR, shortRangeL, shortRangeR, longRangeL, longRangeR, period)
j_j205 25:670a59096bf8 45
j_j205 35:4e09da30bda2 46 Navigation r5nav(scanner, 25);
j_j205 8:90af8914ce03 47 wait(0.1);
j_j205 32:82bfece56f8f 48 /*
j_j205 25:670a59096bf8 49 r5nav.addGraphNode(0, 1, 10.0625, 0);
j_j205 25:670a59096bf8 50 r5nav.addGraphNode(1, 0, 10.0625, 180);
j_j205 25:670a59096bf8 51 r5nav.addGraphNode(1, 2, 10.0625, 270);
j_j205 25:670a59096bf8 52 r5nav.addGraphNode(2, 1, 10.0625, 90);
j_j205 25:670a59096bf8 53 r5nav.addGraphNode(2, 3, 10.0625, 180);
j_j205 25:670a59096bf8 54 r5nav.addGraphNode(3, 2, 10.0625, 0);
j_j205 25:670a59096bf8 55 r5nav.addGraphNode(3, 0, 10.0625, 90);
j_j205 25:670a59096bf8 56 r5nav.addGraphNode(0, 3, 10.0625, 270);
j_j205 29:e77e8891d985 57 */
j_j205 32:82bfece56f8f 58
j_j205 20:53ab414d7b33 59 // loading r5 map
j_j205 32:82bfece56f8f 60 r5nav.addGraphNode(0, 1, 13, 0);
j_j205 32:82bfece56f8f 61 r5nav.addGraphNode(1, 0, 13, 180);
j_j205 35:4e09da30bda2 62 r5nav.addGraphNode(1, 2, 68, 356.5); // original angle = 0
j_j205 32:82bfece56f8f 63 r5nav.addGraphNode(1, 3, 15, 90);
j_j205 35:4e09da30bda2 64 r5nav.addGraphNode(2, 1, 68, 180);
j_j205 32:82bfece56f8f 65 r5nav.addGraphNode(3, 1, 15, 270);
j_j205 35:4e09da30bda2 66 r5nav.addGraphNode(3, 4, 8, 180);
j_j205 35:4e09da30bda2 67 // r5nav.addGraphNode(3, 5, 23, 356.5); // (-5 degrees) original angle = 0
j_j205 35:4e09da30bda2 68 r5nav.addGraphNode(3, 15, 6, 0);
j_j205 35:4e09da30bda2 69 r5nav.addGraphNode(4, 3, 8, 0);
j_j205 35:4e09da30bda2 70 // r5nav.addGraphNode(5, 3, 23, 180);
j_j205 8:90af8914ce03 71 r5nav.addGraphNode(5, 6, 37, 0);
j_j205 32:82bfece56f8f 72 r5nav.addGraphNode(5, 8, 12.5, 90);
j_j205 35:4e09da30bda2 73 r5nav.addGraphNode(5, 15, 17, 180);
j_j205 32:82bfece56f8f 74 r5nav.addGraphNode(6, 5, 37, 176.5); // original angle = 180
j_j205 35:4e09da30bda2 75 r5nav.addGraphNode(6, 7, 6, 0);
j_j205 32:82bfece56f8f 76 r5nav.addGraphNode(6, 10, 12.5, 90);
j_j205 35:4e09da30bda2 77 r5nav.addGraphNode(7, 6, 6, 180);
j_j205 32:82bfece56f8f 78 r5nav.addGraphNode(8, 5, 12.75, 270); // original distance = 12.5
j_j205 35:4e09da30bda2 79 // r5nav.addGraphNode(8, 9, 35, 174.5); // original angle = 180
j_j205 35:4e09da30bda2 80 // r5nav.addGraphNode(8, 10, 37, 0);
j_j205 28:17c17157e728 81 r5nav.addGraphNode(8, 11, 12, 90);
j_j205 35:4e09da30bda2 82 r5nav.addGraphNode(8, 16, 6, 180);
j_j205 35:4e09da30bda2 83 r5nav.addGraphNode(8, 17, 6, 0);
j_j205 35:4e09da30bda2 84 // r5nav.addGraphNode(9, 8, 35, 0);
j_j205 35:4e09da30bda2 85 r5nav.addGraphNode(9, 16, 23, 0);
j_j205 32:82bfece56f8f 86 r5nav.addGraphNode(10, 6, 12.5, 270);
j_j205 35:4e09da30bda2 87 r5nav.addGraphNode(10, 13, 12, 90);
j_j205 35:4e09da30bda2 88 r5nav.addGraphNode(10, 18, 6, 180);
j_j205 28:17c17157e728 89 r5nav.addGraphNode(11, 8, 12, 270);
j_j205 28:17c17157e728 90 r5nav.addGraphNode(11, 12, 35, 180);
j_j205 35:4e09da30bda2 91 r5nav.addGraphNode(11, 19, 6, 180);
j_j205 28:17c17157e728 92 r5nav.addGraphNode(12, 11, 35, 0);
j_j205 35:4e09da30bda2 93 r5nav.addGraphNode(12, 19, 23, 0);
j_j205 35:4e09da30bda2 94 r5nav.addGraphNode(13, 10, 12, 270);
j_j205 28:17c17157e728 95 r5nav.addGraphNode(13, 14, 12, 0);
j_j205 28:17c17157e728 96 r5nav.addGraphNode(14, 13, 12, 180);
j_j205 35:4e09da30bda2 97 r5nav.addGraphNode(14, 20, 21, 90);
j_j205 35:4e09da30bda2 98 r5nav.addGraphNode(15, 3, 6, 180);
j_j205 35:4e09da30bda2 99 r5nav.addGraphNode(15, 5, 17, 0);
j_j205 35:4e09da30bda2 100 r5nav.addGraphNode(16, 8, 6, 0);
j_j205 35:4e09da30bda2 101 r5nav.addGraphNode(16, 9, 23, 180);
j_j205 35:4e09da30bda2 102 r5nav.addGraphNode(17, 8, 6, 180);
j_j205 35:4e09da30bda2 103 r5nav.addGraphNode(17, 18, 25, 0);
j_j205 35:4e09da30bda2 104 r5nav.addGraphNode(18, 10, 6, 0);
j_j205 35:4e09da30bda2 105 r5nav.addGraphNode(18, 17, 25, 180);
j_j205 35:4e09da30bda2 106 r5nav.addGraphNode(19, 11, 6, 0);
j_j205 35:4e09da30bda2 107 r5nav.addGraphNode(19, 12, 23, 180);
j_j205 35:4e09da30bda2 108 r5nav.addGraphNode(20, 14, 21, 270);
j_j205 35:4e09da30bda2 109 r5nav.addGraphNode(20, 21, 24, 90);
j_j205 35:4e09da30bda2 110 r5nav.addGraphNode(21, 20, 24, 270);
j_j205 35:4e09da30bda2 111 r5nav.addGraphNode(21, 22, 4, 180);
j_j205 35:4e09da30bda2 112 r5nav.addGraphNode(22, 21, 4, 0);
j_j205 35:4e09da30bda2 113 r5nav.addGraphNode(22, 23, 80, 180);
j_j205 35:4e09da30bda2 114 r5nav.addGraphNode(23, 22, 80, 0);
j_j205 35:4e09da30bda2 115 r5nav.addGraphNode(23, 24, 2, 270);
j_j205 35:4e09da30bda2 116 r5nav.addGraphNode(24, 23, 2, 90);
j_j205 32:82bfece56f8f 117
j_j205 35:4e09da30bda2 118 const uint16_t V1 = 7;
j_j205 35:4e09da30bda2 119 const uint16_t V2 = 9;
j_j205 35:4e09da30bda2 120 const uint16_t V3 = 12;
j_j205 35:4e09da30bda2 121 const uint16_t V4 = 20;
j_j205 35:4e09da30bda2 122 const uint16_t V5 = 22;
j_j205 35:4e09da30bda2 123 const uint16_t V6 = 24;
j_j205 8:90af8914ce03 124 const uint16_t YELLOW_DROP_ZONE = 4;
j_j205 8:90af8914ce03 125 const uint16_t RED_DROP_ZONE = 2;
j_j205 32:82bfece56f8f 126
j_j205 29:e77e8891d985 127 /*
j_j205 15:d8940756d5d5 128 r5nav.getShortestPath(1);
j_j205 15:d8940756d5d5 129 pc.printf("\n\nDistance from 0 to 1: %i\n", r5nav.getMinDist(1) );
j_j205 15:d8940756d5d5 130 pc.printf("Route:\n");
j_j205 15:d8940756d5d5 131 r5nav.executeRoute(pc, drive);
j_j205 15:d8940756d5d5 132 wait(0.1);
j_j205 20:53ab414d7b33 133
j_j205 15:d8940756d5d5 134 r5nav.getShortestPath(2);
j_j205 15:d8940756d5d5 135 pc.printf("\n\nDistance from 1 to 2: %i\n", r5nav.getMinDist(2) );
j_j205 15:d8940756d5d5 136 pc.printf("Route:\n");
j_j205 15:d8940756d5d5 137 r5nav.executeRoute(pc, drive);
j_j205 15:d8940756d5d5 138 wait(0.1);
j_j205 20:53ab414d7b33 139
j_j205 15:d8940756d5d5 140 r5nav.getShortestPath(3);
j_j205 15:d8940756d5d5 141 pc.printf("\n\nDistance from 2 to 3: %i\n", r5nav.getMinDist(3) );
j_j205 8:90af8914ce03 142 pc.printf("Route:\n");
j_j205 15:d8940756d5d5 143 r5nav.executeRoute(pc, drive);
j_j205 15:d8940756d5d5 144 wait(0.1);
j_j205 20:53ab414d7b33 145
j_j205 15:d8940756d5d5 146 r5nav.getShortestPath(0);
j_j205 15:d8940756d5d5 147 pc.printf("\n\nDistance from 3 to 0: %i\n", r5nav.getMinDist(0) );
j_j205 15:d8940756d5d5 148 pc.printf("Route:\n");
j_j205 15:d8940756d5d5 149 r5nav.executeRoute(pc, drive);
j_j205 15:d8940756d5d5 150 wait(0.1);
j_j205 29:e77e8891d985 151 */
j_j205 32:82bfece56f8f 152
j_j205 35:4e09da30bda2 153 /* Begin 1st peg retrieval */
j_j205 35:4e09da30bda2 154
j_j205 35:4e09da30bda2 155 r5nav.getShortestPath(1);
j_j205 35:4e09da30bda2 156 pc.printf("\n\nDistance from 0 to 1: %i\n", r5nav.getMinDist(1) );
j_j205 15:d8940756d5d5 157 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 158 scanner.setLocalizeRightFlag(1);
j_j205 35:4e09da30bda2 159 scanner.setLocalizeLeftFlag(0);
j_j205 12:e9f878ced6e7 160 r5nav.executeRoute(pc, drive);
j_j205 8:90af8914ce03 161 wait(0.1);
j_j205 1:1958e6db41fc 162
j_j205 35:4e09da30bda2 163 r5nav.getShortestPath(3);
j_j205 35:4e09da30bda2 164 pc.printf("\n\nDistance from 1 to 3: %i\n", r5nav.getMinDist(3) );
j_j205 35:4e09da30bda2 165 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 166 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 167 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 168 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 169 wait(0.1);
j_j205 35:4e09da30bda2 170
j_j205 35:4e09da30bda2 171 r5nav.getShortestPath(15);
j_j205 35:4e09da30bda2 172 pc.printf("\n\nDistance from 3 to 15: %i\n", r5nav.getMinDist(15) );
j_j205 35:4e09da30bda2 173 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 174 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 175 scanner.setLocalizeLeftFlag(1);
j_j205 35:4e09da30bda2 176 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 177 wait(0.1);
j_j205 35:4e09da30bda2 178
j_j205 35:4e09da30bda2 179 r5nav.getShortestPath(V1);
j_j205 35:4e09da30bda2 180 pc.printf("\n\nDistance from 15 to V1: %i\n", r5nav.getMinDist(V1) );
j_j205 35:4e09da30bda2 181 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 182 scanner.setLocalizeRightFlag(1);
j_j205 35:4e09da30bda2 183 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 184 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 185 wait(0.1);
j_j205 35:4e09da30bda2 186
j_j205 35:4e09da30bda2 187 /* add hunt method call */
j_j205 35:4e09da30bda2 188
j_j205 35:4e09da30bda2 189 r5nav.getShortestPath(15);
j_j205 35:4e09da30bda2 190 pc.printf("\n\nDistance from V1 to 15: %i\n", r5nav.getMinDist(15) );
j_j205 35:4e09da30bda2 191 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 192 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 193 scanner.setLocalizeLeftFlag(1);
j_j205 35:4e09da30bda2 194 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 195 wait(0.1);
j_j205 35:4e09da30bda2 196
j_j205 35:4e09da30bda2 197 r5nav.getShortestPath(3);
j_j205 35:4e09da30bda2 198 pc.printf("\n\nDistance from 15 to 3: %i\n", r5nav.getMinDist(3) );
j_j205 8:90af8914ce03 199 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 200 scanner.setLocalizeRightFlag(1);
j_j205 35:4e09da30bda2 201 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 202 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 203 wait(0.1);
j_j205 35:4e09da30bda2 204
j_j205 35:4e09da30bda2 205 if (scanner.getYellowFlag() == 1)
j_j205 35:4e09da30bda2 206 {
j_j205 35:4e09da30bda2 207 r5nav.getShortestPath(YELLOW_DROP_ZONE);
j_j205 35:4e09da30bda2 208 pc.printf("\n\nDistance from %i to YELLOW_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(YELLOW_DROP_ZONE) );
j_j205 35:4e09da30bda2 209 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 210 //r5nav.printRoute(pc);
j_j205 35:4e09da30bda2 211 scanner.setLocalizeRightFlag(1);
j_j205 35:4e09da30bda2 212 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 213 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 214 wait(0.1);
j_j205 35:4e09da30bda2 215 }
j_j205 35:4e09da30bda2 216
j_j205 35:4e09da30bda2 217 else
j_j205 35:4e09da30bda2 218 {
j_j205 35:4e09da30bda2 219 r5nav.getShortestPath(1);
j_j205 35:4e09da30bda2 220 pc.printf("\n\nDistance from 3 to 1: %i\n", r5nav.getMinDist(1) );
j_j205 35:4e09da30bda2 221 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 222 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 223 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 224 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 225 wait(0.1);
j_j205 35:4e09da30bda2 226
j_j205 35:4e09da30bda2 227 r5nav.getShortestPath(RED_DROP_ZONE);
j_j205 35:4e09da30bda2 228 pc.printf("\n\nDistance from %i to RED_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(RED_DROP_ZONE) );
j_j205 35:4e09da30bda2 229 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 230 //r5nav.printRoute(pc);
j_j205 35:4e09da30bda2 231 scanner.setLocalizeRightFlag(1);
j_j205 35:4e09da30bda2 232 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 233 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 234 wait(0.1);
j_j205 35:4e09da30bda2 235 }
j_j205 35:4e09da30bda2 236
j_j205 35:4e09da30bda2 237 /* 1st peg retrieval complete
j_j205 35:4e09da30bda2 238 Begin 2nd peg retrieval */
j_j205 35:4e09da30bda2 239
j_j205 35:4e09da30bda2 240 if (r5nav.getVertex() == YELLOW_DROP_ZONE)
j_j205 35:4e09da30bda2 241 {
j_j205 35:4e09da30bda2 242 r5nav.getShortestPath(3);
j_j205 35:4e09da30bda2 243 pc.printf("\n\nDistance from YELLOW_DROP_ZONE to 3: %i\n", r5nav.getMinDist(3) );
j_j205 35:4e09da30bda2 244 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 245 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 246 scanner.setLocalizeLeftFlag(1);
j_j205 35:4e09da30bda2 247 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 248 wait(0.1);
j_j205 35:4e09da30bda2 249 }
j_j205 35:4e09da30bda2 250
j_j205 35:4e09da30bda2 251 else
j_j205 35:4e09da30bda2 252 {
j_j205 35:4e09da30bda2 253 r5nav.getShortestPath(1);
j_j205 35:4e09da30bda2 254 pc.printf("\n\nDistance from RED_DROP_ZONE to 1: %i\n", r5nav.getMinDist(1) );
j_j205 35:4e09da30bda2 255 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 256 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 257 scanner.setLocalizeLeftFlag(1);
j_j205 35:4e09da30bda2 258 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 259 wait(0.1);
j_j205 35:4e09da30bda2 260
j_j205 35:4e09da30bda2 261 r5nav.getShortestPath(3);
j_j205 35:4e09da30bda2 262 pc.printf("\n\nDistance from 1 to 3: %i\n", r5nav.getMinDist(3) );
j_j205 35:4e09da30bda2 263 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 264 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 265 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 266 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 267 wait(0.1);
j_j205 35:4e09da30bda2 268 }
j_j205 35:4e09da30bda2 269
j_j205 35:4e09da30bda2 270 r5nav.getShortestPath(15);
j_j205 35:4e09da30bda2 271 pc.printf("\n\nDistance from 3 to 15: %i\n", r5nav.getMinDist(15) );
j_j205 35:4e09da30bda2 272 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 273 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 274 scanner.setLocalizeLeftFlag(1);
j_j205 35:4e09da30bda2 275 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 276 wait(0.1);
j_j205 35:4e09da30bda2 277
j_j205 35:4e09da30bda2 278 r5nav.getShortestPath(5);
j_j205 35:4e09da30bda2 279 pc.printf("\n\nDistance from 15 to 5: %i\n", r5nav.getMinDist(5) );
j_j205 35:4e09da30bda2 280 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 281 scanner.setLocalizeRightFlag(1);
j_j205 35:4e09da30bda2 282 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 283 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 284 wait(0.1);
j_j205 35:4e09da30bda2 285
j_j205 35:4e09da30bda2 286 r5nav.getShortestPath(16);
j_j205 35:4e09da30bda2 287 pc.printf("\n\nDistance from 5 to 16: %i\n", r5nav.getMinDist(16) );
j_j205 35:4e09da30bda2 288 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 289 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 290 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 291 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 292 wait(0.1);
j_j205 35:4e09da30bda2 293
j_j205 35:4e09da30bda2 294 r5nav.getShortestPath(V2);
j_j205 35:4e09da30bda2 295 pc.printf("\n\nDistance from 16 to V2: %i\n", r5nav.getMinDist(V2) );
j_j205 35:4e09da30bda2 296 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 297 scanner.setLocalizeRightFlag(1);
j_j205 35:4e09da30bda2 298 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 299 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 300 wait(0.1);
j_j205 35:4e09da30bda2 301
j_j205 35:4e09da30bda2 302 /* add hunt method call */
j_j205 35:4e09da30bda2 303
j_j205 35:4e09da30bda2 304 r5nav.getShortestPath(16);
j_j205 35:4e09da30bda2 305 pc.printf("\n\nDistance from V2 to 16: %i\n", r5nav.getMinDist(16) );
j_j205 35:4e09da30bda2 306 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 307 scanner.setLocalizeRightFlag(1);
j_j205 35:4e09da30bda2 308 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 309 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 310 wait(0.1);
j_j205 35:4e09da30bda2 311
j_j205 35:4e09da30bda2 312 r5nav.getShortestPath(5);
j_j205 35:4e09da30bda2 313 pc.printf("\n\nDistance from 16 to 5: %i\n", r5nav.getMinDist(5) );
j_j205 35:4e09da30bda2 314 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 315 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 316 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 317 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 318 wait(0.1);
j_j205 35:4e09da30bda2 319
j_j205 35:4e09da30bda2 320 r5nav.getShortestPath(15);
j_j205 35:4e09da30bda2 321 pc.printf("\n\nDistance from 5 to 15: %i\n", r5nav.getMinDist(15) );
j_j205 35:4e09da30bda2 322 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 323 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 324 scanner.setLocalizeLeftFlag(1);
j_j205 35:4e09da30bda2 325 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 326 wait(0.1);
j_j205 35:4e09da30bda2 327
j_j205 35:4e09da30bda2 328 r5nav.getShortestPath(3);
j_j205 35:4e09da30bda2 329 pc.printf("\n\nDistance from 15 to 3: %i\n", r5nav.getMinDist(3) );
j_j205 35:4e09da30bda2 330 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 331 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 332 scanner.setLocalizeLeftFlag(1);
j_j205 12:e9f878ced6e7 333 r5nav.executeRoute(pc, drive);
j_j205 8:90af8914ce03 334 wait(0.1);
j_j205 35:4e09da30bda2 335
j_j205 35:4e09da30bda2 336 if (scanner.getYellowFlag() == 1)
j_j205 35:4e09da30bda2 337 {
j_j205 35:4e09da30bda2 338 r5nav.getShortestPath(YELLOW_DROP_ZONE);
j_j205 35:4e09da30bda2 339 pc.printf("\n\nDistance from %i to YELLOW_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(YELLOW_DROP_ZONE) );
j_j205 35:4e09da30bda2 340 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 341 //r5nav.printRoute(pc);
j_j205 35:4e09da30bda2 342 scanner.setLocalizeRightFlag(1);
j_j205 35:4e09da30bda2 343 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 344 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 345 wait(0.1);
j_j205 35:4e09da30bda2 346 }
j_j205 35:4e09da30bda2 347
j_j205 35:4e09da30bda2 348 else
j_j205 35:4e09da30bda2 349 {
j_j205 35:4e09da30bda2 350 r5nav.getShortestPath(1);
j_j205 35:4e09da30bda2 351 pc.printf("\n\nDistance from 3 to 1: %i\n", r5nav.getMinDist(1) );
j_j205 35:4e09da30bda2 352 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 353 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 354 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 355 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 356 wait(0.1);
j_j205 35:4e09da30bda2 357
j_j205 35:4e09da30bda2 358 r5nav.getShortestPath(RED_DROP_ZONE);
j_j205 35:4e09da30bda2 359 pc.printf("\n\nDistance from %i to RED_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(RED_DROP_ZONE) );
j_j205 35:4e09da30bda2 360 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 361 //r5nav.printRoute(pc);
j_j205 35:4e09da30bda2 362 scanner.setLocalizeRightFlag(1);
j_j205 35:4e09da30bda2 363 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 364 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 365 wait(0.1);
j_j205 35:4e09da30bda2 366 }
j_j205 35:4e09da30bda2 367
j_j205 35:4e09da30bda2 368 /* 2nd peg retrieval complete
j_j205 35:4e09da30bda2 369 Begin 3nd peg retrieval */
j_j205 35:4e09da30bda2 370
j_j205 35:4e09da30bda2 371
j_j205 35:4e09da30bda2 372 if (r5nav.getVertex() == YELLOW_DROP_ZONE)
j_j205 35:4e09da30bda2 373 {
j_j205 35:4e09da30bda2 374 r5nav.getShortestPath(3);
j_j205 35:4e09da30bda2 375 pc.printf("\n\nDistance from YELLOW_DROP_ZONE to 3: %i\n", r5nav.getMinDist(3) );
j_j205 35:4e09da30bda2 376 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 377 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 378 scanner.setLocalizeLeftFlag(1);
j_j205 35:4e09da30bda2 379 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 380 wait(0.1);
j_j205 35:4e09da30bda2 381 }
j_j205 1:1958e6db41fc 382
j_j205 35:4e09da30bda2 383 else
j_j205 35:4e09da30bda2 384 {
j_j205 35:4e09da30bda2 385 r5nav.getShortestPath(1);
j_j205 35:4e09da30bda2 386 pc.printf("\n\nDistance from RED_DROP_ZONE to 1: %i\n", r5nav.getMinDist(1) );
j_j205 35:4e09da30bda2 387 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 388 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 389 scanner.setLocalizeLeftFlag(1);
j_j205 35:4e09da30bda2 390 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 391 wait(0.1);
j_j205 35:4e09da30bda2 392
j_j205 35:4e09da30bda2 393 r5nav.getShortestPath(3);
j_j205 35:4e09da30bda2 394 pc.printf("\n\nDistance from 1 to 3: %i\n", r5nav.getMinDist(3) );
j_j205 35:4e09da30bda2 395 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 396 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 397 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 398 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 399 wait(0.1);
j_j205 35:4e09da30bda2 400 }
j_j205 35:4e09da30bda2 401
j_j205 35:4e09da30bda2 402 r5nav.getShortestPath(15);
j_j205 35:4e09da30bda2 403 pc.printf("\n\nDistance from 3 to 15: %i\n", r5nav.getMinDist(15) );
j_j205 8:90af8914ce03 404 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 405 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 406 scanner.setLocalizeLeftFlag(1);
j_j205 35:4e09da30bda2 407 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 408 wait(0.1);
j_j205 35:4e09da30bda2 409
j_j205 35:4e09da30bda2 410 r5nav.getShortestPath(5);
j_j205 35:4e09da30bda2 411 pc.printf("\n\nDistance from 15 to 5: %i\n", r5nav.getMinDist(5) );
j_j205 35:4e09da30bda2 412 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 413 scanner.setLocalizeRightFlag(1);
j_j205 35:4e09da30bda2 414 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 415 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 416 wait(0.1);
j_j205 35:4e09da30bda2 417
j_j205 35:4e09da30bda2 418 r5nav.getShortestPath(19);
j_j205 35:4e09da30bda2 419 pc.printf("\n\nDistance from 5 to 19: %i\n", r5nav.getMinDist(19) );
j_j205 35:4e09da30bda2 420 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 421 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 422 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 423 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 424 wait(0.1);
j_j205 35:4e09da30bda2 425
j_j205 35:4e09da30bda2 426 r5nav.getShortestPath(V3);
j_j205 35:4e09da30bda2 427 pc.printf("\n\nDistance from 19 to V3: %i\n", r5nav.getMinDist(V3) );
j_j205 35:4e09da30bda2 428 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 429 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 430 scanner.setLocalizeLeftFlag(1);
j_j205 12:e9f878ced6e7 431 r5nav.executeRoute(pc, drive);
j_j205 8:90af8914ce03 432 wait(0.1);
j_j205 35:4e09da30bda2 433
j_j205 35:4e09da30bda2 434 /* add hunt method call */
j_j205 35:4e09da30bda2 435
j_j205 35:4e09da30bda2 436 if (scanner.getObjectFound() == 1)
j_j205 35:4e09da30bda2 437 {
j_j205 35:4e09da30bda2 438 r5nav.getShortestPath(19);
j_j205 35:4e09da30bda2 439 pc.printf("\n\nDistance from V3 to 19: %i\n", r5nav.getMinDist(19) );
j_j205 35:4e09da30bda2 440 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 441 scanner.setLocalizeRightFlag(1);
j_j205 35:4e09da30bda2 442 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 443 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 444 wait(0.1);
j_j205 35:4e09da30bda2 445
j_j205 35:4e09da30bda2 446 // at node 5
j_j205 35:4e09da30bda2 447 r5nav.getShortestPath(5);
j_j205 35:4e09da30bda2 448 pc.printf("\n\nDistance from 19 to 5: %i\n", r5nav.getMinDist(5) );
j_j205 35:4e09da30bda2 449 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 450 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 451 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 452 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 453 wait(0.1);
j_j205 35:4e09da30bda2 454
j_j205 35:4e09da30bda2 455 r5nav.getShortestPath(15);
j_j205 35:4e09da30bda2 456 pc.printf("\n\nDistance from 5 to 15: %i\n", r5nav.getMinDist(15) );
j_j205 35:4e09da30bda2 457 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 458 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 459 scanner.setLocalizeLeftFlag(1);
j_j205 35:4e09da30bda2 460 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 461 wait(0.1);
j_j205 35:4e09da30bda2 462
j_j205 35:4e09da30bda2 463 r5nav.getShortestPath(3);
j_j205 35:4e09da30bda2 464 pc.printf("\n\nDistance from 15 to 3: %i\n", r5nav.getMinDist(3) );
j_j205 35:4e09da30bda2 465 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 466 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 467 scanner.setLocalizeLeftFlag(1);
j_j205 35:4e09da30bda2 468 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 469 wait(0.1);
j_j205 35:4e09da30bda2 470
j_j205 35:4e09da30bda2 471 if (scanner.getYellowFlag() == 1)
j_j205 35:4e09da30bda2 472 {
j_j205 35:4e09da30bda2 473 r5nav.getShortestPath(YELLOW_DROP_ZONE);
j_j205 35:4e09da30bda2 474 pc.printf("\n\nDistance from %i to YELLOW_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(YELLOW_DROP_ZONE) );
j_j205 35:4e09da30bda2 475 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 476 //r5nav.printRoute(pc);
j_j205 35:4e09da30bda2 477 scanner.setLocalizeRightFlag(1);
j_j205 35:4e09da30bda2 478 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 479 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 480 wait(0.1);
j_j205 35:4e09da30bda2 481 }
j_j205 35:4e09da30bda2 482
j_j205 35:4e09da30bda2 483 else
j_j205 35:4e09da30bda2 484 {
j_j205 35:4e09da30bda2 485 r5nav.getShortestPath(1);
j_j205 35:4e09da30bda2 486 pc.printf("\n\nDistance from 3 to 1: %i\n", r5nav.getMinDist(1) );
j_j205 35:4e09da30bda2 487 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 488 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 489 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 490 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 491 wait(0.1);
j_j205 35:4e09da30bda2 492
j_j205 35:4e09da30bda2 493 r5nav.getShortestPath(RED_DROP_ZONE);
j_j205 35:4e09da30bda2 494 pc.printf("\n\nDistance from %i to RED_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(RED_DROP_ZONE) );
j_j205 35:4e09da30bda2 495 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 496 //r5nav.printRoute(pc);
j_j205 35:4e09da30bda2 497 scanner.setLocalizeRightFlag(1);
j_j205 35:4e09da30bda2 498 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 499 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 500 wait(0.1);
j_j205 35:4e09da30bda2 501 }
j_j205 35:4e09da30bda2 502 }
j_j205 35:4e09da30bda2 503
j_j205 35:4e09da30bda2 504 else // peg V3 not found
j_j205 35:4e09da30bda2 505 {
j_j205 35:4e09da30bda2 506 /* need to implement rest of moves
j_j205 35:4e09da30bda2 507
j_j205 35:4e09da30bda2 508 r5nav.getShortestPath(19);
j_j205 35:4e09da30bda2 509 pc.printf("\n\nDistance from 12 to 19: %i\n", r5nav.getMinDist(19) );
j_j205 35:4e09da30bda2 510 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 511 scanner.setLocalizeRightFlag(1);
j_j205 35:4e09da30bda2 512 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 513 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 514 wait(0.1);
j_j205 35:4e09da30bda2 515
j_j205 35:4e09da30bda2 516 // at node 5
j_j205 35:4e09da30bda2 517 r5nav.getShortestPath(5);
j_j205 35:4e09da30bda2 518 pc.printf("\n\nDistance from 19 to 5: %i\n", r5nav.getMinDist(5) );
j_j205 35:4e09da30bda2 519 pc.printf("Route:\n");
j_j205 35:4e09da30bda2 520 scanner.setLocalizeRightFlag(0);
j_j205 35:4e09da30bda2 521 scanner.setLocalizeLeftFlag(0);
j_j205 35:4e09da30bda2 522 r5nav.executeRoute(pc, drive);
j_j205 35:4e09da30bda2 523 wait(0.1); */
j_j205 35:4e09da30bda2 524 }
j_j205 35:4e09da30bda2 525
j_j205 35:4e09da30bda2 526 /* 3rd peg retrieval complete
j_j205 35:4e09da30bda2 527 Begin 4th peg retrieval */
j_j205 35:4e09da30bda2 528
j_j205 35:4e09da30bda2 529
j_j205 20:53ab414d7b33 530
j_j205 29:e77e8891d985 531 /*
j_j205 15:d8940756d5d5 532 drive.move(0, ((90.0)*(3.14159 / 180.0))); // return to initial angle
j_j205 15:d8940756d5d5 533 // wait for move to complete
j_j205 15:d8940756d5d5 534 while(!drive.isMoveDone())
j_j205 15:d8940756d5d5 535 {
j_j205 15:d8940756d5d5 536 wait(1e-6);
j_j205 15:d8940756d5d5 537 }
j_j205 29:e77e8891d985 538 */
j_j205 15:d8940756d5d5 539 pc.printf("\n\nExercise Complete");
j_j205 0:a3c39d3359ac 540 return 0;
j_j205 12:e9f878ced6e7 541 }
j_j205 12:e9f878ced6e7 542
j_j205 12:e9f878ced6e7 543 void activate()
j_j205 12:e9f878ced6e7 544 {
j_j205 12:e9f878ced6e7 545 led_red = 1;
j_j205 12:e9f878ced6e7 546 led_green = 0;
j_j205 12:e9f878ced6e7 547 active = true;
j_j205 25:670a59096bf8 548 }