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@12:e9f878ced6e7, 2016-01-27 (annotated)

Committer:
j_j205
Date:
Wed Jan 27 20:48:55 2016 +0000
Revision:
12:e9f878ced6e7
Parent:
11:b5f75d25a4b3
Child:
15:d8940756d5d5
1/27/16 updated JJ

Who changed what in which revision?

UserRevisionLine numberNew contents of line
j_j205 8:90af8914ce03 1 #include "navigation.h"
j_j205 12:e9f878ced6e7 2 #include "StepperDrive.h"
j_j205 8:90af8914ce03 3 #include "stdint.h"
j_j205 2:3b162e764c9d 4 #include "mbed.h"
j_j205 0:a3c39d3359ac 5
j_j205 0:a3c39d3359ac 6 Serial pc(USBTX, USBRX);
j_j205 12:e9f878ced6e7 7 InterruptIn start(SW1);
j_j205 12:e9f878ced6e7 8 DigitalOut led_red(LED_RED);
j_j205 12:e9f878ced6e7 9 DigitalOut led_green(LED_GREEN);
j_j205 12:e9f878ced6e7 10 bool active = false;
j_j205 12:e9f878ced6e7 11
j_j205 12:e9f878ced6e7 12 void activate();
j_j205 0:a3c39d3359ac 13
j_j205 0:a3c39d3359ac 14 int main()
j_j205 0:a3c39d3359ac 15 {
j_j205 0:a3c39d3359ac 16 pc.baud(115200);
j_j205 12:e9f878ced6e7 17 start.mode(PullUp); /* Start/Stop button is active low needing PullUp */
j_j205 12:e9f878ced6e7 18 start.fall(&activate);
j_j205 0:a3c39d3359ac 19
j_j205 12:e9f878ced6e7 20 led_red = 0;
j_j205 12:e9f878ced6e7 21 led_green = 1;
j_j205 12:e9f878ced6e7 22
j_j205 8:90af8914ce03 23 Navigation r5nav(10);
j_j205 8:90af8914ce03 24 pc.printf("Navigation Object Created\n\n");
j_j205 8:90af8914ce03 25 wait(0.1);
j_j205 1:1958e6db41fc 26
j_j205 8:90af8914ce03 27 // loading map
j_j205 8:90af8914ce03 28 r5nav.addGraphNode(0, 1, 18, 0);
j_j205 8:90af8914ce03 29 r5nav.addGraphNode(1, 0, 18, 180);
j_j205 12:e9f878ced6e7 30 r5nav.addGraphNode(1, 2, 72, 0);
j_j205 12:e9f878ced6e7 31 r5nav.addGraphNode(1, 3, 13.5, 90);
j_j205 8:90af8914ce03 32 r5nav.addGraphNode(2, 1, 72, 180);
j_j205 12:e9f878ced6e7 33 r5nav.addGraphNode(3, 1, 13.5, 270);
j_j205 8:90af8914ce03 34 r5nav.addGraphNode(3, 4, 12, 180);
j_j205 8:90af8914ce03 35 r5nav.addGraphNode(3, 5, 23, 0);
j_j205 8:90af8914ce03 36 r5nav.addGraphNode(4, 3, 12, 0);
j_j205 8:90af8914ce03 37 r5nav.addGraphNode(5, 3, 23, 180);
j_j205 8:90af8914ce03 38 r5nav.addGraphNode(5, 6, 37, 0);
j_j205 8:90af8914ce03 39 r5nav.addGraphNode(5, 8, 12, 90);
j_j205 8:90af8914ce03 40 r5nav.addGraphNode(6, 5, 37, 180);
j_j205 8:90af8914ce03 41 r5nav.addGraphNode(6, 7, 12, 0);
j_j205 8:90af8914ce03 42 r5nav.addGraphNode(7, 6, 12, 180);
j_j205 8:90af8914ce03 43 r5nav.addGraphNode(8, 5, 12, 270);
j_j205 8:90af8914ce03 44 r5nav.addGraphNode(8, 9, 35, 180);
j_j205 8:90af8914ce03 45 r5nav.addGraphNode(9, 8, 35, 0);
j_j205 8:90af8914ce03 46
j_j205 8:90af8914ce03 47 const uint16_t YELLOW_VICTIM = 7;
j_j205 8:90af8914ce03 48 const uint16_t RED_VICTIM = 9;
j_j205 8:90af8914ce03 49 const uint16_t YELLOW_DROP_ZONE = 4;
j_j205 8:90af8914ce03 50 const uint16_t RED_DROP_ZONE = 2;
j_j205 12:e9f878ced6e7 51
j_j205 12:e9f878ced6e7 52 StepperDrive drive(pc, PTE19, PTE18, 1,PTE3, PTE2, 0, 5.0, 5.0, 1000);
j_j205 12:e9f878ced6e7 53 //(stepPinLeft, dirPinLeft, invertLeft, stepPinRight, dirPinRight, invertRight, wheelCircum, wheelSepar, periodUs)
j_j205 1:1958e6db41fc 54
j_j205 8:90af8914ce03 55 pc.printf("Size of graph is %i\n", r5nav.graphSize() );
j_j205 8:90af8914ce03 56 wait(0.1);
j_j205 8:90af8914ce03 57
j_j205 12:e9f878ced6e7 58 pc.printf("\nWaiting for START BUTTON\n");
j_j205 12:e9f878ced6e7 59 while(!active) // wait for start_button
j_j205 12:e9f878ced6e7 60 {
j_j205 12:e9f878ced6e7 61 wait(1e-6);
j_j205 12:e9f878ced6e7 62 }
j_j205 12:e9f878ced6e7 63
j_j205 8:90af8914ce03 64 r5nav.getShortestPath(YELLOW_VICTIM);
j_j205 8:90af8914ce03 65 pc.printf("\nDistance from 0 to YELLOW_VICTIM: %i\n", r5nav.getMinDist(YELLOW_VICTIM) );
j_j205 8:90af8914ce03 66 pc.printf("Route:\n");
j_j205 12:e9f878ced6e7 67 /*r5nav.printRoute(pc);
j_j205 12:e9f878ced6e7 68 r5nav.setVertex(YELLOW_VICTIM);*/
j_j205 12:e9f878ced6e7 69 r5nav.executeRoute(pc, drive);
j_j205 8:90af8914ce03 70 wait(0.1);
j_j205 1:1958e6db41fc 71
j_j205 8:90af8914ce03 72 r5nav.getShortestPath(YELLOW_DROP_ZONE);
j_j205 8:90af8914ce03 73 pc.printf("\nDistance from %i to YELLOW_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(YELLOW_DROP_ZONE) );
j_j205 8:90af8914ce03 74 pc.printf("Route:\n");
j_j205 12:e9f878ced6e7 75 /*r5nav.printRoute(pc);
j_j205 12:e9f878ced6e7 76 r5nav.setVertex(YELLOW_DROP_ZONE);*/
j_j205 12:e9f878ced6e7 77 r5nav.executeRoute(pc, drive);
j_j205 8:90af8914ce03 78 wait(0.1);
j_j205 1:1958e6db41fc 79
j_j205 8:90af8914ce03 80 r5nav.getShortestPath(RED_VICTIM);
j_j205 8:90af8914ce03 81 pc.printf("\nDistance from %i to RED_VICTIM: %i\n", r5nav.getVertex(), r5nav.getMinDist(RED_VICTIM) );
j_j205 8:90af8914ce03 82 pc.printf("Route:\n");
j_j205 12:e9f878ced6e7 83 /*r5nav.printRoute(pc);
j_j205 12:e9f878ced6e7 84 r5nav.setVertex(RED_VICTIM);*/
j_j205 12:e9f878ced6e7 85 r5nav.executeRoute(pc, drive);
j_j205 8:90af8914ce03 86 wait(0.1);
j_j205 1:1958e6db41fc 87
j_j205 8:90af8914ce03 88 r5nav.getShortestPath(RED_DROP_ZONE);
j_j205 8:90af8914ce03 89 pc.printf("\nDistance from %i to RED_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(RED_DROP_ZONE) );
j_j205 8:90af8914ce03 90 pc.printf("Route:\n");
j_j205 12:e9f878ced6e7 91 /*r5nav.printRoute(pc);
j_j205 12:e9f878ced6e7 92 r5nav.setVertex(RED_DROP_ZONE);*/
j_j205 12:e9f878ced6e7 93 r5nav.executeRoute(pc, drive);
j_j205 8:90af8914ce03 94 wait(0.1);
j_j205 11:b5f75d25a4b3 95
j_j205 12:e9f878ced6e7 96 //r5nav.printGraph(pc);
j_j205 0:a3c39d3359ac 97
j_j205 0:a3c39d3359ac 98 return 0;
j_j205 12:e9f878ced6e7 99 }
j_j205 12:e9f878ced6e7 100
j_j205 12:e9f878ced6e7 101 void activate()
j_j205 12:e9f878ced6e7 102 {
j_j205 12:e9f878ced6e7 103 led_red = 1;
j_j205 12:e9f878ced6e7 104 led_green = 0;
j_j205 12:e9f878ced6e7 105 active = true;
j_j205 1:1958e6db41fc 106 }