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@42:1284491771ff, 2016-04-08 (annotated)

Committer:
j_j205
Date:
Fri Apr 08 05:00:33 2016 +0000
Revision:
42:1284491771ff
Parent:
41:029135230ebf
Child:
43:048c307bf8ac
latest

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