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@32:82bfece56f8f, 2016-03-26 (annotated)

Committer:
j_j205
Date:
Sat Mar 26 05:26:23 2016 +0000
Revision:
32:82bfece56f8f
Parent:
30:c07381708ffd
Child:
35:4e09da30bda2
3/26/16 12:25 am JJ

Who changed what in which revision?

UserRevisionLine numberNew 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 32:82bfece56f8f 5 #include "ShortRangeSensor.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 32:82bfece56f8f 34 StepperDrive drive(pc, PTE19, PTE18, 0, PTE3, PTE2, 1, 10.0625, 8.4800, 500); // 8.375
j_j205 25:670a59096bf8 35 //(serial &, stepPinLeft, dirPinLeft, invertLeft, stepPinRight, dirPinRight, invertRight, wheelCircum, wheelSepar, periodUs)
j_j205 25:670a59096bf8 36
j_j205 32:82bfece56f8f 37 ShortRangeSensor shortRangeL(PTE0, PTE1); // verify i2c pins
j_j205 32:82bfece56f8f 38 ShortRangeSensor shortRangeR(PTC9, PTC8); // verify i2c pins
j_j205 26:d99d078921a0 39 LongRangeSensor longRangeL(pc, PTB2);
j_j205 26:d99d078921a0 40 LongRangeSensor longRangeR(pc, PTB3);
j_j205 25:670a59096bf8 41
Hellkite85 30:c07381708ffd 42 Scanner scanner(pc, drive, 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 32:82bfece56f8f 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 32:82bfece56f8f 57
j_j205 20:53ab414d7b33 58 // loading r5 map
j_j205 32:82bfece56f8f 59 r5nav.addGraphNode(0, 1, 13, 0);
j_j205 32:82bfece56f8f 60 r5nav.addGraphNode(1, 0, 13, 180);
j_j205 32:82bfece56f8f 61 r5nav.addGraphNode(1, 2, 72, 356.5); // original angle = 0
j_j205 32:82bfece56f8f 62 r5nav.addGraphNode(1, 3, 15, 90);
j_j205 8:90af8914ce03 63 r5nav.addGraphNode(2, 1, 72, 180);
j_j205 32:82bfece56f8f 64 r5nav.addGraphNode(3, 1, 15, 270);
j_j205 8:90af8914ce03 65 r5nav.addGraphNode(3, 4, 12, 180);
j_j205 32:82bfece56f8f 66 r5nav.addGraphNode(3, 5, 23, 356.5); // (-5 degrees) original angle = 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 32:82bfece56f8f 70 r5nav.addGraphNode(5, 8, 12.5, 90);
j_j205 32:82bfece56f8f 71 r5nav.addGraphNode(6, 5, 37, 176.5); // original angle = 180
j_j205 8:90af8914ce03 72 r5nav.addGraphNode(6, 7, 12, 0);
j_j205 32:82bfece56f8f 73 r5nav.addGraphNode(6, 10, 12.5, 90);
j_j205 8:90af8914ce03 74 r5nav.addGraphNode(7, 6, 12, 180);
j_j205 32:82bfece56f8f 75 r5nav.addGraphNode(8, 5, 12.75, 270); // original distance = 12.5
j_j205 32:82bfece56f8f 76 r5nav.addGraphNode(8, 9, 35, 174.5); // original angle = 180
j_j205 28:17c17157e728 77 r5nav.addGraphNode(8, 10, 37, 0);
j_j205 28:17c17157e728 78 r5nav.addGraphNode(8, 11, 12, 90);
j_j205 32:82bfece56f8f 79 r5nav.addGraphNode(9, 8, 35, 0);
j_j205 32:82bfece56f8f 80 r5nav.addGraphNode(10, 6, 12.5, 270);
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(13, 14, 12, 0);
j_j205 28:17c17157e728 85 r5nav.addGraphNode(14, 13, 12, 180);
j_j205 32:82bfece56f8f 86
j_j205 8:90af8914ce03 87 const uint16_t YELLOW_VICTIM = 7;
j_j205 8:90af8914ce03 88 const uint16_t RED_VICTIM = 9;
j_j205 8:90af8914ce03 89 const uint16_t YELLOW_DROP_ZONE = 4;
j_j205 8:90af8914ce03 90 const uint16_t RED_DROP_ZONE = 2;
j_j205 32:82bfece56f8f 91
j_j205 29:e77e8891d985 92 /*
j_j205 15:d8940756d5d5 93 r5nav.getShortestPath(1);
j_j205 15:d8940756d5d5 94 pc.printf("\n\nDistance from 0 to 1: %i\n", r5nav.getMinDist(1) );
j_j205 15:d8940756d5d5 95 pc.printf("Route:\n");
j_j205 15:d8940756d5d5 96 r5nav.executeRoute(pc, drive);
j_j205 15:d8940756d5d5 97 wait(0.1);
j_j205 20:53ab414d7b33 98
j_j205 15:d8940756d5d5 99 r5nav.getShortestPath(2);
j_j205 15:d8940756d5d5 100 pc.printf("\n\nDistance from 1 to 2: %i\n", r5nav.getMinDist(2) );
j_j205 15:d8940756d5d5 101 pc.printf("Route:\n");
j_j205 15:d8940756d5d5 102 r5nav.executeRoute(pc, drive);
j_j205 15:d8940756d5d5 103 wait(0.1);
j_j205 20:53ab414d7b33 104
j_j205 15:d8940756d5d5 105 r5nav.getShortestPath(3);
j_j205 15:d8940756d5d5 106 pc.printf("\n\nDistance from 2 to 3: %i\n", r5nav.getMinDist(3) );
j_j205 8:90af8914ce03 107 pc.printf("Route:\n");
j_j205 15:d8940756d5d5 108 r5nav.executeRoute(pc, drive);
j_j205 15:d8940756d5d5 109 wait(0.1);
j_j205 20:53ab414d7b33 110
j_j205 15:d8940756d5d5 111 r5nav.getShortestPath(0);
j_j205 15:d8940756d5d5 112 pc.printf("\n\nDistance from 3 to 0: %i\n", r5nav.getMinDist(0) );
j_j205 15:d8940756d5d5 113 pc.printf("Route:\n");
j_j205 15:d8940756d5d5 114 r5nav.executeRoute(pc, drive);
j_j205 15:d8940756d5d5 115 wait(0.1);
j_j205 29:e77e8891d985 116 */
j_j205 32:82bfece56f8f 117
j_j205 15:d8940756d5d5 118 r5nav.getShortestPath(YELLOW_VICTIM);
j_j205 15:d8940756d5d5 119 pc.printf("\n\nDistance from 0 to YELLOW_VICTIM: %i\n", r5nav.getMinDist(YELLOW_VICTIM) );
j_j205 15:d8940756d5d5 120 pc.printf("Route:\n");
j_j205 15:d8940756d5d5 121 //r5nav.printRoute(pc);
j_j205 15:d8940756d5d5 122 //r5nav.setVertex(YELLOW_VICTIM)
j_j205 12:e9f878ced6e7 123 r5nav.executeRoute(pc, drive);
j_j205 8:90af8914ce03 124 wait(0.1);
j_j205 1:1958e6db41fc 125
j_j205 8:90af8914ce03 126 r5nav.getShortestPath(YELLOW_DROP_ZONE);
j_j205 15:d8940756d5d5 127 pc.printf("\n\nDistance from %i to YELLOW_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(YELLOW_DROP_ZONE) );
j_j205 8:90af8914ce03 128 pc.printf("Route:\n");
j_j205 15:d8940756d5d5 129 //r5nav.printRoute(pc);
j_j205 15:d8940756d5d5 130 //r5nav.setVertex(YELLOW_DROP_ZONE);
j_j205 12:e9f878ced6e7 131 r5nav.executeRoute(pc, drive);
j_j205 8:90af8914ce03 132 wait(0.1);
j_j205 1:1958e6db41fc 133
j_j205 8:90af8914ce03 134 r5nav.getShortestPath(RED_VICTIM);
j_j205 15:d8940756d5d5 135 pc.printf("\n\nDistance from %i to RED_VICTIM: %i\n", r5nav.getVertex(), r5nav.getMinDist(RED_VICTIM) );
j_j205 8:90af8914ce03 136 pc.printf("Route:\n");
j_j205 15:d8940756d5d5 137 //r5nav.printRoute(pc);
j_j205 15:d8940756d5d5 138 //r5nav.setVertex(RED_VICTIM);
j_j205 12:e9f878ced6e7 139 r5nav.executeRoute(pc, drive);
j_j205 8:90af8914ce03 140 wait(0.1);
j_j205 1:1958e6db41fc 141
j_j205 8:90af8914ce03 142 r5nav.getShortestPath(RED_DROP_ZONE);
j_j205 15:d8940756d5d5 143 pc.printf("\n\nDistance from %i to RED_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(RED_DROP_ZONE) );
j_j205 8:90af8914ce03 144 pc.printf("Route:\n");
j_j205 15:d8940756d5d5 145 //r5nav.printRoute(pc);
j_j205 15:d8940756d5d5 146 //r5nav.setVertex(RED_DROP_ZONE);
j_j205 12:e9f878ced6e7 147 r5nav.executeRoute(pc, drive);
j_j205 8:90af8914ce03 148 wait(0.1);
j_j205 20:53ab414d7b33 149
j_j205 29:e77e8891d985 150 /*
j_j205 15:d8940756d5d5 151 drive.move(0, ((90.0)*(3.14159 / 180.0))); // return to initial angle
j_j205 15:d8940756d5d5 152 // wait for move to complete
j_j205 15:d8940756d5d5 153 while(!drive.isMoveDone())
j_j205 15:d8940756d5d5 154 {
j_j205 15:d8940756d5d5 155 wait(1e-6);
j_j205 15:d8940756d5d5 156 }
j_j205 29:e77e8891d985 157 */
j_j205 15:d8940756d5d5 158 pc.printf("\n\nExercise Complete");
j_j205 0:a3c39d3359ac 159 return 0;
j_j205 12:e9f878ced6e7 160 }
j_j205 12:e9f878ced6e7 161
j_j205 12:e9f878ced6e7 162 void activate()
j_j205 12:e9f878ced6e7 163 {
j_j205 12:e9f878ced6e7 164 led_red = 1;
j_j205 12:e9f878ced6e7 165 led_green = 0;
j_j205 12:e9f878ced6e7 166 active = true;
j_j205 25:670a59096bf8 167 }