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@20:53ab414d7b33, 2016-02-23 (annotated)

Committer:
j_j205
Date:
Tue Feb 23 23:36:14 2016 +0000
Revision:
20:53ab414d7b33
Parent:
15:d8940756d5d5
Child:
25:670a59096bf8
2/23/16 JJ

Who changed what in which revision?

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