File content as of revision 36:5b0bf9153c3d:

#include "navigation.h"
#include "Gripper.h"
#include "scanner.h"
#include "StepperDrive.h"
#include "ShortRangeSensor.h"
#include "stdint.h"
#include "mbed.h"

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

void activate();

int main()
{
    pc.baud(115200);
    // pc.baud(9600);  /* interface via Bluetooth at 9600 */
    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.4800, 500); // 8.375
              //(serial &, stepPinLeft, dirPinLeft, invertLeft, stepPinRight, dirPinRight, invertRight, wheelCircum, wheelSepar, periodUs)

    ShortRangeSensor shortRangeL(PTE0, PTE1); // verify i2c pins
    ShortRangeSensor shortRangeR(PTC9, PTC8); // verify i2c pins
    LongRangeSensor longRangeL(pc, PTB2);
    LongRangeSensor longRangeR(pc, PTB3);
    Gripper gripper(PTB2, PTB3); // grip pin, wrist pin

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

    Navigation r5nav(scanner, 25);
    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, 13, 0);
    r5nav.addGraphNode(1, 0, 13, 180);
    r5nav.addGraphNode(1, 2, 68, 356.5); // original angle = 0
    r5nav.addGraphNode(1, 3, 15, 90);
    r5nav.addGraphNode(2, 1, 68, 180);
    r5nav.addGraphNode(3, 1, 15, 270);
    r5nav.addGraphNode(3, 4, 8, 180);
//    r5nav.addGraphNode(3, 5, 23, 356.5); // (-5 degrees) original angle = 0
    r5nav.addGraphNode(3, 15, 6, 0);
    r5nav.addGraphNode(4, 3, 8, 0);
//    r5nav.addGraphNode(5, 3, 23, 180);
    r5nav.addGraphNode(5, 6, 37, 0);
    r5nav.addGraphNode(5, 8, 12.5, 90);
    r5nav.addGraphNode(5, 15, 17, 180);
    r5nav.addGraphNode(6, 5, 37, 176.5); // original angle = 180
    r5nav.addGraphNode(6, 7, 6, 0);
    r5nav.addGraphNode(6, 10, 12.5, 90);
    r5nav.addGraphNode(7, 6, 6, 180);
    r5nav.addGraphNode(8, 5, 12.75, 270); // original distance = 12.5
//    r5nav.addGraphNode(8, 9, 35, 174.5); // original angle = 180
//    r5nav.addGraphNode(8, 10, 37, 0);
    r5nav.addGraphNode(8, 11, 12, 90);
    r5nav.addGraphNode(8, 16, 6, 180);
    r5nav.addGraphNode(8, 17, 6, 0);
//    r5nav.addGraphNode(9, 8, 35, 0);
    r5nav.addGraphNode(9, 16, 23, 0);
    r5nav.addGraphNode(10, 6, 12.5, 270);
    r5nav.addGraphNode(10, 13, 12, 90);
    r5nav.addGraphNode(10, 18, 6, 180);
    r5nav.addGraphNode(11, 8, 12, 270);
    r5nav.addGraphNode(11, 12, 35, 180);
    r5nav.addGraphNode(11, 19, 6, 180);
    r5nav.addGraphNode(12, 11, 35, 0);
    r5nav.addGraphNode(12, 19, 23, 0);
    r5nav.addGraphNode(13, 10, 12, 270);
    r5nav.addGraphNode(13, 14, 12, 0);
    r5nav.addGraphNode(14, 13, 12, 180);
    r5nav.addGraphNode(14, 20, 21, 90);
    r5nav.addGraphNode(15, 3, 6, 180);
    r5nav.addGraphNode(15, 5, 17, 0);
    r5nav.addGraphNode(16, 8, 6, 0);
    r5nav.addGraphNode(16, 9, 23, 180);
    r5nav.addGraphNode(17, 8, 6, 180);
    r5nav.addGraphNode(17, 18, 25, 0);
    r5nav.addGraphNode(18, 10, 6, 0);
    r5nav.addGraphNode(18, 17, 25, 180);
    r5nav.addGraphNode(19, 11, 6, 0);
    r5nav.addGraphNode(19, 12, 23, 180);
    r5nav.addGraphNode(20, 14, 21, 270);
    r5nav.addGraphNode(20, 21, 24, 90);
    r5nav.addGraphNode(21, 20, 24, 270);
    r5nav.addGraphNode(21, 22, 4, 180);
    r5nav.addGraphNode(22, 21, 4, 0);
    r5nav.addGraphNode(22, 23, 80, 180);
    r5nav.addGraphNode(23, 22, 80, 0);
    r5nav.addGraphNode(23, 24, 2, 270);
    r5nav.addGraphNode(24, 23, 2, 90);    

    const uint16_t V1 = 7;
    const uint16_t V2 = 9;
    const uint16_t V3 = 12;
    const uint16_t V4 = 20;
    const uint16_t V5 = 22;
    const uint16_t V6 = 24;
    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);
