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@44:d4207182bfc2, 2016-04-09 (annotated)

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