File content as of revision 0:76bca1f580a0:

#include "scanner.h"
#include "StepperDrive.h"
#include "LongRangeSensor.h"
#include "stdint.h"
#include "mbed.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;

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); /* Start/Stop button is active low needing PullUp */
    start.fall(&activate);

    led_red = 0;
    led_green = 1;

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

    LongRangeSensor longRangeL(pc, PTB2);
    LongRangeSensor longRangeR(pc, PTB3);

    Scanner scanner(pc, drive, PTB0, PTB1, longRangeL, longRangeR, 1.0); // original period = 0.075
        // (Serial &, PinName servoL, PinName servoR, shortRangeL, shortRangeR, longRangeL, longRangeR, period)
    
    pc.printf("\nWaiting for START BUTTON\n");
    bluetooth.printf("\nWaiting for START BUTTON\n");
    while(!active) // wait for start_button
    {
        wait(1e-6);
    }
              
    pc.printf("\nBegin 1st peg retrieval");
    bluetooth.printf("\nBegin 1st peg retrieval");
    
    drive.move(13.0, 0.0);
    scanner.setLocalizeRightFlag(0);
pc.printf("\n    LocalizeRight on");
bluetooth.printf("\n    LocalizeRight on");
    scanner.setLocalizeLeftFlag(0);
    // wait for move to complete
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }
    pc.printf("\nMoved from 0 to 1");
    bluetooth.printf("\nMoved from 0 to 1");
    
    scanner.setLocalizeRightFlag(0);
pc.printf("\n    LocalizeRight off");
bluetooth.printf("\n    LocalizeRight off");
    scanner.setLocalizeLeftFlag(0);
    drive.move(0.0, (-90.0)*(3.14159 / 180.0));
    // wait for move to complete
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }
    pc.printf("\nMoved left 90 degrees");
    bluetooth.printf("\nMoved left 90 degrees");
    
    drive.move(15.0, 0.0);
    scanner.setLocalizeRightFlag(0);
    scanner.setLocalizeLeftFlag(0);
    // wait for move to complete
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }
    pc.printf("\nMoved from 1 to 3");
    bluetooth.printf("\nMoved from 1 to 3");
    
    scanner.setLocalizeRightFlag(0);
    scanner.setLocalizeLeftFlag(0);
    drive.move(0.0, (90.0)*(3.14159 / 180.0));
    // wait for move to complete
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }
    pc.printf("\nMoved right 90 degrees");
    bluetooth.printf("\nMoved right 90 degrees");
    
    drive.move(6.0, 0.0);
    scanner.setLocalizeRightFlag(0);
    scanner.setLocalizeLeftFlag(0);
    // wait for move to complete
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }
    pc.printf("\nMoved from 3 to 15");
    bluetooth.printf("\nMoved from 3 to 15");
    
    drive.move(60.0, 0.0);
    scanner.setLocalizeRightFlag(1);
pc.printf("\n    LocalizeRight on");
    scanner.setLocalizeLeftFlag(0);
    // wait for move to complete
    while(!drive.isMoveDone())
    {
        wait(1e-6);
    }
    pc.printf("\nMoved from 15 to 7");
    bluetooth.printf("\nMoved from 15 to 7");
    
    pc.printf("\nExercise Complete");
    bluetooth.printf("\nExercise Complete");
}