File content as of revision 41:029135230ebf:

#include "mbed.h"
#include "StepperDrive.h"
#include "LongRangeSensor.h"
#include "DistanceSensor.h"
#include "navigation.h"
#include "Gripper.h"


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

void reverseMove(StepperDrive &drive, float dist, float angle);

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

int main() 
{
    pc.baud(115200);
    bluetooth.baud(9600);  /* interface via Bluetooth at 9600 */
    start.mode(PullUp); /* Button is active low needing PullUp */
    start.fall(&activate);

    led_red = 0;
    led_green = 1;
    
    DistanceSensor shortRangeL(PTC2, 2);
    DistanceSensor shortRangeR(PTC1, 3);
pc.printf("\nShort Range Sensors created");
bluetooth.printf("\nShort Range Sensors created");
    
    LongRangeSensor longRangeL(bluetooth, PTB2);
    LongRangeSensor longRangeR(bluetooth, PTB3);
pc.printf("\nLong Range Sensors created");
bluetooth.printf("\nLong Range Sensors created");

    StepperDrive drive(bluetooth, PTE19, PTE18, 1, PTE3, PTE2, 0, 10.0625, 
        8.1875, 700); // 8.4800
              /* (serial &, stepPinLeft, dirPinLeft, invertLeft, 
              stepPinRight, dirPinRight, invertRight, wheelCircum, 
              wheelSepar, periodUs) */
pc.printf("\nStepperDrive created");
bluetooth.printf("\nStepperDrive created");

//    Gripper gripper(PTE20, PTE21); // grip pin, wrist pin
    
    Navigation r5map(bluetooth, drive, longRangeL, longRangeR, led_red, led_green, 61);
pc.printf("\nNavigation created");
bluetooth.printf("\nNavigation created");
    
    //loading r5 map...
    r5map.addGraphNode(0, 1, 6.75, 0);
    r5map.addGraphNode(1, 0, 6.75, 180);
    r5map.addGraphNode(1, 2, 7, 0);
    r5map.addGraphNode(2, 1, 7, 180);
    r5map.addGraphNode(2, 3, 8, 0);
    r5map.addGraphNode(2, 12, 14.75, 90);
    r5map.addGraphNode(3, 2, 8, 180);
    r5map.addGraphNode(3, 4, 8, 0);
    r5map.addGraphNode(4, 3, 8, 180);
    r5map.addGraphNode(4, 5, 8, 0);
    r5map.addGraphNode(5, 4, 8, 180);
    r5map.addGraphNode(5, 6, 8, 0);
    r5map.addGraphNode(6, 5, 8, 180);
    r5map.addGraphNode(6, 7, 8, 0);
    r5map.addGraphNode(7, 6, 8, 180);
    r5map.addGraphNode(7, 8, 8, 0);
    r5map.addGraphNode(8, 7, 8, 180);
    r5map.addGraphNode(8, 9, 8, 0);
    r5map.addGraphNode(9, 8, 8, 180);
    r5map.addGraphNode(9, 10, 8, 0);
    r5map.addGraphNode(10, 9, 8, 180);
    r5map.addGraphNode(10, 11, 4, 0);
    r5map.addGraphNode(11, 10, 4, 180);
    r5map.addGraphNode(12, 2, 14.75, 270);
    r5map.addGraphNode(12, 13, 8, 180);
    r5map.addGraphNode(12, 14, 6, 0);
    r5map.addGraphNode(13, 12, 8, 0);
    r5map.addGraphNode(14, 12, 6, 180);
    r5map.addGraphNode(14, 15, 7, 0);
    r5map.addGraphNode(15, 14, 7, 180);
    r5map.addGraphNode(15, 16, 7, 0);
    r5map.addGraphNode(16, 15, 7, 180);
    r5map.addGraphNode(16, 17, 3, 0);
    r5map.addGraphNode(17, 16, 3, 180);
    r5map.addGraphNode(17, 18, 8, 0);
    r5map.addGraphNode(17, 30, 12, 90);
    r5map.addGraphNode(18, 17, 8, 180);
    r5map.addGraphNode(18, 19, 8, 0);
    r5map.addGraphNode(19, 18, 8, 180);
    r5map.addGraphNode(19, 20, 7, 0);
    r5map.addGraphNode(20, 19, 7, 180);
    r5map.addGraphNode(20, 21, 7, 0);
    r5map.addGraphNode(21, 20, 7, 180);
    r5map.addGraphNode(21, 22, 7, 0);
    r5map.addGraphNode(22, 21, 7, 180);
    r5map.addGraphNode(22, 23, 6, 0);
    r5map.addGraphNode(23, 22, 6, 180);
    r5map.addGraphNode(24, 22, 12, 270);
    r5map.addGraphNode(24, 25, 6, 180);
    r5map.addGraphNode(24, 42, 12, 90);
    r5map.addGraphNode(25, 24, 6, 0);
    r5map.addGraphNode(25, 26, 8, 180);
    r5map.addGraphNode(26, 25, 7, 0);
    r5map.addGraphNode(26, 27, 7, 180);
    r5map.addGraphNode(27, 26, 7, 0);
    r5map.addGraphNode(27, 28, 7, 180);
    r5map.addGraphNode(28, 27, 7, 0);
    r5map.addGraphNode(28, 29, 4, 180);
    r5map.addGraphNode(29, 28, 4, 0);
    r5map.addGraphNode(29, 30, 7, 180);
    r5map.addGraphNode(30, 18, 12, 270);
    r5map.addGraphNode(30, 29, 6, 0);
    r5map.addGraphNode(30, 31, 6, 180);
    r5map.addGraphNode(30, 36, 12, 90);
    r5map.addGraphNode(31, 30, 6, 0);
    r5map.addGraphNode(31, 32, 7, 180);
    r5map.addGraphNode(32, 31, 7, 0);
    r5map.addGraphNode(32, 33, 7, 180);
    r5map.addGraphNode(33, 32, 7, 0);
    r5map.addGraphNode(33, 34, 7, 180);
    r5map.addGraphNode(34, 33, 7, 0);
    r5map.addGraphNode(34, 35, 2, 180);
    r5map.addGraphNode(35, 34, 2, 0);
    r5map.addGraphNode(36, 30, 12, 270);
    r5map.addGraphNode(36, 37, 7, 180);
    r5map.addGraphNode(37, 36, 7, 0);
    r5map.addGraphNode(37, 38, 7, 180);
    r5map.addGraphNode(38, 37, 7, 0);
    r5map.addGraphNode(38, 39, 7, 180);
    r5map.addGraphNode(39, 38, 7, 0);
    r5map.addGraphNode(39, 40, 7, 180);
    r5map.addGraphNode(40, 39, 7, 0);
    r5map.addGraphNode(40, 41, 2, 180);
    r5map.addGraphNode(41, 40, 2, 0);
    r5map.addGraphNode(42, 24, 12, 270);
    r5map.addGraphNode(42, 43, 6, 0);
    r5map.addGraphNode(43, 42, 6, 180);
    r5map.addGraphNode(43, 44, 6, 0);
    r5map.addGraphNode(44, 43, 6, 180);
    r5map.addGraphNode(44, 45, 7, 90);
    r5map.addGraphNode(45, 44, 7, 270);
    r5map.addGraphNode(45, 46, 7, 90);
    r5map.addGraphNode(46, 45, 7, 270);
    r5map.addGraphNode(46, 47, 7, 90);
    r5map.addGraphNode(47, 46, 7, 270);
    r5map.addGraphNode(47, 48, 8, 90);
    r5map.addGraphNode(48, 47, 8, 270);
    r5map.addGraphNode(48, 49, 8, 90);
    r5map.addGraphNode(49, 48, 8, 270);
    r5map.addGraphNode(49, 50, 8, 90);
    r5map.addGraphNode(50, 49, 8, 270);
    r5map.addGraphNode(50, 51, 4, 180);
    r5map.addGraphNode(51, 50, 4, 0);
    r5map.addGraphNode(51, 52, 10, 180);
    r5map.addGraphNode(52, 51, 10, 0);
    r5map.addGraphNode(52, 53, 10, 180);     
    r5map.addGraphNode(53, 52, 10, 0);
    r5map.addGraphNode(53, 54, 10, 180);
    r5map.addGraphNode(54, 53, 10, 0);
    r5map.addGraphNode(54, 55, 10, 180);  
    r5map.addGraphNode(55, 54, 10, 0);
    r5map.addGraphNode(55, 56, 10, 180);
    r5map.addGraphNode(56, 55, 10, 0);
    r5map.addGraphNode(56, 57, 10, 180);
    r5map.addGraphNode(57, 56, 10, 0);
    r5map.addGraphNode(57, 55, 10, 180);
    r5map.addGraphNode(58, 57, 10, 0);
    r5map.addGraphNode(58, 59, 10, 180);
    r5map.addGraphNode(59, 58, 10, 0);
    r5map.addGraphNode(59, 60, 2, 270);
    r5map.addGraphNode(60, 59, 2, 90);
    
//    gripper.lift();
//    gripper.release();
    
    const uint8_t V1 = 23;
    const uint8_t V2 = 35;
    const uint8_t V3 = 41;
    const uint8_t V4 = 47;
    const uint8_t V5 = 51;
    const uint8_t V6 = 60;
    const uint8_t YELLOW_DROP_ZONE = 13;
    const uint8_t RED_DROP_ZONE = 11;
              
    pc.printf("\nWaiting for START BUTTON\n");
    bluetooth.printf("\nWaiting for START BUTTON\n");
    while(!active) // wait for start_button
    {
        wait(1e-6);
    }
    
    int target;
    target = V1;    
    r5map.getShortestPath(target);
    pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V1) );
    bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V1) );
    r5map.executeRoute();
    wait(0.1);
    
