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@45:9a7ddd922706, 2016-04-09 (annotated)

Committer:
Hypna
Date:
Sat Apr 09 11:01:18 2016 +0000
Revision:
45:9a7ddd922706
Parent:
44:d4207182bfc2 
Added some more debug

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