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@9:a956ba674cea, 2015-10-30 (annotated)

Committer:
j_j205
Date:
Fri Oct 30 15:56:19 2015 +0000
Revision:
9:a956ba674cea
Parent:
8:90af8914ce03
Child:
11:b5f75d25a4b3
included stepper library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
j_j205 9:a956ba674cea 1 #include "StepperMotor.h"
j_j205 8:90af8914ce03 2 #include "navigation.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 0:a3c39d3359ac 7
j_j205 0:a3c39d3359ac 8 int main()
j_j205 0:a3c39d3359ac 9 {
j_j205 0:a3c39d3359ac 10 pc.baud(115200);
j_j205 0:a3c39d3359ac 11
j_j205 8:90af8914ce03 12 Navigation r5nav(10);
j_j205 8:90af8914ce03 13 pc.printf("Navigation Object Created\n\n");
j_j205 8:90af8914ce03 14 wait(0.1);
j_j205 1:1958e6db41fc 15
j_j205 8:90af8914ce03 16 // loading map
j_j205 8:90af8914ce03 17 r5nav.addGraphNode(0, 1, 18, 0);
j_j205 8:90af8914ce03 18 r5nav.addGraphNode(1, 0, 18, 180);
j_j205 8:90af8914ce03 19 r5nav.addGraphNode(1, 2, 7, 0);
j_j205 8:90af8914ce03 20 r5nav.addGraphNode(1, 3, 1, 90);
j_j205 8:90af8914ce03 21 r5nav.addGraphNode(2, 1, 72, 180);
j_j205 8:90af8914ce03 22 r5nav.addGraphNode(3, 1, 13, 270);
j_j205 8:90af8914ce03 23 r5nav.addGraphNode(3, 4, 12, 180);
j_j205 8:90af8914ce03 24 r5nav.addGraphNode(3, 5, 23, 0);
j_j205 8:90af8914ce03 25 r5nav.addGraphNode(4, 3, 12, 0);
j_j205 8:90af8914ce03 26 r5nav.addGraphNode(5, 3, 23, 180);
j_j205 8:90af8914ce03 27 r5nav.addGraphNode(5, 6, 37, 0);
j_j205 8:90af8914ce03 28 r5nav.addGraphNode(5, 8, 12, 90);
j_j205 8:90af8914ce03 29 r5nav.addGraphNode(6, 5, 37, 180);
j_j205 8:90af8914ce03 30 r5nav.addGraphNode(6, 7, 12, 0);
j_j205 8:90af8914ce03 31 r5nav.addGraphNode(7, 6, 12, 180);
j_j205 8:90af8914ce03 32 r5nav.addGraphNode(8, 5, 12, 270);
j_j205 8:90af8914ce03 33 r5nav.addGraphNode(8, 9, 35, 180);
j_j205 8:90af8914ce03 34 r5nav.addGraphNode(9, 8, 35, 0);
j_j205 8:90af8914ce03 35
j_j205 8:90af8914ce03 36 const uint16_t YELLOW_VICTIM = 7;
j_j205 8:90af8914ce03 37 const uint16_t RED_VICTIM = 9;
j_j205 8:90af8914ce03 38 const uint16_t YELLOW_DROP_ZONE = 4;
j_j205 8:90af8914ce03 39 const uint16_t RED_DROP_ZONE = 2;
j_j205 1:1958e6db41fc 40
j_j205 8:90af8914ce03 41 pc.printf("Size of graph is %i\n", r5nav.graphSize() );
j_j205 8:90af8914ce03 42 wait(0.1);
j_j205 8:90af8914ce03 43
j_j205 8:90af8914ce03 44 r5nav.getShortestPath(YELLOW_VICTIM);
j_j205 8:90af8914ce03 45 pc.printf("\nDistance from 0 to YELLOW_VICTIM: %i\n", r5nav.getMinDist(YELLOW_VICTIM) );
j_j205 8:90af8914ce03 46 pc.printf("Route:\n");
j_j205 8:90af8914ce03 47 r5nav.printRoute(pc);
j_j205 8:90af8914ce03 48 r5nav.setVertex(YELLOW_VICTIM);
j_j205 8:90af8914ce03 49 wait(0.1);
j_j205 1:1958e6db41fc 50
j_j205 8:90af8914ce03 51 r5nav.getShortestPath(YELLOW_DROP_ZONE);
j_j205 8:90af8914ce03 52 pc.printf("\nDistance from %i to YELLOW_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(YELLOW_DROP_ZONE) );
j_j205 8:90af8914ce03 53 pc.printf("Route:\n");
j_j205 8:90af8914ce03 54 r5nav.printRoute(pc);
j_j205 8:90af8914ce03 55 r5nav.setVertex(YELLOW_DROP_ZONE);
j_j205 8:90af8914ce03 56 wait(0.1);
j_j205 1:1958e6db41fc 57
j_j205 8:90af8914ce03 58 r5nav.getShortestPath(RED_VICTIM);
j_j205 8:90af8914ce03 59 pc.printf("\nDistance from %i to RED_VICTIM: %i\n", r5nav.getVertex(), r5nav.getMinDist(RED_VICTIM) );
j_j205 8:90af8914ce03 60 pc.printf("Route:\n");
j_j205 8:90af8914ce03 61 r5nav.printRoute(pc);
j_j205 8:90af8914ce03 62 r5nav.setVertex(RED_VICTIM);
j_j205 8:90af8914ce03 63 wait(0.1);
j_j205 1:1958e6db41fc 64
j_j205 8:90af8914ce03 65 r5nav.getShortestPath(RED_DROP_ZONE);
j_j205 8:90af8914ce03 66 pc.printf("\nDistance from %i to RED_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(RED_DROP_ZONE) );
j_j205 8:90af8914ce03 67 pc.printf("Route:\n");
j_j205 8:90af8914ce03 68 r5nav.printRoute(pc);
j_j205 8:90af8914ce03 69 r5nav.setVertex(RED_DROP_ZONE);
j_j205 8:90af8914ce03 70 wait(0.1);
j_j205 0:a3c39d3359ac 71
j_j205 0:a3c39d3359ac 72 return 0;
j_j205 1:1958e6db41fc 73 }