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@43:048c307bf8ac, 2016-04-09 (annotated)

Committer:
j_j205
Date:
Sat Apr 09 00:37:08 2016 +0000
Revision:
43:048c307bf8ac
Parent:
42:1284491771ff
Child:
44:d4207182bfc2
4/8/16 7:36pm jj

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 41:029135230ebf 41 8.1875, 700); // 8.4800
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 40:2d33bb4d6d6f 55 r5map.addGraphNode(0, 1, 6.75, 0);
j_j205 40:2d33bb4d6d6f 56 r5map.addGraphNode(1, 0, 6.75, 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 40:2d33bb4d6d6f 60 r5map.addGraphNode(2, 12, 14.75, 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 40:2d33bb4d6d6f 78 r5map.addGraphNode(12, 2, 14.75, 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 43:048c307bf8ac 115 r5map.addGraphNode(30, 17, 9, 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 40:2d33bb4d6d6f 129 r5map.addGraphNode(36, 37, 7, 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 40:2d33bb4d6d6f 196 int target;
j_j205 40:2d33bb4d6d6f 197 target = V1;
j_j205 39:1e26cc57c8b7 198 r5map.getShortestPath(target);
j_j205 39:1e26cc57c8b7 199 pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V1) );
j_j205 39:1e26cc57c8b7 200 bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V1) );
j_j205 39:1e26cc57c8b7 201 r5map.executeRoute();
j_j205 39:1e26cc57c8b7 202 wait(0.1);
j_j205 39:1e26cc57c8b7 203
j_j205 40:2d33bb4d6d6f 204 // call to hunt
j_j205 39:1e26cc57c8b7 205
j_j205 40:2d33bb4d6d6f 206 // back up to safe turn-around location
j_j205 39:1e26cc57c8b7 207 pc.printf("\nBack up to safe turn around location");
j_j205 39:1e26cc57c8b7 208 bluetooth.printf("\nBack up to safe turn around location");
j_j205 43:048c307bf8ac 209 reverseMove(drive, 4.5, 0);
j_j205 39:1e26cc57c8b7 210 while(!drive.isMoveDone())
j_j205 39:1e26cc57c8b7 211 {
j_j205 39:1e26cc57c8b7 212 wait(1e-6);
j_j205 39:1e26cc57c8b7 213 }
j_j205 39:1e26cc57c8b7 214 r5map.setVertex(22);
j_j205 39:1e26cc57c8b7 215
j_j205 39:1e26cc57c8b7 216 target = YELLOW_DROP_ZONE;
j_j205 39:1e26cc57c8b7 217 r5map.getShortestPath(target);
j_j205 39:1e26cc57c8b7 218 pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(YELLOW_DROP_ZONE) );
j_j205 39:1e26cc57c8b7 219 bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(YELLOW_DROP_ZONE) );
j_j205 39:1e26cc57c8b7 220 r5map.executeRoute();
j_j205 39:1e26cc57c8b7 221 wait(0.1);
j_j205 39:1e26cc57c8b7 222
j_j205 40:2d33bb4d6d6f 223 // back up to safe turn-around location
j_j205 39:1e26cc57c8b7 224 pc.printf("\nBack up to a safe turn around location");
j_j205 39:1e26cc57c8b7 225 bluetooth.printf("\nBack up to a safe turn around location");
j_j205 39:1e26cc57c8b7 226 r5map.localizeLeftReverse();
j_j205 41:029135230ebf 227 reverseMove(drive, 8.0, 0);
j_j205 39:1e26cc57c8b7 228 // wait for move to complete
j_j205 39:1e26cc57c8b7 229 while(!drive.isMoveDone())
j_j205 39:1e26cc57c8b7 230 {
j_j205 39:1e26cc57c8b7 231 wait(1e-6);
j_j205 39:1e26cc57c8b7 232 }
j_j205 39:1e26cc57c8b7 233 r5map.setVertex(12);
j_j205 39:1e26cc57c8b7 234 r5map.localizeLeftReverse();
j_j205 39:1e26cc57c8b7 235
j_j205 40:2d33bb4d6d6f 236 // turn 180 degrees to left
j_j205 40:2d33bb4d6d6f 237 pc.printf("\nTurn left 180 degrees");
j_j205 40:2d33bb4d6d6f 238 bluetooth.printf("\nTurn left 180 degrees");
j_j205 41:029135230ebf 239 drive.move(0, -185.0*(3.14159/180));
j_j205 40:2d33bb4d6d6f 240 // wait for move to complete
j_j205 40:2d33bb4d6d6f 241 while(!drive.isMoveDone())
j_j205 40:2d33bb4d6d6f 242 {
j_j205 40:2d33bb4d6d6f 243 wait(1e-6);
j_j205 40:2d33bb4d6d6f 244 }
j_j205 40:2d33bb4d6d6f 245 r5map.setAngle(0);
j_j205 40:2d33bb4d6d6f 246
j_j205 40:2d33bb4d6d6f 247 // move to compensate for coming up short
j_j205 40:2d33bb4d6d6f 248 pc.printf("\nCompensation move forward 2.0");
j_j205 40:2d33bb4d6d6f 249 bluetooth.printf("\nCompensation move forward 2.0");
j_j205 40:2d33bb4d6d6f 250 drive.move(2.0, 0);
j_j205 40:2d33bb4d6d6f 251 // wait for move to complete
j_j205 40:2d33bb4d6d6f 252 while(!drive.isMoveDone())
j_j205 40:2d33bb4d6d6f 253 {
j_j205 40:2d33bb4d6d6f 254 wait(1e-6);
j_j205 40:2d33bb4d6d6f 255 }
j_j205 40:2d33bb4d6d6f 256
j_j205 39:1e26cc57c8b7 257 target = V2;
j_j205 39:1e26cc57c8b7 258 r5map.getShortestPath(target);
j_j205 39:1e26cc57c8b7 259 pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V2) );
j_j205 39:1e26cc57c8b7 260 bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V2) );
j_j205 39:1e26cc57c8b7 261 r5map.executeRoute();
j_j205 39:1e26cc57c8b7 262 wait(0.1);
j_j205 39:1e26cc57c8b7 263
j_j205 39:1e26cc57c8b7 264 /* back up to safe turn-around location */
j_j205 39:1e26cc57c8b7 265 pc.printf("\nBack up to a safe turn around location");
j_j205 39:1e26cc57c8b7 266 bluetooth.printf("\nBack up to a safe turn around location");
j_j205 43:048c307bf8ac 267 r5map.newLocalizeRightReverse();
j_j205 39:1e26cc57c8b7 268 reverseMove(drive, 2.0, 0);
j_j205 39:1e26cc57c8b7 269 // wait for move to complete
j_j205 39:1e26cc57c8b7 270 while(!drive.isMoveDone())
j_j205 39:1e26cc57c8b7 271 {
j_j205 39:1e26cc57c8b7 272 wait(1e-6);
j_j205 39:1e26cc57c8b7 273 }
j_j205 39:1e26cc57c8b7 274 r5map.setVertex(34);
j_j205 43:048c307bf8ac 275 r5map.newLocalizeRightReverse();
j_j205 39:1e26cc57c8b7 276 reverseMove(drive, 7.0, 0);
j_j205 39:1e26cc57c8b7 277 // wait for move to complete
j_j205 39:1e26cc57c8b7 278 while(!drive.isMoveDone())
j_j205 39:1e26cc57c8b7 279 {
j_j205 39:1e26cc57c8b7 280 wait(1e-6);
j_j205 39:1e26cc57c8b7 281 }
j_j205 39:1e26cc57c8b7 282 r5map.setVertex(33);
j_j205 43:048c307bf8ac 283 r5map.newLocalizeRightReverse();
j_j205 39:1e26cc57c8b7 284 reverseMove(drive, 7.0, 0);
j_j205 39:1e26cc57c8b7 285 // wait for move to complete
j_j205 39:1e26cc57c8b7 286 while(!drive.isMoveDone())
j_j205 39:1e26cc57c8b7 287 {
j_j205 39:1e26cc57c8b7 288 wait(1e-6);
j_j205 39:1e26cc57c8b7 289 }
j_j205 39:1e26cc57c8b7 290 r5map.setVertex(32);
j_j205 43:048c307bf8ac 291 r5map.newLocalizeRightReverse();
j_j205 39:1e26cc57c8b7 292 reverseMove(drive, 7.0, 0);
j_j205 39:1e26cc57c8b7 293 // wait for move to complete
j_j205 39:1e26cc57c8b7 294 while(!drive.isMoveDone())
j_j205 39:1e26cc57c8b7 295 {
j_j205 39:1e26cc57c8b7 296 wait(1e-6);
j_j205 39:1e26cc57c8b7 297 }
j_j205 39:1e26cc57c8b7 298 r5map.setVertex(31);
j_j205 43:048c307bf8ac 299 r5map.newLocalizeRightReverse();
j_j205 39:1e26cc57c8b7 300 reverseMove(drive, 6.0, 0);
j_j205 39:1e26cc57c8b7 301 // wait for move to complete
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(30);
j_j205 41:029135230ebf 307 r5map.newLocalizeLeftReverse();
j_j205 39:1e26cc57c8b7 308
j_j205 39:1e26cc57c8b7 309
j_j205 39:1e26cc57c8b7 310 target = RED_DROP_ZONE;
j_j205 39:1e26cc57c8b7 311 r5map.getShortestPath(target);
j_j205 39:1e26cc57c8b7 312 pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(RED_DROP_ZONE) );
j_j205 39:1e26cc57c8b7 313 bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(RED_DROP_ZONE) );
j_j205 39:1e26cc57c8b7 314 r5map.executeRoute();
j_j205 39:1e26cc57c8b7 315 wait(0.1);
j_j205 39:1e26cc57c8b7 316
j_j205 39:1e26cc57c8b7 317 target = V3;
j_j205 39:1e26cc57c8b7 318 r5map.getShortestPath(target);
j_j205 39:1e26cc57c8b7 319 pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V3) );
j_j205 39:1e26cc57c8b7 320 bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V3) );
j_j205 39:1e26cc57c8b7 321 r5map.executeRoute();
j_j205 39:1e26cc57c8b7 322 wait(0.1);
j_j205 39:1e26cc57c8b7 323
j_j205 39:1e26cc57c8b7 324 target = RED_DROP_ZONE;
j_j205 39:1e26cc57c8b7 325 r5map.getShortestPath(target);
j_j205 39:1e26cc57c8b7 326 pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(RED_DROP_ZONE) );
j_j205 39:1e26cc57c8b7 327 bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(RED_DROP_ZONE) );
j_j205 39:1e26cc57c8b7 328 r5map.executeRoute();
j_j205 39:1e26cc57c8b7 329 wait(0.1);
j_j205 39:1e26cc57c8b7 330
j_j205 39:1e26cc57c8b7 331 target = V4;
j_j205 39:1e26cc57c8b7 332 r5map.getShortestPath(target);
j_j205 39:1e26cc57c8b7 333 pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V4) );
j_j205 39:1e26cc57c8b7 334 bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V4) );
j_j205 39:1e26cc57c8b7 335 r5map.executeRoute();
j_j205 39:1e26cc57c8b7 336 wait(0.1);
j_j205 39:1e26cc57c8b7 337
j_j205 39:1e26cc57c8b7 338 target = YELLOW_DROP_ZONE;
j_j205 39:1e26cc57c8b7 339 r5map.getShortestPath(target);
j_j205 39:1e26cc57c8b7 340 pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(YELLOW_DROP_ZONE) );
j_j205 39:1e26cc57c8b7 341 bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(YELLOW_DROP_ZONE) );
j_j205 39:1e26cc57c8b7 342 r5map.executeRoute();
j_j205 39:1e26cc57c8b7 343 wait(0.1);
j_j205 39:1e26cc57c8b7 344
j_j205 39:1e26cc57c8b7 345 target = 0;
j_j205 39:1e26cc57c8b7 346 r5map.getShortestPath(target);
j_j205 39:1e26cc57c8b7 347 pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(0) );
j_j205 39:1e26cc57c8b7 348 bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(0) );
j_j205 39:1e26cc57c8b7 349 r5map.executeRoute();
j_j205 39:1e26cc57c8b7 350 wait(0.1);
j_j205 39:1e26cc57c8b7 351
j_j205 39:1e26cc57c8b7 352 pc.printf("\nExercise Complete");
j_j205 39:1e26cc57c8b7 353 bluetooth.printf("\nExercise Complete");
j_j205 39:1e26cc57c8b7 354 } // end of main
j_j205 39:1e26cc57c8b7 355
j_j205 39:1e26cc57c8b7 356 // FUNCTION:
j_j205 39:1e26cc57c8b7 357 // void reverseMove(float dist, float angle)
j_j205 39:1e26cc57c8b7 358 // IN-PARAMETERS:
j_j205 39:1e26cc57c8b7 359 // dist (float), angle(float)
j_j205 39:1e26cc57c8b7 360 // OUT-PARAMETERS:
j_j205 39:1e26cc57c8b7 361 // None
j_j205 39:1e26cc57c8b7 362 // DESCRIPTION:
j_j205 39:1e26cc57c8b7 363 // Inverts values of invertLeft and invertRight and sends move
j_j205 39:1e26cc57c8b7 364 // command to move robot in reverse direction.
j_j205 39:1e26cc57c8b7 365 void reverseMove(StepperDrive &drive, float dist, float angle)
j_j205 39:1e26cc57c8b7 366 {
j_j205 39:1e26cc57c8b7 367 // swap values for invertLeft and invertRight
j_j205 39:1e26cc57c8b7 368 drive.setInvertLeft(!drive.getInvertLeft());
j_j205 39:1e26cc57c8b7 369 drive.setInvertRight(!drive.getInvertRight());
j_j205 39:1e26cc57c8b7 370
j_j205 39:1e26cc57c8b7 371 drive.move(dist, angle);
j_j205 39:1e26cc57c8b7 372 // wait for move to complete
j_j205 39:1e26cc57c8b7 373 while(!drive.isMoveDone())
j_j205 39:1e26cc57c8b7 374 {
j_j205 39:1e26cc57c8b7 375 wait(1e-6);
j_j205 39:1e26cc57c8b7 376 }
j_j205 39:1e26cc57c8b7 377
j_j205 39:1e26cc57c8b7 378 // restore original values for invertLeft and invertRight
j_j205 39:1e26cc57c8b7 379 drive.setInvertLeft(!drive.getInvertLeft());
j_j205 39:1e26cc57c8b7 380 drive.setInvertRight(!drive.getInvertRight());
j_j205 43:048c307bf8ac 381 }