![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
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@41:029135230ebf, 2016-04-08 (annotated)
- Committer:
- j_j205
- Date:
- Fri Apr 08 04:36:00 2016 +0000
- Revision:
- 41:029135230ebf
- Parent:
- 40:2d33bb4d6d6f
- Child:
- 42:1284491771ff
latest
Who changed what in which revision?
User | Revision | Line number | New 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 | 39:1e26cc57c8b7 | 121 | r5map.addGraphNode(30, 18, 12, 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 | } |