File content as of revision 0:345761c4cf73:

#include "mbed.h"
#include "DistanceSensor.h"
#include "Gripper.h"
#include "R5_StepperDrive.h"
#include <cmath>

int main()
{
    Serial bluetooth(PTE16,PTE17);
    DistanceSensor dsRight(PTC2,3);
    DistanceSensor dsLeft(PTC1,1);
    Gripper grip(PTE20,PTE21);
    StepperDrive drive(bluetooth,PTE19,PTE18,1,PTE3,PTE2,PTE22,0,10.0625,8.1875,700);
    double lDist, rDist;
    const int readThresh = 10;
    const double pi = 3.14159;
    const double theta = pi/4; //scan servo angle in radians
    const int k1 = x; //distance from plane of the sensors to center of rotation
    const int k2 = x; //distance from sensors to center line
    double a, b, c, A, B, adjust, dist;
    char side;

    bluetooth.printf("Beginning Search\n\r");
    while(true)
    {
        lDist = dsLeft.getDistance();
        rDist = dsRight.getDistance();
        
        if(lDist < readThresh || rDist < readThresh)
            break;
        
        drive.move(2,0);
    }
    
    bluetooth.printf("Range Found\n\r");
    
    if(lDist < rDist)
    {
        c = lDist;
        side = 'l';
        bluetooth.printf("Using Left Distance %d\n\r", c);
    }
    else
    {
        c = rDist;
        side = 'r';
        bluetooth.printf("Using Left Distance %d\n\r", c);
    }
        
    a = c*sin(theta);
    b = sqrt(pow(c,2) - pow(a,2)); //sqrt(c^2 - a^2)
    A = a + k1;
    
    if(side == 'l')
        B = b - k2;
    else
        B = k2 - b;

    adjust =  pi/2 - atan(A/B);
    dist = sqrt(pow(A,2) + pow(B,2));
    
    bluetooth.printf("Approach Move is %f at %f rads\n\r", dist, adjust);
    
    drive.move(dist,adjust);
    while(!drive.isMoveDone())
        wait_us(5);
    
    bluetooth.printf("Move Complete\n\r");
    bluetooth.printf("Grabbing\n\r");
    grip.grip();
    grip.lift();
    bluetooth.printf("Done!\n\r");
    
    return 0;
}