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

Committer:
j_j205
Date:
2015-10-30
Revision:
8:90af8914ce03
Parent:
3:72b652325643
Child:
9:a956ba674cea

File content as of revision 8:90af8914ce03:

#include "navigation.h"
#include "stdint.h"
#include "mbed.h"

Serial pc(USBTX, USBRX);

int main()
{
    pc.baud(115200);
    
    Navigation r5nav(10);
    pc.printf("Navigation Object Created\n\n");
    wait(0.1);

    // loading map
    r5nav.addGraphNode(0, 1, 18, 0);
    r5nav.addGraphNode(1, 0, 18, 180);
    r5nav.addGraphNode(1, 2, 7, 0);
    r5nav.addGraphNode(1, 3, 1, 90);
    r5nav.addGraphNode(2, 1, 72, 180);
    r5nav.addGraphNode(3, 1, 13, 270);
    r5nav.addGraphNode(3, 4, 12, 180);
    r5nav.addGraphNode(3, 5, 23, 0);
    r5nav.addGraphNode(4, 3, 12, 0);
    r5nav.addGraphNode(5, 3, 23, 180);
    r5nav.addGraphNode(5, 6, 37, 0);
    r5nav.addGraphNode(5, 8, 12, 90);
    r5nav.addGraphNode(6, 5, 37, 180);
    r5nav.addGraphNode(6, 7, 12, 0);
    r5nav.addGraphNode(7, 6, 12, 180);
    r5nav.addGraphNode(8, 5, 12, 270);
    r5nav.addGraphNode(8, 9, 35, 180);
    r5nav.addGraphNode(9, 8, 35, 0);
    
    const uint16_t YELLOW_VICTIM = 7;
    const uint16_t RED_VICTIM = 9;
    const uint16_t YELLOW_DROP_ZONE = 4;
    const uint16_t RED_DROP_ZONE = 2;

    pc.printf("Size of graph is %i\n", r5nav.graphSize() );
    wait(0.1);

    r5nav.getShortestPath(YELLOW_VICTIM);
    pc.printf("\nDistance from 0 to YELLOW_VICTIM: %i\n", r5nav.getMinDist(YELLOW_VICTIM) );
    pc.printf("Route:\n");
    r5nav.printRoute(pc);
    r5nav.setVertex(YELLOW_VICTIM);
    wait(0.1);

    r5nav.getShortestPath(YELLOW_DROP_ZONE);
    pc.printf("\nDistance from %i to YELLOW_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(YELLOW_DROP_ZONE) );
    pc.printf("Route:\n");
    r5nav.printRoute(pc);
    r5nav.setVertex(YELLOW_DROP_ZONE);
    wait(0.1);

    r5nav.getShortestPath(RED_VICTIM);
    pc.printf("\nDistance from %i to RED_VICTIM: %i\n", r5nav.getVertex(), r5nav.getMinDist(RED_VICTIM) );
    pc.printf("Route:\n");
    r5nav.printRoute(pc);
    r5nav.setVertex(RED_VICTIM);
    wait(0.1);

    r5nav.getShortestPath(RED_DROP_ZONE);
    pc.printf("\nDistance from %i to RED_DROP_ZONE: %i\n", r5nav.getVertex(), r5nav.getMinDist(RED_DROP_ZONE) );
    pc.printf("Route:\n");
    r5nav.printRoute(pc);
    r5nav.setVertex(RED_DROP_ZONE);
    wait(0.1);

    return 0;
}