File content as of revision 3:e54506c893b7:

#include "mbed.h"
#include "MotorDriver_SU.h"
 
DigitalIn sensor1(P1_21);
DigitalIn sensor2(P0_21);
DigitalIn sensor3(P0_12);
DigitalIn sensor4(P0_13);
MotorDriver_SU motor(MOTOR_DOUBLE);
MotorDriver_SU lift(MOTOR_SINGLE);
 
int main(){
    int x = 0;
    
    while(1){
        if(sensor1 == 0 && sensor2 == 0){
            motor.Drive(0,CW,1500);
            motor.Drive(1,CW,1500);
            } 
        else if(sensor1 == 0 && sensor2 == 1){
                x = 1;
                motor.Drive(0,CW,0);
                motor.Drive(1,CW,1500);
                } 
        else if(sensor1 == 1 && sensor2 == 0){
                    x = 2;
                    motor.Drive(0,CW,1500);
                    motor.Drive(1,CW,0);
                    }
         else  if(x == 1 && sensor1 == 1 && sensor2 == 1){
         
                    motor.Drive(0,CW,0);
                    motor.Drive(1,CW,1500);
                    }
         else  if(x == 2 && sensor1 == 1 && sensor2 == 1){
                    motor.Drive(0,CW,1500);
                    motor.Drive(1,CW,0);
                    }
                    else  if(sensor3 == 0 && sensor4 == 0){
                    lift.Drive(0,CW,1500);
                    motor.Drive(0,CW,0);
                    motor.Drive(1,CW,0);
                    break;
                    }
                    }
    while(1){
        if(sensor1 == 0 && sensor2 == 0){
            motor.Drive(0,CW,1500);
            motor.Drive(1,CW,1500);
            } 
        else if(sensor1 == 0 && sensor2 == 1){
                x = 1;
                motor.Drive(0,CW,0);
                motor.Drive(1,CW,1500);
                } 
        else if(sensor1 == 1 && sensor2 == 0){
                    x = 2;
                    motor.Drive(0,CW,1500);
                    motor.Drive(1,CW,0);
                    }
         else  if(x == 1 && sensor1 == 1 && sensor2 == 1){
         
                    motor.Drive(0,CW,0);
                    motor.Drive(1,CW,1500);
                    }
         else  if(x == 2 && sensor1 == 1 && sensor2 == 1){
                    motor.Drive(0,CW,1500);
                    motor.Drive(1,CW,0);
                    }
                    else  if(sensor3 == 0 && sensor4 == 0){
                    lift.Drive(0,CCW,1500);
                    motor.Drive(0,CW,0);
                    motor.Drive(1,CW,0);
                    }
                    }
                    }