*/

    /* Begin 1st peg retrieval */
    
    r5nav.getShortestPath(1);
    pc.printf("\n\nDistance from 0 to 1: %i\n", r5nav.getMinDist(1) );
    pc.printf("Route:\n");
    scanner.setLocalizeRightFlag(1);
    scanner.setLocalizeLeftFlag(0);
    r5nav.executeRoute(pc, drive);
    wait(0.1);

    r5nav.getShortestPath(3);
    pc.printf("\n\nDistance from 1 to 3: %i\n", r5nav.getMinDist(3) );
    pc.printf("Route:\n");
    scanner.setLocalizeRightFlag(0);
    scanner.setLocalizeLeftFlag(0);
    r5nav.executeRoute(pc, drive);
    wait(0.1);
    
    r5nav.getShortestPath(15);
    pc.printf("\n\nDistance from 3 to 15: %i\n", r5nav.getMinDist(15) );
    pc.printf("Route:\n");
    scanner.setLocalizeRightFlag(0);
    scanner.setLocalizeLeftFlag(1);
    r5nav.executeRoute(pc, drive);
    wait(0.1);
    
    r5nav.getShortestPath(V1);
    pc.printf("\n\nDistance from 15 to V1: %i\n", r5nav.getMinDist(V1) );
    pc.printf("Route:\n");
    scanner.setLocalizeRightFlag(1);
    scanner.setLocalizeLeftFlag(0);
    r5nav.executeRoute(pc, drive);
    wait(0.1);
    
    /* add hunt method call */
    
    r5nav.getShortestPath(15);
    pc.printf("\n\nDistance from V1 to 15: %i\n", r5nav.getMinDist(15) );
    pc.printf("Route:\n");
    scanner.setLocalizeRightFlag(0);
    scanner.setLocalizeLeftFlag(1);
    r5nav.executeRoute(pc, drive);
    wait(0.1);
    
    r5nav.getShortestPath(3);
    pc.printf("\n\nDistance from 15 to 3: %i\n", r5nav.getMinDist(3) );
    pc.printf("Route:\n");
    scanner.setLocalizeRightFlag(1);
    scanner.setLocalizeLeftFlag(0);
    r5nav.executeRoute(pc, drive);
    wait(0.1);
    
    if (scanner.getYellowFlag() == 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);
        scanner.setLocalizeRightFlag(1);
        scanner.setLocalizeLeftFlag(0);
        r5nav.executeRoute(pc, drive);
        wait(0.1);
    }
    
    else
    {
        r5nav.getShortestPath(1);
        pc.printf("\n\nDistance from 3 to 1: %i\n", r5nav.getMinDist(1) );
        pc.printf("Route:\n");
        scanner.setLocalizeRightFlag(0);
        scanner.setLocalizeLeftFlag(0);
        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);
        scanner.setLocalizeRightFlag(1);
        scanner.setLocalizeLeftFlag(0);
        r5nav.executeRoute(pc, drive);
        wait(0.1);
    }
    
    /*  1st peg retrieval complete
        Begin 2nd peg retrieval   */  
    
    if (r5nav.getVertex() == YELLOW_DROP_ZONE)
    {
        r5nav.getShortestPath(3);
        pc.printf("\n\nDistance from YELLOW_DROP_ZONE to 3: %i\n", r5nav.getMinDist(3) );
        pc.printf("Route:\n");
        scanner.setLocalizeRightFlag(0);
        scanner.setLocalizeLeftFlag(1);
        r5nav.executeRoute(pc, drive);
        wait(0.1);
    }

    else
    {
        r5nav.getShortestPath(1);
        pc.printf("\n\nDistance from RED_DROP_ZONE to 1: %i\n", r5nav.getMinDist(1) );
        pc.printf("Route:\n");
        scanner.setLocalizeRightFlag(0);
        scanner.setLocalizeLeftFlag(1);
        r5nav.executeRoute(pc, drive);
        wait(0.1);
        
        r5nav.getShortestPath(3);
        pc.printf("\n\nDistance from 1 to 3: %i\n", r5nav.getMinDist(3) );
        pc.printf("Route:\n");
        scanner.setLocalizeRightFlag(0);
        scanner.setLocalizeLeftFlag(0);
        r5nav.executeRoute(pc, drive);
        wait(0.1);
    }
    
    r5nav.getShortestPath(15);
    pc.printf("\n\nDistance from 3 to 15: %i\n", r5nav.getMinDist(15) );
    pc.printf("Route:\n");
    scanner.setLocalizeRightFlag(0);
    scanner.setLocalizeLeftFlag(1);
    r5nav.executeRoute(pc, drive);
    wait(0.1);
    
    r5nav.getShortestPath(5);
    pc.printf("\n\nDistance from 15 to 5: %i\n", r5nav.getMinDist(5) );
    pc.printf("Route:\n");
    scanner.setLocalizeRightFlag(1);
    scanner.setLocalizeLeftFlag(0);
    r5nav.executeRoute(pc, drive);
    wait(0.1);
    
    r5nav.getShortestPath(16);
    pc.printf("\n\nDistance from 5 to 16: %i\n", r5nav.getMinDist(16) );
    pc.printf("Route:\n");
    scanner.setLocalizeRightFlag(0);
    scanner.setLocalizeLeftFlag(0);
    r5nav.executeRoute(pc, drive);
    wait(0.1);
    
    r5nav.getShortestPath(V2);
    pc.printf("\n\nDistance from 16 to V2: %i\n", r5nav.getMinDist(V2) );
    pc.printf("Route:\n");
    scanner.setLocalizeRightFlag(1);
    scanner.setLocalizeLeftFlag(0);
    r5nav.executeRoute(pc, drive);
    wait(0.1);
    
    /* add hunt method call */
    
    r5nav.getShortestPath(16);
    pc.printf("\n\nDistance from V2 to 16: %i\n", r5nav.getMinDist(16) );
    pc.printf("Route:\n");
    scanner.setLocalizeRightFlag(1);
    scanner.setLocalizeLeftFlag(0);
    r5nav.executeRoute(pc, drive);
    wait(0.1);
    
    r5nav.getShortestPath(5);
    pc.printf("\n\nDistance from 16 to 5: %i\n", r5nav.getMinDist(5) );
    pc.printf("Route:\n");
    scanner.setLocalizeRightFlag(0);
    scanner.setLocalizeLeftFlag(0);
    r5nav.executeRoute(pc, drive);
    wait(0.1);
    
    r5nav.getShortestPath(15);
    pc.printf("\n\nDistance from 5 to 15: %i\n", r5nav.getMinDist(15) );
    pc.printf("Route:\n");
    scanner.setLocalizeRightFlag(0);
    scanner.setLocalizeLeftFlag(1);
    r5nav.executeRoute(pc, drive);
    wait(0.1);
    
    r5nav.getShortestPath(3);
    pc.printf("\n\nDistance from 15 to 3: %i\n", r5nav.getMinDist(3) );
    pc.printf("Route:\n");
    scanner.setLocalizeRightFlag(0);
    scanner.setLocalizeLeftFlag(1);
    r5nav.executeRoute(pc, drive);
    wait(0.1);
    
    if (scanner.getYellowFlag() == 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);
        scanner.setLocalizeRightFlag(1);
        scanner.setLocalizeLeftFlag(0);
        r5nav.executeRoute(pc, drive);
        wait(0.1);
    }
    
    else
    {
        r5nav.getShortestPath(1);
        pc.printf("\n\nDistance from 3 to 1: %i\n", r5nav.getMinDist(1) );
        pc.printf("Route:\n");
        scanner.setLocalizeRightFlag(0);
        scanner.setLocalizeLeftFlag(0);
        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);
        scanner.setLocalizeRightFlag(1);
        scanner.setLocalizeLeftFlag(0);
        r5nav.executeRoute(pc, drive);
        wait(0.1);
    }
    
    /*  2nd peg retrieval complete
        Begin 3nd peg retrieval   */
        
    
    if (r5nav.getVertex() == YELLOW_DROP_ZONE)
    {
        r5nav.getShortestPath(3);
        pc.printf("\n\nDistance from YELLOW_DROP_ZONE to 3: %i\n", r5nav.getMinDist(3) );
        pc.printf("Route:\n");
        scanner.setLocalizeRightFlag(0);
        scanner.setLocalizeLeftFlag(1);
        r5nav.executeRoute(pc, drive);
        wait(0.1);
    }

    else
    {
        r5nav.getShortestPath(1);
        pc.printf("\n\nDistance from RED_DROP_ZONE to 1: %i\n", r5nav.getMinDist(1) );
        pc.printf("Route:\n");
        scanner.setLocalizeRightFlag(0);
        scanner.setLocalizeLeftFlag(1);
        r5nav.executeRoute(pc, drive);
        wait(0.1);
        
        r5nav.getShortestPath(3);
        pc.printf("\n\nDistance from 1 to 3: %i\n", r5nav.getMinDist(3) );
        pc.printf("Route:\n");
        scanner.setLocalizeRightFlag(0);
        scanner.setLocalizeLeftFlag(0);
        r5nav.executeRoute(pc, drive);
        wait(0.1);
    }
    
    r5nav.getShortestPath(15);
    pc.printf("\n\nDistance from 3 to 15: %i\n", r5nav.getMinDist(15) );
    pc.printf("Route:\n");
    scanner.setLocalizeRightFlag(0);
    scanner.setLocalizeLeftFlag(1);
    r5nav.executeRoute(pc, drive);
    wait(0.1);
    
    r5nav.getShortestPath(5);
    pc.printf("\n\nDistance from 15 to 5: %i\n", r5nav.getMinDist(5) );
    pc.printf("Route:\n");
    scanner.setLocalizeRightFlag(1);
    scanner.setLocalizeLeftFlag(0);
    r5nav.executeRoute(pc, drive);
    wait(0.1);
    
    r5nav.getShortestPath(19);
    pc.printf("\n\nDistance from 5 to 19: %i\n", r5nav.getMinDist(19) );
    pc.printf("Route:\n");
    scanner.setLocalizeRightFlag(0);
    scanner.setLocalizeLeftFlag(0);
    r5nav.executeRoute(pc, drive);
    wait(0.1);
    
    r5nav.getShortestPath(V3);
    pc.printf("\n\nDistance from 19 to V3: %i\n", r5nav.getMinDist(V3) );
    pc.printf("Route:\n");
    scanner.setLocalizeRightFlag(0);
    scanner.setLocalizeLeftFlag(1);
    r5nav.executeRoute(pc, drive);
    wait(0.1);
    
    /* add hunt method call */
    
    if (scanner.getObjectFound() == 1)
    {
        r5nav.getShortestPath(19);
        pc.printf("\n\nDistance from V3 to 19: %i\n", r5nav.getMinDist(19) );
        pc.printf("Route:\n");
        scanner.setLocalizeRightFlag(1);
        scanner.setLocalizeLeftFlag(0);
        r5nav.executeRoute(pc, drive);
        wait(0.1);
        
        // at node 5
        r5nav.getShortestPath(5);
        pc.printf("\n\nDistance from 19 to 5: %i\n", r5nav.getMinDist(5) );
        pc.printf("Route:\n");
        scanner.setLocalizeRightFlag(0);
        scanner.setLocalizeLeftFlag(0);
        r5nav.executeRoute(pc, drive);
        wait(0.1);
        
        r5nav.getShortestPath(15);
        pc.printf("\n\nDistance from 5 to 15: %i\n", r5nav.getMinDist(15) );
        pc.printf("Route:\n");
        scanner.setLocalizeRightFlag(0);
        scanner.setLocalizeLeftFlag(1);
        r5nav.executeRoute(pc, drive);
        wait(0.1);
        
        r5nav.getShortestPath(3);
        pc.printf("\n\nDistance from 15 to 3: %i\n", r5nav.getMinDist(3) );
        pc.printf("Route:\n");
        scanner.setLocalizeRightFlag(0);
        scanner.setLocalizeLeftFlag(1);
        r5nav.executeRoute(pc, drive);
        wait(0.1);
        
        if (scanner.getYellowFlag() == 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);
            scanner.setLocalizeRightFlag(1);
            scanner.setLocalizeLeftFlag(0);
            r5nav.executeRoute(pc, drive);
            wait(0.1);
        }
        
        else
        {
            r5nav.getShortestPath(1);
            pc.printf("\n\nDistance from 3 to 1: %i\n", r5nav.getMinDist(1) );
            pc.printf("Route:\n");
            scanner.setLocalizeRightFlag(0);
            scanner.setLocalizeLeftFlag(0);
            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);
            scanner.setLocalizeRightFlag(1);
            scanner.setLocalizeLeftFlag(0);
            r5nav.executeRoute(pc, drive);
            wait(0.1);
        }
    }
    
    else // peg V3 not found
    {
        /* need to implement rest of moves
        
        r5nav.getShortestPath(19);
        pc.printf("\n\nDistance from 12 to 19: %i\n", r5nav.getMinDist(19) );
        pc.printf("Route:\n");
        scanner.setLocalizeRightFlag(1);
        scanner.setLocalizeLeftFlag(0);
        r5nav.executeRoute(pc, drive);
        wait(0.1);
        
        // at node 5
        r5nav.getShortestPath(5);
        pc.printf("\n\nDistance from 19 to 5: %i\n", r5nav.getMinDist(5) );
        pc.printf("Route:\n");
        scanner.setLocalizeRightFlag(0);
        scanner.setLocalizeLeftFlag(0);
        r5nav.executeRoute(pc, drive);
        wait(0.1); */
    }
    
    /*  3rd peg retrieval complete
        Begin 4th peg retrieval   */
    
    

/*
    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;
}