//    call to hunt

    // back up to safe turn-around location 
    pc.printf("\nBack up to safe turn around location");
    bluetooth.printf("\nBack up to safe turn around location");
    reverseMove(drive, 5.0, 0);
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }
    r5map.setVertex(22);
    
    target = YELLOW_DROP_ZONE;
    r5map.getShortestPath(target);
    pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(YELLOW_DROP_ZONE) );
    bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(YELLOW_DROP_ZONE) );
    r5map.executeRoute();
    wait(0.1);
    
    // back up to safe turn-around location 
    pc.printf("\nBack up to a safe turn around location");
    bluetooth.printf("\nBack up to a safe turn around location");
    r5map.localizeLeftReverse();
    reverseMove(drive, 8.0, 0);
    // wait for move to complete
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }
    r5map.setVertex(12);
    r5map.localizeLeftReverse();
    
    // turn 180 degrees to left
    pc.printf("\nTurn left 180 degrees");
    bluetooth.printf("\nTurn left 180 degrees");    
    drive.move(0, -185.0*(3.14159/180));
    // wait for move to complete
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }
    r5map.setAngle(0);
    
    // move to compensate for coming up short
    pc.printf("\nCompensation move forward 2.0");
    bluetooth.printf("\nCompensation move forward 2.0");    
    drive.move(2.0, 0);
    // wait for move to complete
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }
    
    target = V2;
    r5map.getShortestPath(target);
    pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V2) );
    bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V2) );
    r5map.executeRoute();
    wait(0.1);
    
    /* back up to safe turn-around location */
    pc.printf("\nBack up to a safe turn around location");
    bluetooth.printf("\nBack up to a safe turn around location");
    r5map.newLocalizeLeftReverse();
    reverseMove(drive, 2.0, 0);
    // wait for move to complete
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    } 
    r5map.setVertex(34);
    r5map.newLocalizeLeftReverse();
    reverseMove(drive, 7.0, 0);
    // wait for move to complete
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }  
    r5map.setVertex(33);
    r5map.newLocalizeLeftReverse();
    reverseMove(drive, 7.0, 0);
    // wait for move to complete
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }  
    r5map.setVertex(32);
    r5map.newLocalizeLeftReverse();
    reverseMove(drive, 7.0, 0);
    // wait for move to complete
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    } 
    r5map.setVertex(31);
    r5map.newLocalizeLeftReverse();
    reverseMove(drive, 6.0, 0);
    // wait for move to complete
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }  
    r5map.setVertex(30);
    r5map.newLocalizeLeftReverse();
    
    
    target = RED_DROP_ZONE;
    r5map.getShortestPath(target);
    pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(RED_DROP_ZONE) );
    bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(RED_DROP_ZONE) );
    r5map.executeRoute();
    wait(0.1);
    
    /*// back up to safe turn around location 
    pc.printf("\nBack up to a safe turn around location");
    bluetooth.printf("\nBack up to a safe turn around location");
    r5map.localizeLeftReverse();
    reverseMove(drive, 4.0, 0);
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }
    r5map.setVertex(10);
    r5map.localizeLeftReverse();
    reverseMove(drive, 8.0, 0);
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }
    r5map.setVertex(9);
    r5map.localizeLeftReverse();
    reverseMove(drive, 8.0, 0);
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }
    r5map.setVertex(8);
    r5map.localizeLeftReverse();
    reverseMove(drive, 8.0, 0);
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }
    r5map.setVertex(7);
    r5map.localizeLeftReverse();
    reverseMove(drive, 8.0, 0);
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }
    r5map.setVertex(6);
    r5map.localizeLeftReverse();
    reverseMove(drive, 8.0, 0);
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }
    r5map.setVertex(5);
    r5map.localizeLeftReverse();
    reverseMove(drive, 8.0, 0);
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }
    r5map.setVertex(4);
    r5map.localizeLeftReverse();
    reverseMove(drive, 8.0, 0);
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }
    r5map.setVertex(3);
    r5map.localizeLeftReverse();
    reverseMove(drive, 8.0, 0);
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }
    r5map.setVertex(2);
    r5map.localizeLeftReverse();*/
    
    target = V3;
    r5map.getShortestPath(target);
    pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V3) );
    bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V3) );
    r5map.executeRoute();
    wait(0.1);
    
    target = RED_DROP_ZONE;
    r5map.getShortestPath(target);
    pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(RED_DROP_ZONE) );
    bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(RED_DROP_ZONE) );
    r5map.executeRoute();
    wait(0.1);
    
    target = V4;
    r5map.getShortestPath(target);
    pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V4) );
    bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(V4) );
    r5map.executeRoute();
    wait(0.1);
    
    target = YELLOW_DROP_ZONE;
    r5map.getShortestPath(target);
    pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(YELLOW_DROP_ZONE) );
    bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(YELLOW_DROP_ZONE) );
    r5map.executeRoute();
    wait(0.1);
    
    target = 0;
    r5map.getShortestPath(target);
    pc.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(0) );
    bluetooth.printf("\n\nDistance from %i to %i: %f\n", r5map.getVertex(), target, r5map.getMinDist(0) );
    r5map.executeRoute();
    wait(0.1);
    
    pc.printf("\nExercise Complete");
    bluetooth.printf("\nExercise Complete");
} // end of main

// FUNCTION:
//      void reverseMove(float dist, float angle)
// IN-PARAMETERS:
//      dist (float), angle(float)
// OUT-PARAMETERS:
//      None
// DESCRIPTION:
//      Inverts values of invertLeft and invertRight and sends move
//      command to move robot in reverse direction.
void reverseMove(StepperDrive &drive, float dist, float angle)
{
     // swap values for invertLeft and invertRight
     drive.setInvertLeft(!drive.getInvertLeft());
     drive.setInvertRight(!drive.getInvertRight());
     
     drive.move(dist, angle);
     // wait for move to complete
     while(!drive.isMoveDone())
     { 
          wait(1e-6);
     } 
     
     // restore original values for invertLeft and invertRight
     drive.setInvertLeft(!drive.getInvertLeft());
     drive.setInvertRight(!drive.getInvertRight());
}