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@29:e77e8891d985, 2016-03-25 (annotated)
- Committer:
- j_j205
- Date:
- Fri Mar 25 19:51:18 2016 +0000
- Revision:
- 29:e77e8891d985
- Parent:
- 28:17c17157e728
- Child:
- 30:c07381708ffd
3/25/16 2:50 JJ
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
j_j205 | 8:90af8914ce03 | 1 | #include "navigation.h" |
j_j205 | 29:e77e8891d985 | 2 | #include "Gripper.h" |
j_j205 | 15:d8940756d5d5 | 3 | #include "scanner.h" |
j_j205 | 12:e9f878ced6e7 | 4 | #include "StepperDrive.h" |
j_j205 | 15:d8940756d5d5 | 5 | #include "VL6180x.h" |
j_j205 | 8:90af8914ce03 | 6 | #include "stdint.h" |
j_j205 | 2:3b162e764c9d | 7 | #include "mbed.h" |
j_j205 | 0:a3c39d3359ac | 8 | |
j_j205 | 0:a3c39d3359ac | 9 | Serial pc(USBTX, USBRX); |
j_j205 | 28:17c17157e728 | 10 | // Serial pc(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 | 12:e9f878ced6e7 | 15 | |
j_j205 | 12:e9f878ced6e7 | 16 | void activate(); |
j_j205 | 0:a3c39d3359ac | 17 | |
j_j205 | 0:a3c39d3359ac | 18 | int main() |
j_j205 | 0:a3c39d3359ac | 19 | { |
j_j205 | 0:a3c39d3359ac | 20 | pc.baud(115200); |
j_j205 | 28:17c17157e728 | 21 | // pc.baud(9600); /* interface via Bluetooth at 9600 */ |
j_j205 | 12:e9f878ced6e7 | 22 | start.mode(PullUp); /* Start/Stop button is active low needing PullUp */ |
j_j205 | 12:e9f878ced6e7 | 23 | start.fall(&activate); |
j_j205 | 20:53ab414d7b33 | 24 | |
j_j205 | 12:e9f878ced6e7 | 25 | led_red = 0; |
j_j205 | 12:e9f878ced6e7 | 26 | led_green = 1; |
j_j205 | 20:53ab414d7b33 | 27 | |
j_j205 | 25:670a59096bf8 | 28 | pc.printf("\nWaiting for START BUTTON\n"); |
j_j205 | 25:670a59096bf8 | 29 | while(!active) // wait for start_button |
j_j205 | 25:670a59096bf8 | 30 | { |
j_j205 | 25:670a59096bf8 | 31 | wait(1e-6); |
j_j205 | 25:670a59096bf8 | 32 | } |
j_j205 | 25:670a59096bf8 | 33 | |
j_j205 | 25:670a59096bf8 | 34 | StepperDrive drive(pc, PTE19, PTE18, 0, PTE3, PTE2, 1, 10.0625, 8.3750, 500); |
j_j205 | 25:670a59096bf8 | 35 | //(serial &, stepPinLeft, dirPinLeft, invertLeft, stepPinRight, dirPinRight, invertRight, wheelCircum, wheelSepar, periodUs) |
j_j205 | 25:670a59096bf8 | 36 | |
j_j205 | 25:670a59096bf8 | 37 | VL6180x shortRangeL(PTE0, PTE1, 0x52); // I2C1, 0x29 left-shifted by 1 bit |
j_j205 | 25:670a59096bf8 | 38 | VL6180x shortRangeR(PTC9, PTC8, 0x52); // I2C0, 0x29 left-shifted by 1 bit |
j_j205 | 26:d99d078921a0 | 39 | LongRangeSensor longRangeL(pc, PTB2); |
j_j205 | 26:d99d078921a0 | 40 | LongRangeSensor longRangeR(pc, PTB3); |
j_j205 | 25:670a59096bf8 | 41 | |
j_j205 | 25:670a59096bf8 | 42 | Scanner scanner(pc, PTB0, PTB1, shortRangeL, shortRangeR, longRangeL, longRangeR, 0.075); |
j_j205 | 25:670a59096bf8 | 43 | // (Serial &, PinName servoL, PinName servoR, shortRangeL, shortRangeR, longRangeL, longRangeR, period) |
j_j205 | 25:670a59096bf8 | 44 | |
j_j205 | 29:e77e8891d985 | 45 | Navigation r5nav(scanner, 15); |
j_j205 | 8:90af8914ce03 | 46 | wait(0.1); |
j_j205 | 29:e77e8891d985 | 47 | /* |
j_j205 | 25:670a59096bf8 | 48 | r5nav.addGraphNode(0, 1, 10.0625, 0); |
j_j205 | 25:670a59096bf8 | 49 | r5nav.addGraphNode(1, 0, 10.0625, 180); |
j_j205 | 25:670a59096bf8 | 50 | r5nav.addGraphNode(1, 2, 10.0625, 270); |
j_j205 | 25:670a59096bf8 | 51 | r5nav.addGraphNode(2, 1, 10.0625, 90); |
j_j205 | 25:670a59096bf8 | 52 | r5nav.addGraphNode(2, 3, 10.0625, 180); |
j_j205 | 25:670a59096bf8 | 53 | r5nav.addGraphNode(3, 2, 10.0625, 0); |
j_j205 | 25:670a59096bf8 | 54 | r5nav.addGraphNode(3, 0, 10.0625, 90); |
j_j205 | 25:670a59096bf8 | 55 | r5nav.addGraphNode(0, 3, 10.0625, 270); |
j_j205 | 29:e77e8891d985 | 56 | */ |
j_j205 | 29:e77e8891d985 | 57 | |
j_j205 | 20:53ab414d7b33 | 58 | // loading r5 map |
j_j205 | 8:90af8914ce03 | 59 | r5nav.addGraphNode(0, 1, 18, 0); |
j_j205 | 8:90af8914ce03 | 60 | r5nav.addGraphNode(1, 0, 18, 180); |
j_j205 | 12:e9f878ced6e7 | 61 | r5nav.addGraphNode(1, 2, 72, 0); |
j_j205 | 12:e9f878ced6e7 | 62 | r5nav.addGraphNode(1, 3, 13.5, 90); |
j_j205 | 8:90af8914ce03 | 63 | r5nav.addGraphNode(2, 1, 72, 180); |
j_j205 | 12:e9f878ced6e7 | 64 | r5nav.addGraphNode(3, 1, 13.5, 270); |
j_j205 | 8:90af8914ce03 | 65 | r5nav.addGraphNode(3, 4, 12, 180); |
j_j205 | 8:90af8914ce03 | 66 | r5nav.addGraphNode(3, 5, 23, 0); |
j_j205 | 8:90af8914ce03 | 67 | r5nav.addGraphNode(4, 3, 12, 0); |
j_j205 | 8:90af8914ce03 | 68 | r5nav.addGraphNode(5, 3, 23, 180); |
j_j205 | 8:90af8914ce03 | 69 | r5nav.addGraphNode(5, 6, 37, 0); |
j_j205 | 8:90af8914ce03 | 70 | r5nav.addGraphNode(5, 8, 12, 90); |
j_j205 | 8:90af8914ce03 | 71 | r5nav.addGraphNode(6, 5, 37, 180); |
j_j205 | 8:90af8914ce03 | 72 | r5nav.addGraphNode(6, 7, 12, 0); |
j_j205 | 8:90af8914ce03 | 73 | r5nav.addGraphNode(7, 6, 12, 180); |
j_j205 | 8:90af8914ce03 | 74 | r5nav.addGraphNode(8, 5, 12, 270); |
j_j205 | 8:90af8914ce03 | 75 | r5nav.addGraphNode(8, 9, 35, 180); |
j_j205 | 8:90af8914ce03 | 76 | r5nav.addGraphNode(9, 8, 35, 0); |
j_j205 | 28:17c17157e728 | 77 | r5nav.addGraphNode(8, 10, 37, 0); |
j_j205 | 28:17c17157e728 | 78 | r5nav.addGraphNode(6, 10, 12, 90); |
j_j205 | 28:17c17157e728 | 79 | r5nav.addGraphNode(10, 6, 12, 270); |
j_j205 | 28:17c17157e728 | 80 | r5nav.addGraphNode(8, 11, 12, 90); |
j_j205 | 28:17c17157e728 | 81 | r5nav.addGraphNode(11, 8, 12, 270); |
j_j205 | 28:17c17157e728 | 82 | r5nav.addGraphNode(11, 12, 35, 180); |
j_j205 | 28:17c17157e728 | 83 | r5nav.addGraphNode(12, 11, 35, 0); |
j_j205 | 28:17c17157e728 | 84 | r5nav.addGraphNode(11, 13, 37, 0); |
j_j205 | 28:17c17157e728 | 85 | r5nav.addGraphNode(13, 11, 37, 180); |
j_j205 | 28:17c17157e728 | 86 | r5nav.addGraphNode(13, 14, 12, 0); |
j_j205 | 28:17c17157e728 | 87 | r5nav.addGraphNode(14, 13, 12, 180); |
j_j205 | 28:17c17157e728 | 88 | |
j_j205 | 8:90af8914ce03 | 89 | const uint16_t YELLOW_VICTIM = 7; |
j_j205 | 8:90af8914ce03 | 90 | const uint16_t RED_VICTIM = 9; |
j_j205 | 8:90af8914ce03 | 91 | const uint16_t YELLOW_DROP_ZONE = 4; |
j_j205 | 8:90af8914ce03 | 92 | const uint16_t RED_DROP_ZONE = 2; |
j_j205 | 29:e77e8891d985 | 93 | |
j_j205 | 29:e77e8891d985 | 94 | /* |
j_j205 | 15:d8940756d5d5 | 95 | r5nav.getShortestPath(1); |
j_j205 | 15:d8940756d5d5 | 96 | pc.printf("\n\nDistance from 0 to 1: %i\n", r5nav.getMinDist(1) ); |
j_j205 | 15:d8940756d5d5 | 97 | pc.printf("Route:\n"); |
j_j205 | 15:d8940756d5d5 | 98 | r5nav.executeRoute(pc, drive); |
j_j205 | 15:d8940756d5d5 | 99 | wait(0.1); |
j_j205 | 20:53ab414d7b33 | 100 | |
j_j205 | 15:d8940756d5d5 | 101 | r5nav.getShortestPath(2); |
j_j205 | 15:d8940756d5d5 | 102 | pc.printf("\n\nDistance from 1 to 2: %i\n", r5nav.getMinDist(2) ); |
j_j205 | 15:d8940756d5d5 | 103 | pc.printf("Route:\n"); |
j_j205 | 15:d8940756d5d5 | 104 | r5nav.executeRoute(pc, drive); |
j_j205 | 15:d8940756d5d5 | 105 | wait(0.1); |
j_j205 | 20:53ab414d7b33 | 106 | |
j_j205 | 15:d8940756d5d5 | 107 | r5nav.getShortestPath(3); |
j_j205 | 15:d8940756d5d5 | 108 | pc.printf("\n\nDistance from 2 to 3: %i\n", r5nav.getMinDist(3) ); |
j_j205 | 8:90af8914ce03 | 109 | pc.printf("Route:\n"); |
j_j205 | 15:d8940756d5d5 | 110 | r5nav.executeRoute(pc, drive); |
j_j205 | 15:d8940756d5d5 | 111 | wait(0.1); |
j_j205 | 20:53ab414d7b33 | 112 | |
j_j205 | 15:d8940756d5d5 | 113 | r5nav.getShortestPath(0); |
j_j205 | 15:d8940756d5d5 | 114 | pc.printf("\n\nDistance from 3 to 0: %i\n", r5nav.getMinDist(0) ); |
j_j205 | 15:d8940756d5d5 | 115 | pc.printf("Route:\n"); |
j_j205 | 15:d8940756d5d5 | 116 | r5nav.executeRoute(pc, drive); |
j_j205 | 15:d8940756d5d5 | 117 | wait(0.1); |
j_j205 | 29:e77e8891d985 | 118 | */ |
j_j205 | 29:e77e8891d985 | 119 | |
j_j205 | 15:d8940756d5d5 | 120 | r5nav.getShortestPath(YELLOW_VICTIM); |
j_j205 | 15:d8940756d5d5 | 121 | pc.printf("\n\nDistance from 0 to YELLOW_VICTIM: %i\n", r5nav.getMinDist(YELLOW_VICTIM) ); |
j_j205 | 15:d8940756d5d5 | 122 | pc.printf("Route:\n"); |
j_j205 | 15:d8940756d5d5 | 123 | //r5nav.printRoute(pc); |
j_j205 | 15:d8940756d5d5 | 124 | //r5nav.setVertex(YELLOW_VICTIM) |
j_j205 | 12:e9f878ced6e7 | 125 | r5nav.executeRoute(pc, drive); |
j_j205 | 8:90af8914ce03 | 126 | wait(0.1); |
j_j205 | 1:1958e6db41fc | 127 | |
j_j205 | 8:90af8914ce03 | 128 | r5nav.getShortestPath(YELLOW_DROP_ZONE); |
j_j205 | 15:d8940756d5d5 | 129 | pc.printf("\n\nDistance from %i to YELLOW_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(YELLOW_DROP_ZONE) ); |
j_j205 | 8:90af8914ce03 | 130 | pc.printf("Route:\n"); |
j_j205 | 15:d8940756d5d5 | 131 | //r5nav.printRoute(pc); |
j_j205 | 15:d8940756d5d5 | 132 | //r5nav.setVertex(YELLOW_DROP_ZONE); |
j_j205 | 12:e9f878ced6e7 | 133 | r5nav.executeRoute(pc, drive); |
j_j205 | 8:90af8914ce03 | 134 | wait(0.1); |
j_j205 | 1:1958e6db41fc | 135 | |
j_j205 | 8:90af8914ce03 | 136 | r5nav.getShortestPath(RED_VICTIM); |
j_j205 | 15:d8940756d5d5 | 137 | pc.printf("\n\nDistance from %i to RED_VICTIM: %i\n", r5nav.getVertex(), r5nav.getMinDist(RED_VICTIM) ); |
j_j205 | 8:90af8914ce03 | 138 | pc.printf("Route:\n"); |
j_j205 | 15:d8940756d5d5 | 139 | //r5nav.printRoute(pc); |
j_j205 | 15:d8940756d5d5 | 140 | //r5nav.setVertex(RED_VICTIM); |
j_j205 | 12:e9f878ced6e7 | 141 | r5nav.executeRoute(pc, drive); |
j_j205 | 8:90af8914ce03 | 142 | wait(0.1); |
j_j205 | 1:1958e6db41fc | 143 | |
j_j205 | 8:90af8914ce03 | 144 | r5nav.getShortestPath(RED_DROP_ZONE); |
j_j205 | 15:d8940756d5d5 | 145 | pc.printf("\n\nDistance from %i to RED_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(RED_DROP_ZONE) ); |
j_j205 | 8:90af8914ce03 | 146 | pc.printf("Route:\n"); |
j_j205 | 15:d8940756d5d5 | 147 | //r5nav.printRoute(pc); |
j_j205 | 15:d8940756d5d5 | 148 | //r5nav.setVertex(RED_DROP_ZONE); |
j_j205 | 12:e9f878ced6e7 | 149 | r5nav.executeRoute(pc, drive); |
j_j205 | 8:90af8914ce03 | 150 | wait(0.1); |
j_j205 | 20:53ab414d7b33 | 151 | |
j_j205 | 29:e77e8891d985 | 152 | /* |
j_j205 | 15:d8940756d5d5 | 153 | drive.move(0, ((90.0)*(3.14159 / 180.0))); // return to initial angle |
j_j205 | 15:d8940756d5d5 | 154 | // wait for move to complete |
j_j205 | 15:d8940756d5d5 | 155 | while(!drive.isMoveDone()) |
j_j205 | 15:d8940756d5d5 | 156 | { |
j_j205 | 15:d8940756d5d5 | 157 | wait(1e-6); |
j_j205 | 15:d8940756d5d5 | 158 | } |
j_j205 | 29:e77e8891d985 | 159 | */ |
j_j205 | 15:d8940756d5d5 | 160 | pc.printf("\n\nExercise Complete"); |
j_j205 | 0:a3c39d3359ac | 161 | return 0; |
j_j205 | 12:e9f878ced6e7 | 162 | } |
j_j205 | 12:e9f878ced6e7 | 163 | |
j_j205 | 12:e9f878ced6e7 | 164 | void activate() |
j_j205 | 12:e9f878ced6e7 | 165 | { |
j_j205 | 12:e9f878ced6e7 | 166 | led_red = 1; |
j_j205 | 12:e9f878ced6e7 | 167 | led_green = 0; |
j_j205 | 12:e9f878ced6e7 | 168 | active = true; |
j_j205 | 25:670a59096bf8 | 169 | } |