File content as of revision 26:d99d078921a0:

#include "navigation.h"
#include "scanner.h"
#include "StepperDrive.h"
#include "VL6180x.h"
#include "stdint.h"
#include "mbed.h"

Serial pc(USBTX, USBRX);
InterruptIn start(SW1);
DigitalOut led_red(LED_RED);
DigitalOut led_green(LED_GREEN);
bool active = false;

void activate();

int main()
{
    pc.baud(115200);
    start.mode(PullUp); /* Start/Stop button is active low needing PullUp */
    start.fall(&activate);

    led_red = 0;
    led_green = 1;

    pc.printf("\nWaiting for START BUTTON\n");
    while(!active) // wait for start_button
    {
        wait(1e-6);
    }

    StepperDrive drive(pc, PTE19, PTE18, 0, PTE3, PTE2, 1, 10.0625, 8.3750, 500);
              //(serial &, stepPinLeft, dirPinLeft, invertLeft, stepPinRight, dirPinRight, invertRight, wheelCircum, wheelSepar, periodUs)

    VL6180x shortRangeL(PTE0, PTE1, 0x52); // I2C1, 0x29 left-shifted by 1 bit
    VL6180x shortRangeR(PTC9, PTC8, 0x52); // I2C0, 0x29 left-shifted by 1 bit
    LongRangeSensor longRangeL(pc, PTB2);
    LongRangeSensor longRangeR(pc, PTB3);

    Scanner scanner(pc, PTB0, PTB1, shortRangeL, shortRangeR, longRangeL, longRangeR, 0.075);
        // (Serial &, PinName servoL, PinName servoR, shortRangeL, shortRangeR, longRangeL, longRangeR, period)

    Navigation r5nav(scanner, 10);
    wait(0.1);

    r5nav.addGraphNode(0, 1, 10.0625, 0);
    r5nav.addGraphNode(1, 0, 10.0625, 180);
    r5nav.addGraphNode(1, 2, 10.0625, 270);
    r5nav.addGraphNode(2, 1, 10.0625, 90);
    r5nav.addGraphNode(2, 3, 10.0625, 180);
    r5nav.addGraphNode(3, 2, 10.0625, 0);
    r5nav.addGraphNode(3, 0, 10.0625, 90);
    r5nav.addGraphNode(0, 3, 10.0625, 270);

    /*
    // loading r5 map
    r5nav.addGraphNode(0, 1, 18, 0);
    r5nav.addGraphNode(1, 0, 18, 180);
    r5nav.addGraphNode(1, 2, 72, 0);
    r5nav.addGraphNode(1, 3, 13.5, 90);
    r5nav.addGraphNode(2, 1, 72, 180);
    r5nav.addGraphNode(3, 1, 13.5, 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;
    */

    r5nav.getShortestPath(1);
    pc.printf("\n\nDistance from 0 to 1: %i\n", r5nav.getMinDist(1) );
    pc.printf("Route:\n");
    r5nav.executeRoute(pc, drive);
    wait(0.1);

    r5nav.getShortestPath(2);
    pc.printf("\n\nDistance from 1 to 2: %i\n", r5nav.getMinDist(2) );
    pc.printf("Route:\n");
    r5nav.executeRoute(pc, drive);
    wait(0.1);

    r5nav.getShortestPath(3);
    pc.printf("\n\nDistance from 2 to 3: %i\n", r5nav.getMinDist(3) );
    pc.printf("Route:\n");
    r5nav.executeRoute(pc, drive);
    wait(0.1);

    r5nav.getShortestPath(0);
    pc.printf("\n\nDistance from 3 to 0: %i\n", r5nav.getMinDist(0) );
    pc.printf("Route:\n");
    r5nav.executeRoute(pc, drive);
    wait(0.1);

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

    r5nav.getShortestPath(YELLOW_DROP_ZONE);
    pc.printf("\n\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);
    r5nav.executeRoute(pc, drive);
    wait(0.1);

    r5nav.getShortestPath(RED_VICTIM);
    pc.printf("\n\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);
    r5nav.executeRoute(pc, drive);
    wait(0.1);

    r5nav.getShortestPath(RED_DROP_ZONE);
    pc.printf("\n\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);
    r5nav.executeRoute(pc, drive);
    wait(0.1);
    */

    drive.move(0, ((90.0)*(3.14159 / 180.0))); // return to initial angle
    // wait for move to complete
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }

    pc.printf("\n\nExercise Complete");
    return 0;
}

void activate()
{
    led_red = 1;
    led_green = 0;
    active = true;
}