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@39:1e26cc57c8b7, 2016-04-06 (annotated)

Committer:
j_j205
Date:
Wed Apr 06 22:07:38 2016 +0000
Revision:
39:1e26cc57c8b7
Parent:
38:ebf73b654553
Child:
40:2d33bb4d6d6f
changes in localization

Who changed what in which revision?

UserRevisionLine numberNew contents of line
j_j205 39:1e26cc57c8b7 1 #include "mbed.h"
j_j205 39:1e26cc57c8b7 2 #include "StepperDrive.h"
j_j205 39:1e26cc57c8b7 3 #include "LongRangeSensor.h"
j_j205 8:90af8914ce03 4 #include "navigation.h"
j_j205 29:e77e8891d985 5 #include "Gripper.h"
j_j205 39:1e26cc57c8b7 6
j_j205 0:a3c39d3359ac 7
j_j205 0:a3c39d3359ac 8 Serial pc(USBTX, USBRX);
j_j205 39:1e26cc57c8b7 9 Serial bluetooth(PTE16,PTE17); // bluetooth
j_j205 12:e9f878ced6e7 10 InterruptIn start(SW1);
j_j205 12:e9f878ced6e7 11 DigitalOut led_red(LED_RED);
j_j205 12:e9f878ced6e7 12 DigitalOut led_green(LED_GREEN);
j_j205 12:e9f878ced6e7 13 bool active = false;
j_j205 39:1e26cc57c8b7 14 float distLocalL = 0.0;
j_j205 35:4e09da30bda2 15
j_j205 39:1e26cc57c8b7 16 void reverseMove(StepperDrive &drive, float dist, float angle);
j_j205 12:e9f878ced6e7 17
j_j205 12:e9f878ced6e7 18 void activate()
j_j205 12:e9f878ced6e7 19 {
j_j205 12:e9f878ced6e7 20 led_red = 1;
j_j205 12:e9f878ced6e7 21 led_green = 0;
j_j205 12:e9f878ced6e7 22 active = true;
j_j205 25:670a59096bf8 23 }
j_j205 39:1e26cc57c8b7 24
j_j205 39:1e26cc57c8b7 25 int main()
j_j205 39:1e26cc57c8b7 26 {
j_j205 39:1e26cc57c8b7 27 pc.baud(115200);
j_j205 39:1e26cc57c8b7 28 bluetooth.baud(9600); /* interface via Bluetooth at 9600 */
j_j205 39:1e26cc57c8b7 29 start.mode(PullUp); /* Button is active low needing PullUp */
j_j205 39:1e26cc57c8b7 30 start.fall(&activate);
j_j205 39:1e26cc57c8b7 31
j_j205 39:1e26cc57c8b7 32 led_red = 0;
j_j205 39:1e26cc57c8b7 33 led_green = 1;
j_j205 39:1e26cc57c8b7 34
j_j205 39:1e26cc57c8b7 35 LongRangeSensor longRangeL(bluetooth, PTB2);
j_j205 39:1e26cc57c8b7 36 LongRangeSensor longRangeR(bluetooth, PTB3);
j_j205 39:1e26cc57c8b7 37 pc.printf("\nLong Range Sensors created");
j_j205 39:1e26cc57c8b7 38 bluetooth.printf("\nLong Range Sensors created");
j_j205 39:1e26cc57c8b7 39
j_j205 39:1e26cc57c8b7 40 StepperDrive drive(bluetooth, PTE19, PTE18, 1, PTE3, PTE2, 0, 10.0625,
j_j205 39:1e26cc57c8b7 41 8.4800, 650); // 8.375
j_j205 39:1e26cc57c8b7 42 /* (serial &, stepPinLeft, dirPinLeft, invertLeft,
j_j205 39:1e26cc57c8b7 43 stepPinRight, dirPinRight, invertRight, wheelCircum,
j_j205 39:1e26cc57c8b7 44 wheelSepar, periodUs) */
j_j205 39:1e26cc57c8b7 45 pc.printf("\nStepperDrive created");
j_j205 39:1e26cc57c8b7 46 bluetooth.printf("\nStepperDrive created");
j_j205 39:1e26cc57c8b7 47
j_j205 39:1e26cc57c8b7 48 // Gripper gripper(PTE20, PTE21); // grip pin, wrist pin
j_j205 39:1e26cc57c8b7 49
j_j205 39:1e26cc57c8b7 50 Navigation r5map(bluetooth, drive, longRangeL, longRangeR, led_red, led_green, 61);
j_j205 39:1e26cc57c8b7 51 pc.printf("\nNavigation created");
j_j205 39:1e26cc57c8b7 52 bluetooth.printf("\nNavigation created");
j_j205 39:1e26cc57c8b7 53
j_j205 39:1e26cc57c8b7 54 //loading r5 map...
j_j205 39:1e26cc57c8b7 55 r5map.addGraphNode(0, 1, 6, 0);
j_j205 39:1e26cc57c8b7 56 r5map.addGraphNode(1, 0, 6, 180);
j_j205 39:1e26cc57c8b7 57 r5map.addGraphNode(1, 2, 7, 0);
j_j205 39:1e26cc57c8b7 58 r5map.addGraphNode(2, 1, 7, 180);
j_j205 39:1e26cc57c8b7 59 r5map.addGraphNode(2, 3, 8, 0);
j_j205 39:1e26cc57c8b7 60 r5map.addGraphNode(2, 12, 15, 90);
j_j205 39:1e26cc57c8b7 61 r5map.addGraphNode(3, 2, 8, 180);
j_j205 39:1e26cc57c8b7 62 r5map.addGraphNode(3, 4, 8, 0);
j_j205 39:1e26cc57c8b7 63 r5map.addGraphNode(4, 3, 8, 180);
j_j205 39:1e26cc57c8b7 64 r5map.addGraphNode(4, 5, 8, 0);
j_j205 39:1e26cc57c8b7 65 r5map.addGraphNode(5, 4, 8, 180);
j_j205 39:1e26cc57c8b7 66 r5map.addGraphNode(5, 6, 8, 0);
j_j205 39:1e26cc57c8b7 67 r5map.addGraphNode(6, 5, 8, 180);
j_j205 39:1e26cc57c8b7 68 r5map.addGraphNode(6, 7, 8, 0);
j_j205 39:1e26cc57c8b7 69 r5map.addGraphNode(7, 6, 8, 180);
j_j205 39:1e26cc57c8b7 70 r5map.addGraphNode(7, 8, 8, 0);
j_j205 39:1e26cc57c8b7 71 r5map.addGraphNode(8, 7, 8, 180);
j_j205 39:1e26cc57c8b7 72 r5map.addGraphNode(8, 9, 8, 0);
j_j205 39:1e26cc57c8b7 73 r5map.addGraphNode(9, 8, 8, 180);
j_j205 39:1e26cc57c8b7 74 r5map.addGraphNode(9, 10, 8, 0);
j_j205 39:1e26cc57c8b7 75 r5map.addGraphNode(10, 9, 8, 180);
j_j205 39:1e26cc57c8b7 76 r5map.addGraphNode(10, 11, 4, 0);
j_j205 39:1e26cc57c8b7 77 r5map.addGraphNode(11, 10, 4, 180);
j_j205 39:1e26cc57c8b7 78 r5map.addGraphNode(12, 2, 15, 270);
j_j205 39:1e26cc57c8b7 79 r5map.addGraphNode(12, 13, 8, 180);
j_j205 39:1e26cc57c8b7 80 r5map.addGraphNode(12, 14, 6, 0);
j_j205 39:1e26cc57c8b7 81 r5map.addGraphNode(13, 12, 8, 0);
j_j205 39:1e26cc57c8b7 82 r5map.addGraphNode(14, 12, 6, 180);
j_j205 39:1e26cc57c8b7 83 r5map.addGraphNode(14, 15, 7, 0);
j_j205 39:1e26cc57c8b7 84 r5map.addGraphNode(15, 14, 7, 180);
j_j205 39:1e26cc57c8b7 85 r5map.addGraphNode(15, 16, 7, 0);
j_j205 39:1e26cc57c8b7 86 r5map.addGraphNode(16, 15, 7, 180);
j_j205 39:1e26cc57c8b7 87 r5map.addGraphNode(16, 17, 3, 0);
j_j205 39:1e26cc57c8b7 88 r5map.addGraphNode(17, 16, 3, 180);
j_j205 39:1e26cc57c8b7 89 r5map.addGraphNode(17, 18, 8, 0);
j_j205 39:1e26cc57c8b7 90 r5map.addGraphNode(17, 30, 12, 90);
j_j205 39:1e26cc57c8b7 91 r5map.addGraphNode(18, 17, 8, 180);
j_j205 39:1e26cc57c8b7 92 r5map.addGraphNode(18, 19, 8, 0);
j_j205 39:1e26cc57c8b7 93 r5map.addGraphNode(19, 18, 8, 180);
j_j205 39:1e26cc57c8b7 94 r5map.addGraphNode(19, 20, 7, 0);
j_j205 39:1e26cc57c8b7 95 r5map.addGraphNode(20, 19, 7, 180);
j_j205 39:1e26cc57c8b7 96 r5map.addGraphNode(20, 21, 7, 0);
j_j205 39:1e26cc57c8b7 97 r5map.addGraphNode(21, 20, 7, 180);
j_j205 39:1e26cc57c8b7 98 r5map.addGraphNode(21, 22, 7, 0);
j_j205 39:1e26cc57c8b7 99 r5map.addGraphNode(22, 21, 7, 180);
j_j205 39:1e26cc57c8b7 100 r5map.addGraphNode(22, 23, 6, 0);
j_j205 39:1e26cc57c8b7 101 r5map.addGraphNode(23, 22, 6, 180);
j_j205 39:1e26cc57c8b7 102 r5map.addGraphNode(24, 22, 12, 270);
j_j205 39:1e26cc57c8b7 103 r5map.addGraphNode(24, 25, 6, 180);
j_j205 39:1e26cc57c8b7 104 r5map.addGraphNode(24, 42, 12, 90);
j_j205 39:1e26cc57c8b7 105 r5map.addGraphNode(25, 24, 6, 0);
j_j205 39:1e26cc57c8b7 106 r5map.addGraphNode(25, 26, 8, 180);
j_j205 39:1e26cc57c8b7 107 r5map.addGraphNode(26, 25, 7, 0);
j_j205 39:1e26cc57c8b7 108 r5map.addGraphNode(26, 27, 7, 180);
j_j205 39:1e26cc57c8b7 109 r5map.addGraphNode(27, 26, 7, 0);
j_j205 39:1e26cc57c8b7 110 r5map.addGraphNode(27, 28, 7, 180);
j_j205 39:1e26cc57c8b7 111 r5map.addGraphNode(28, 27, 7, 0);
j_j205 39:1e26cc57c8b7 112 r5map.addGraphNode(28, 29, 4, 180);
j_j205 39:1e26cc57c8b7 113 r5map.addGraphNode(29, 28, 4, 0);
j_j205 39:1e26cc57c8b7 114 r5map.addGraphNode(29, 30, 7, 180);
j_j205 39:1e26cc57c8b7 115 r5map.addGraphNode(30, 18, 12, 270);
j_j205 39:1e26cc57c8b7 116 r5map.addGraphNode(30, 29, 6, 0);
j_j205 39:1e26cc57c8b7 117 r5map.addGraphNode(30, 31, 6, 180);
j_j205 39:1e26cc57c8b7 118 r5map.addGraphNode(30, 36, 12, 90);
j_j205 39:1e26cc57c8b7 119 r5map.addGraphNode(31, 30, 6, 0);
j_j205 39:1e26cc57c8b7 120 r5map.addGraphNode(31, 32, 7, 180);
j_j205 39:1e26cc57c8b7 121 r5map.addGraphNode(32, 31, 7, 0);
j_j205 39:1e26cc57c8b7 122 r5map.addGraphNode(32, 33, 7, 180);
j_j205 39:1e26cc57c8b7 123 r5map.addGraphNode(33, 32, 7, 0);
j_j205 39:1e26cc57c8b7 124 r5map.addGraphNode(33, 34, 7, 180);
j_j205 39:1e26cc57c8b7 125 r5map.addGraphNode(34, 33, 7, 0);
j_j205 39:1e26cc57c8b7 126 r5map.addGraphNode(34, 35, 2, 180);
j_j205 39:1e26cc57c8b7 127 r5map.addGraphNode(35, 34, 2, 0);
j_j205 39:1e26cc57c8b7 128 r5map.addGraphNode(36, 30, 12, 270);
j_j205 39:1e26cc57c8b7 129 r5map.addGraphNode(36, 37, 6, 180);
j_j205 39:1e26cc57c8b7 130 r5map.addGraphNode(37, 36, 7, 0);
j_j205 39:1e26cc57c8b7 131 r5map.addGraphNode(37, 38, 7, 180);
j_j205 39:1e26cc57c8b7 132 r5map.addGraphNode(38, 37, 7, 0);
j_j205 39:1e26cc57c8b7 133 r5map.addGraphNode(38, 39, 7, 180);
j_j205 39:1e26cc57c8b7 134 r5map.addGraphNode(39, 38, 7, 0);
j_j205 39:1e26cc57c8b7 135 r5map.addGraphNode(39, 40, 7, 180);
j_j205 39:1e26cc57c8b7 136 r5map.addGraphNode(40, 39, 7, 0);
j_j205 39:1e26cc57c8b7 137 r5map.addGraphNode(40, 41, 2, 180);
j_j205 39:1e26cc57c8b7 138 r5map.addGraphNode(41, 40, 2, 0);
j_j205 39:1e26cc57c8b7 139 r5map.addGraphNode(42, 24, 12, 270);
j_j205 39:1e26cc57c8b7 140 r5map.addGraphNode(42, 43, 6, 0);
j_j205 39:1e26cc57c8b7 141 r5map.addGraphNode(43, 42, 6, 180);
j_j205 39:1e26cc57c8b7 142 r5map.addGraphNode(43, 44, 6, 0);
j_j205 39:1e26cc57c8b7 143 r5map.addGraphNode(44, 43, 6, 180);
j_j205 39:1e26cc57c8b7 144 r5map.addGraphNode(44, 45, 7, 90);
j_j205 39:1e26cc57c8b7 145 r5map.addGraphNode(45, 44, 7, 270);
j_j205 39:1e26cc57c8b7 146 r5map.addGraphNode(45, 46, 7, 90);
j_j205 39:1e26cc57c8b7 147 r5map.addGraphNode(46, 45, 7, 270);
j_j205 39:1e26cc57c8b7 148 r5map.addGraphNode(46, 47, 7, 90);
j_j205 39:1e26cc57c8b7 149 r5map.addGraphNode(47, 46, 7, 270);
j_j205 39:1e26cc57c8b7 150 r5map.addGraphNode(47, 48, 8, 90);
j_j205 39:1e26cc57c8b7 151 r5map.addGraphNode(48, 47, 8, 270);
j_j205 39:1e26cc57c8b7 152 r5map.addGraphNode(48, 49, 8, 90);
j_j205 39:1e26cc57c8b7 153 r5map.addGraphNode(49, 48, 8, 270);
j_j205 39:1e26cc57c8b7 154 r5map.addGraphNode(49, 50, 8, 90);
j_j205 39:1e26cc57c8b7 155 r5map.addGraphNode(50, 49, 8, 270);
j_j205 39:1e26cc57c8b7 156 r5map.addGraphNode(50, 51, 4, 180);
j_j205 39:1e26cc57c8b7 157 r5map.addGraphNode(51, 50, 4, 0);
j_j205 39:1e26cc57c8b7 158 r5map.addGraphNode(51, 52, 10, 180);
j_j205 39:1e26cc57c8b7 159 r5map.addGraphNode(52, 51, 10, 0);
j_j205 39:1e26cc57c8b7 160 r5map.addGraphNode(52, 53, 10, 180);
j_j205 39:1e26cc57c8b7 161 r5map.addGraphNode(53, 52, 10, 0);
j_j205 39:1e26cc57c8b7 162 r5map.addGraphNode(53, 54, 10, 180);
j_j205 39:1e26cc57c8b7 163 r5map.addGraphNode(54, 53, 10, 0);
j_j205 39:1e26cc57c8b7 164 r5map.addGraphNode(54, 55, 10, 180);
j_j205 39:1e26cc57c8b7 165 r5map.addGraphNode(55, 54, 10, 0);
j_j205 39:1e26cc57c8b7 166 r5map.addGraphNode(55, 56, 10, 180);
j_j205 39:1e26cc57c8b7 167 r5map.addGraphNode(56, 55, 10, 0);
j_j205 39:1e26cc57c8b7 168 r5map.addGraphNode(56, 57, 10, 180);
j_j205 39:1e26cc57c8b7 169 r5map.addGraphNode(57, 56, 10, 0);
j_j205 39:1e26cc57c8b7 170 r5map.addGraphNode(57, 55, 10, 180);
j_j205 39:1e26cc57c8b7 171 r5map.addGraphNode(58, 57, 10, 0);
j_j205 39:1e26cc57c8b7 172 r5map.addGraphNode(58, 59, 10, 180);
j_j205 39:1e26cc57c8b7 173 r5map.addGraphNode(59, 58, 10, 0);
j_j205 39:1e26cc57c8b7 174 r5map.addGraphNode(59, 60, 2, 270);
j_j205 39:1e26cc57c8b7 175 r5map.addGraphNode(60, 59, 2, 90);
j_j205 39:1e26cc57c8b7 176
j_j205 39:1e26cc57c8b7 177 // gripper.lift();
j_j205 39:1e26cc57c8b7 178 // gripper.release();
j_j205 39:1e26cc57c8b7 179
j_j205 39:1e26cc57c8b7 180 const uint8_t V1 = 23;
j_j205 39:1e26cc57c8b7 181 const uint8_t V2 = 35;
j_j205 39:1e26cc57c8b7 182 const uint8_t V3 = 41;
j_j205 39:1e26cc57c8b7 183 const uint8_t V4 = 47;
j_j205 39:1e26cc57c8b7 184 const uint8_t V5 = 51;
j_j205 39:1e26cc57c8b7 185 const uint8_t V6 = 60;
j_j205 39:1e26cc57c8b7 186 const uint8_t YELLOW_DROP_ZONE = 13;
j_j205 39:1e26cc57c8b7 187 const uint8_t RED_DROP_ZONE = 11;
j_j205 39:1e26cc57c8b7 188
j_j205 39:1e26cc57c8b7 189 pc.printf("\nWaiting for START BUTTON\n");
j_j205 39:1e26cc57c8b7 190 bluetooth.printf("\nWaiting for START BUTTON\n");
j_j205 39:1e26cc57c8b7 191 while(!active) // wait for start_button
j_j205 39:1e26cc57c8b7 192 {
j_j205 39:1e26cc57c8b7 193 wait(1e-6);
j_j205 39:1e26cc57c8b7 194 }
j_j205 39:1e26cc57c8b7 195
j_j205 39:1e26cc57c8b7 196 int target = V1;
j_j205 39:1e26cc57c8b7 197 r5map.getShortestPath(target);
j_j205 39:1e26cc57c8b7 198 pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V1) );
j_j205 39:1e26cc57c8b7 199 bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V1) );
j_j205 39:1e26cc57c8b7 200 r5map.executeRoute();
j_j205 39:1e26cc57c8b7 201 wait(0.1);
j_j205 39:1e26cc57c8b7 202
j_j205 39:1e26cc57c8b7 203 // gripper.lower();
j_j205 39:1e26cc57c8b7 204 // gripper.grip();
j_j205 39:1e26cc57c8b7 205 // gripper.lift();
j_j205 39:1e26cc57c8b7 206
j_j205 39:1e26cc57c8b7 207 /* back up to safe turn-around location */
j_j205 39:1e26cc57c8b7 208 pc.printf("\nBack up to safe turn around location");
j_j205 39:1e26cc57c8b7 209 bluetooth.printf("\nBack up to safe turn around location");
j_j205 39:1e26cc57c8b7 210 reverseMove(drive, 6.0, 0);
j_j205 39:1e26cc57c8b7 211 while(!drive.isMoveDone())
j_j205 39:1e26cc57c8b7 212 {
j_j205 39:1e26cc57c8b7 213 wait(1e-6);
j_j205 39:1e26cc57c8b7 214 }
j_j205 39:1e26cc57c8b7 215 r5map.setVertex(22);
j_j205 39:1e26cc57c8b7 216
j_j205 39:1e26cc57c8b7 217 target = YELLOW_DROP_ZONE;
j_j205 39:1e26cc57c8b7 218 r5map.getShortestPath(target);
j_j205 39:1e26cc57c8b7 219 pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(YELLOW_DROP_ZONE) );
j_j205 39:1e26cc57c8b7 220 bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(YELLOW_DROP_ZONE) );
j_j205 39:1e26cc57c8b7 221 r5map.executeRoute();
j_j205 39:1e26cc57c8b7 222 wait(0.1);
j_j205 39:1e26cc57c8b7 223
j_j205 39:1e26cc57c8b7 224 /* back up to safe turn-around location */
j_j205 39:1e26cc57c8b7 225 pc.printf("\nBack up to a safe turn around location");
j_j205 39:1e26cc57c8b7 226 bluetooth.printf("\nBack up to a safe turn around location");
j_j205 39:1e26cc57c8b7 227 r5map.localizeLeftReverse();
j_j205 39:1e26cc57c8b7 228 reverseMove(drive, 6.0, 0);
j_j205 39:1e26cc57c8b7 229 // wait for move to complete
j_j205 39:1e26cc57c8b7 230 while(!drive.isMoveDone())
j_j205 39:1e26cc57c8b7 231 {
j_j205 39:1e26cc57c8b7 232 wait(1e-6);
j_j205 39:1e26cc57c8b7 233 }
j_j205 39:1e26cc57c8b7 234 r5map.setVertex(12);
j_j205 39:1e26cc57c8b7 235 r5map.localizeLeftReverse();
j_j205 39:1e26cc57c8b7 236
j_j205 39:1e26cc57c8b7 237 target = V2;
j_j205 39:1e26cc57c8b7 238 r5map.getShortestPath(target);
j_j205 39:1e26cc57c8b7 239 pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V2) );
j_j205 39:1e26cc57c8b7 240 bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V2) );
j_j205 39:1e26cc57c8b7 241 r5map.executeRoute();
j_j205 39:1e26cc57c8b7 242 wait(0.1);
j_j205 39:1e26cc57c8b7 243
j_j205 39:1e26cc57c8b7 244 /* back up to safe turn-around location */
j_j205 39:1e26cc57c8b7 245 pc.printf("\nBack up to a safe turn around location");
j_j205 39:1e26cc57c8b7 246 bluetooth.printf("\nBack up to a safe turn around location");
j_j205 39:1e26cc57c8b7 247 r5map.localizeLeftReverse();
j_j205 39:1e26cc57c8b7 248 reverseMove(drive, 2.0, 0);
j_j205 39:1e26cc57c8b7 249 // wait for move to complete
j_j205 39:1e26cc57c8b7 250 while(!drive.isMoveDone())
j_j205 39:1e26cc57c8b7 251 {
j_j205 39:1e26cc57c8b7 252 wait(1e-6);
j_j205 39:1e26cc57c8b7 253 }
j_j205 39:1e26cc57c8b7 254 r5map.setVertex(34);
j_j205 39:1e26cc57c8b7 255 r5map.localizeLeftReverse();
j_j205 39:1e26cc57c8b7 256 reverseMove(drive, 7.0, 0);
j_j205 39:1e26cc57c8b7 257 // wait for move to complete
j_j205 39:1e26cc57c8b7 258 while(!drive.isMoveDone())
j_j205 39:1e26cc57c8b7 259 {
j_j205 39:1e26cc57c8b7 260 wait(1e-6);
j_j205 39:1e26cc57c8b7 261 }
j_j205 39:1e26cc57c8b7 262 r5map.setVertex(33);
j_j205 39:1e26cc57c8b7 263 r5map.localizeLeftReverse();
j_j205 39:1e26cc57c8b7 264 reverseMove(drive, 7.0, 0);
j_j205 39:1e26cc57c8b7 265 // wait for move to complete
j_j205 39:1e26cc57c8b7 266 while(!drive.isMoveDone())
j_j205 39:1e26cc57c8b7 267 {
j_j205 39:1e26cc57c8b7 268 wait(1e-6);
j_j205 39:1e26cc57c8b7 269 }
j_j205 39:1e26cc57c8b7 270 r5map.setVertex(32);
j_j205 39:1e26cc57c8b7 271 r5map.localizeLeftReverse();
j_j205 39:1e26cc57c8b7 272 reverseMove(drive, 7.0, 0);
j_j205 39:1e26cc57c8b7 273 // wait for move to complete
j_j205 39:1e26cc57c8b7 274 while(!drive.isMoveDone())
j_j205 39:1e26cc57c8b7 275 {
j_j205 39:1e26cc57c8b7 276 wait(1e-6);
j_j205 39:1e26cc57c8b7 277 }
j_j205 39:1e26cc57c8b7 278 r5map.setVertex(31);
j_j205 39:1e26cc57c8b7 279 r5map.localizeLeftReverse();
j_j205 39:1e26cc57c8b7 280 reverseMove(drive, 6.0, 0);
j_j205 39:1e26cc57c8b7 281 // wait for move to complete
j_j205 39:1e26cc57c8b7 282 while(!drive.isMoveDone())
j_j205 39:1e26cc57c8b7 283 {
j_j205 39:1e26cc57c8b7 284 wait(1e-6);
j_j205 39:1e26cc57c8b7 285 }
j_j205 39:1e26cc57c8b7 286 r5map.setVertex(30);
j_j205 39:1e26cc57c8b7 287 r5map.localizeLeftReverse();
j_j205 39:1e26cc57c8b7 288
j_j205 39:1e26cc57c8b7 289
j_j205 39:1e26cc57c8b7 290 target = RED_DROP_ZONE;
j_j205 39:1e26cc57c8b7 291 r5map.getShortestPath(target);
j_j205 39:1e26cc57c8b7 292 pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(RED_DROP_ZONE) );
j_j205 39:1e26cc57c8b7 293 bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(RED_DROP_ZONE) );
j_j205 39:1e26cc57c8b7 294 r5map.executeRoute();
j_j205 39:1e26cc57c8b7 295 wait(0.1);
j_j205 39:1e26cc57c8b7 296
j_j205 39:1e26cc57c8b7 297 /* back up to safe turn around location
j_j205 39:1e26cc57c8b7 298 pc.printf("\nBack up to a safe turn around location");
j_j205 39:1e26cc57c8b7 299 bluetooth.printf("\nBack up to a safe turn around location");
j_j205 39:1e26cc57c8b7 300 r5map.localizeLeftReverse();
j_j205 39:1e26cc57c8b7 301 reverseMove(drive, 4.0, 0);
j_j205 39:1e26cc57c8b7 302 while(!drive.isMoveDone())
j_j205 39:1e26cc57c8b7 303 {
j_j205 39:1e26cc57c8b7 304 wait(1e-6);
j_j205 39:1e26cc57c8b7 305 }
j_j205 39:1e26cc57c8b7 306 r5map.setVertex(10);
j_j205 39:1e26cc57c8b7 307 r5map.localizeLeftReverse();
j_j205 39:1e26cc57c8b7 308 reverseMove(drive, 8.0, 0);
j_j205 39:1e26cc57c8b7 309 while(!drive.isMoveDone())
j_j205 39:1e26cc57c8b7 310 {
j_j205 39:1e26cc57c8b7 311 wait(1e-6);
j_j205 39:1e26cc57c8b7 312 }
j_j205 39:1e26cc57c8b7 313 r5map.setVertex(9);
j_j205 39:1e26cc57c8b7 314 r5map.localizeLeftReverse();
j_j205 39:1e26cc57c8b7 315 reverseMove(drive, 8.0, 0);
j_j205 39:1e26cc57c8b7 316 while(!drive.isMoveDone())
j_j205 39:1e26cc57c8b7 317 {
j_j205 39:1e26cc57c8b7 318 wait(1e-6);
j_j205 39:1e26cc57c8b7 319 }
j_j205 39:1e26cc57c8b7 320 r5map.setVertex(8);
j_j205 39:1e26cc57c8b7 321 r5map.localizeLeftReverse();
j_j205 39:1e26cc57c8b7 322 reverseMove(drive, 8.0, 0);
j_j205 39:1e26cc57c8b7 323 while(!drive.isMoveDone())
j_j205 39:1e26cc57c8b7 324 {
j_j205 39:1e26cc57c8b7 325 wait(1e-6);
j_j205 39:1e26cc57c8b7 326 }
j_j205 39:1e26cc57c8b7 327 r5map.setVertex(7);
j_j205 39:1e26cc57c8b7 328 r5map.localizeLeftReverse();
j_j205 39:1e26cc57c8b7 329 reverseMove(drive, 8.0, 0);
j_j205 39:1e26cc57c8b7 330 while(!drive.isMoveDone())
j_j205 39:1e26cc57c8b7 331 {
j_j205 39:1e26cc57c8b7 332 wait(1e-6);
j_j205 39:1e26cc57c8b7 333 }
j_j205 39:1e26cc57c8b7 334 r5map.setVertex(6);
j_j205 39:1e26cc57c8b7 335 r5map.localizeLeftReverse();
j_j205 39:1e26cc57c8b7 336 reverseMove(drive, 8.0, 0);
j_j205 39:1e26cc57c8b7 337 while(!drive.isMoveDone())
j_j205 39:1e26cc57c8b7 338 {
j_j205 39:1e26cc57c8b7 339 wait(1e-6);
j_j205 39:1e26cc57c8b7 340 }
j_j205 39:1e26cc57c8b7 341 r5map.setVertex(5);
j_j205 39:1e26cc57c8b7 342 r5map.localizeLeftReverse();
j_j205 39:1e26cc57c8b7 343 reverseMove(drive, 8.0, 0);
j_j205 39:1e26cc57c8b7 344 while(!drive.isMoveDone())
j_j205 39:1e26cc57c8b7 345 {
j_j205 39:1e26cc57c8b7 346 wait(1e-6);
j_j205 39:1e26cc57c8b7 347 }
j_j205 39:1e26cc57c8b7 348 r5map.setVertex(4);
j_j205 39:1e26cc57c8b7 349 r5map.localizeLeftReverse();
j_j205 39:1e26cc57c8b7 350 reverseMove(drive, 8.0, 0);
j_j205 39:1e26cc57c8b7 351 while(!drive.isMoveDone())
j_j205 39:1e26cc57c8b7 352 {
j_j205 39:1e26cc57c8b7 353 wait(1e-6);
j_j205 39:1e26cc57c8b7 354 }
j_j205 39:1e26cc57c8b7 355 r5map.setVertex(3);
j_j205 39:1e26cc57c8b7 356 r5map.localizeLeftReverse();
j_j205 39:1e26cc57c8b7 357 reverseMove(drive, 8.0, 0);
j_j205 39:1e26cc57c8b7 358 while(!drive.isMoveDone())
j_j205 39:1e26cc57c8b7 359 {
j_j205 39:1e26cc57c8b7 360 wait(1e-6);
j_j205 39:1e26cc57c8b7 361 }
j_j205 39:1e26cc57c8b7 362 r5map.setVertex(2);
j_j205 39:1e26cc57c8b7 363 r5map.localizeLeftReverse();*/
j_j205 39:1e26cc57c8b7 364
j_j205 39:1e26cc57c8b7 365 target = V3;
j_j205 39:1e26cc57c8b7 366 r5map.getShortestPath(target);
j_j205 39:1e26cc57c8b7 367 pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V3) );
j_j205 39:1e26cc57c8b7 368 bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V3) );
j_j205 39:1e26cc57c8b7 369 r5map.executeRoute();
j_j205 39:1e26cc57c8b7 370 wait(0.1);
j_j205 39:1e26cc57c8b7 371
j_j205 39:1e26cc57c8b7 372 target = RED_DROP_ZONE;
j_j205 39:1e26cc57c8b7 373 r5map.getShortestPath(target);
j_j205 39:1e26cc57c8b7 374 pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(RED_DROP_ZONE) );
j_j205 39:1e26cc57c8b7 375 bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(RED_DROP_ZONE) );
j_j205 39:1e26cc57c8b7 376 r5map.executeRoute();
j_j205 39:1e26cc57c8b7 377 wait(0.1);
j_j205 39:1e26cc57c8b7 378
j_j205 39:1e26cc57c8b7 379 target = V4;
j_j205 39:1e26cc57c8b7 380 r5map.getShortestPath(target);
j_j205 39:1e26cc57c8b7 381 pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V4) );
j_j205 39:1e26cc57c8b7 382 bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V4) );
j_j205 39:1e26cc57c8b7 383 r5map.executeRoute();
j_j205 39:1e26cc57c8b7 384 wait(0.1);
j_j205 39:1e26cc57c8b7 385
j_j205 39:1e26cc57c8b7 386 target = YELLOW_DROP_ZONE;
j_j205 39:1e26cc57c8b7 387 r5map.getShortestPath(target);
j_j205 39:1e26cc57c8b7 388 pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(YELLOW_DROP_ZONE) );
j_j205 39:1e26cc57c8b7 389 bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(YELLOW_DROP_ZONE) );
j_j205 39:1e26cc57c8b7 390 r5map.executeRoute();
j_j205 39:1e26cc57c8b7 391 wait(0.1);
j_j205 39:1e26cc57c8b7 392
j_j205 39:1e26cc57c8b7 393 target = 0;
j_j205 39:1e26cc57c8b7 394 r5map.getShortestPath(target);
j_j205 39:1e26cc57c8b7 395 pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(0) );
j_j205 39:1e26cc57c8b7 396 bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(0) );
j_j205 39:1e26cc57c8b7 397 r5map.executeRoute();
j_j205 39:1e26cc57c8b7 398 wait(0.1);
j_j205 39:1e26cc57c8b7 399
j_j205 39:1e26cc57c8b7 400 pc.printf("\nExercise Complete");
j_j205 39:1e26cc57c8b7 401 bluetooth.printf("\nExercise Complete");
j_j205 39:1e26cc57c8b7 402 } // end of main
j_j205 39:1e26cc57c8b7 403
j_j205 39:1e26cc57c8b7 404 // FUNCTION:
j_j205 39:1e26cc57c8b7 405 // void reverseMove(float dist, float angle)
j_j205 39:1e26cc57c8b7 406 // IN-PARAMETERS:
j_j205 39:1e26cc57c8b7 407 // dist (float), angle(float)
j_j205 39:1e26cc57c8b7 408 // OUT-PARAMETERS:
j_j205 39:1e26cc57c8b7 409 // None
j_j205 39:1e26cc57c8b7 410 // DESCRIPTION:
j_j205 39:1e26cc57c8b7 411 // Inverts values of invertLeft and invertRight and sends move
j_j205 39:1e26cc57c8b7 412 // command to move robot in reverse direction.
j_j205 39:1e26cc57c8b7 413 void reverseMove(StepperDrive &drive, float dist, float angle)
j_j205 39:1e26cc57c8b7 414 {
j_j205 39:1e26cc57c8b7 415 // swap values for invertLeft and invertRight
j_j205 39:1e26cc57c8b7 416 drive.setInvertLeft(!drive.getInvertLeft());
j_j205 39:1e26cc57c8b7 417 drive.setInvertRight(!drive.getInvertRight());
j_j205 39:1e26cc57c8b7 418
j_j205 39:1e26cc57c8b7 419 drive.move(dist, angle);
j_j205 39:1e26cc57c8b7 420 // wait for move to complete
j_j205 39:1e26cc57c8b7 421 while(!drive.isMoveDone())
j_j205 39:1e26cc57c8b7 422 {
j_j205 39:1e26cc57c8b7 423 wait(1e-6);
j_j205 39:1e26cc57c8b7 424 }
j_j205 39:1e26cc57c8b7 425
j_j205 39:1e26cc57c8b7 426 // restore original values for invertLeft and invertRight
j_j205 39:1e26cc57c8b7 427 drive.setInvertLeft(!drive.getInvertLeft());
j_j205 39:1e26cc57c8b7 428 drive.setInvertRight(!drive.getInvertRight());
j_j205 39:1e26cc57c8b7 429 }