File content as of revision 0:23ce86997770:

#include "mbed.h"
#include "motordriver.h"
#include "math.h"
 
// Middle = M, Left = L, Right = R
 
Motor RA(p23, p30, p24, 1);
Motor LA(p22, p29, p24, 1);
Motor MA(p21, p26, p24, 1);
 
Motor MB(p21, p24, p26, 1);
Motor RB(p23, p24, p30, 1);
Motor LB(p22, p24, p29, 1);
 
AnalogIn SML(p17); // sensor middle and left above right whell
AnalogIn SMR(p19); // sensor middle and right above left whell
AnalogIn SRL(p20); // sensor right and left above middle whell
 
void stop()
{
    MA.coast();
    LA.coast();
    RA.coast();
    MB.coast();
    LB.coast();
    RB.coast();
}
 
float getDistance(float a, float b, float c, float x)
{
    return (a - b*x + c*pow(x,2));
}
 
int main()
{
    while(1){
 
        // move back SML - Right
        // y = 99.77605 - 382.7846*x + 398.975*x^2
        float distanceSML = getDistance(99.77605, 382.7846, 398.975 , SML.read()); // TODO: put parameter for this sensor for a and b
        // backSML(0.3, 1000);

        // move back SLR - Middle
        // y = 99.71885 - 382.2867*x + 398.2079*x^2
        float distanceSLR = getDistance(99.71885, 384.0377, 400.3844, SRL.read()); // TODO: put parameter for this sensor for a and b
        // backSLR(speedSLR, durationSLR);

        // move back SMR - Left
        // y = 99.97613 - 384.0377*x + 400.3844*x^2
        float distanceSMR = getDistance(99.97613, 384.0377, 400.3844, SMR.read()); // TODO: put parameter for this sensor for a and b
        // backSMR(speedSMR, durationSMR);
       
        if(distanceSMR <= 9.5 && distanceSLR <= 9.5 && distanceSML > 9.5){
            MB.speed(0.7);
            LA.speed(0.7);
            RA.speed(0.0);
        } else if(distanceSMR > 9.5 && distanceSLR > 9.5 && distanceSML <= 9.5){
            MA.speed(0.7);
            LB.speed(0.7);
            RA.speed(0.0);
        }
        
        if(distanceSML <= 9.5 && distanceSLR <= 9.5 && distanceSMR > 9.5 ){  
            RB.speed(0.7);
            MA.speed(0.7);
            LA.speed(0.0);
        } else if(distanceSML > 9.5 && distanceSLR > 9.5 && distanceSMR <= 9.5 ){  
            RA.speed(0.7);
            MB.speed(0.7);
            LA.speed(0.0);
        }

        if(distanceSML <= 9.5 && distanceSMR <= 9.5 && distanceSLR > 9.5){
            LB.speed(0.7);
            RA.speed(0.7);
            MB.speed(0.0);     
        } else if(distanceSML > 9.5 && distanceSMR > 9.5 && distanceSLR <= 9.5){
            LA.speed(0.7);
            RB.speed(0.7);
            MB.speed(0.0);
        }
        
        // spin 

        while(distanceSML > 11 && distanceSLR > 11 && distanceSMR > 11) {
            RA.speed(0.3);
            MA.speed(0.3);
            LA.speed(0.3);
            // move back SML - Right
            // y = 99.77605 - 382.7846*x + 398.975*x^2
            float distanceSML = getDistance(99.77605, 382.7846, 398.975 , SML.read()); // TODO: put parameter for this sensor for a and b
            // backSML(0.3, 1000);

            // move back SLR - Middle
            // y = 99.71885 - 382.2867*x + 398.2079*x^2
            float distanceSLR = getDistance(99.71885, 384.0377, 400.3844, SRL.read()); // TODO: put parameter for this sensor for a and b
            // backSLR(speedSLR, durationSLR);

            // move back SMR - Left
            // y = 99.97613 - 384.0377*x + 400.3844*x^2
            float distanceSMR = getDistance(99.97613, 384.0377, 400.3844, SMR.read()); // TODO: put parameter for this sensor for a and b
            // backSMR(speedSMR, durationSMR);
        
            if(distanceSMR <= 9.5 && distanceSLR <= 9.5 && distanceSML > 9.5){
                MB.speed(0.7);
                LA.speed(0.7);
                RA.speed(0.0);
            } else if(distanceSMR > 9.5 && distanceSLR > 9.5 && distanceSML <= 9.5){
                MA.speed(0.7);
                LB.speed(0.7);
                RA.speed(0.0);
            }
            
            if(distanceSML <= 9.5 && distanceSLR <= 9.5 && distanceSMR > 9.5 ){  
                RB.speed(0.7);
                MA.speed(0.7);
                LA.speed(0.0);
            } else if(distanceSML > 9.5 && distanceSLR > 9.5 && distanceSMR <= 9.5 ){  
                RA.speed(0.7);
                MB.speed(0.7);
                LA.speed(0.0);
            }

            if(distanceSML <= 9.5 && distanceSMR <= 9.5 && distanceSLR > 9.5){
                LB.speed(0.7);
                RA.speed(0.7);
                MB.speed(0.0);     
            } else if(distanceSML > 9.5 && distanceSMR > 9.5 && distanceSLR <= 9.5){
                LA.speed(0.7);
                RB.speed(0.7);
                MB.speed(0.0);
            }
        }


        stop();
    }    
}