File content as of revision 0:b9f6821eb6b5:

#include "mbed.h"
#include "MotorDriver_SU.h"

DigitalIn   sw(P0_1, PullUp);
DigitalIn   rightline(P0_12);
DigitalIn   leftline(P0_14);
DigitalIn   rightlift(P0_22);
DigitalIn   leftlift(P0_23);
DigitalOut  led1(P1_20);  //left
DigitalOut  led2(P1_27);
DigitalOut  led3(P1_21);
DigitalOut  led4(P1_26);  //right

MotorDriver_SU motor(MOTOR_DOUBLE);
MotorDriver_SU lift(MOTOR_SINGLE);

int a = 0;

int main(){
    while(1){
        while(sw == 0){
            motor.Drive(0, STOP, 0);                     
            motor.Drive(1, STOP, 0);               
            lift.Drive(0, STOP, 0);
        }
        while(sw == 1){
            if((rightline == 0 && leftline == 1) && (rightlift == 1 && leftlift == 1)){    //WWBW
                motor.Drive(0, CW, 2200);                     
                motor.Drive(1, CW, 2200);               
                lift.Drive(0, STOP, 0);
                led1 = 0;
                led2 = 0;
                led3 = 1;
                led4 = 0;
            }
            
            else if((rightline == 0 && leftline == 0) && (rightlift == 0 && leftlift == 0)){ //BBBB
               if(a == 0){
                    led1 = 1;
                    led2 = 1;
                    led3 = 1;
                    led4 = 1;
                    motor.Drive(0, CW, 2000);       //0 = rightmotor, 1 = leftmotor              
                    motor.Drive(1, CW, 2000);               
                    lift.Drive(0, CCW, 4095);
                    wait(1.5);
                    lift.Drive(0, STOP, 0);
                    a = 1;
                }
                else{
                    led1 = 1;
                    led2 = 1;
                    led3 = 1;
                    led4 = 1;
                    motor.Drive(0, STOP, 0); 
                    motor.Drive(1, STOP, 0);               
                    lift.Drive(0, CW, 4095);
                    wait(1.5);
                    lift.Drive(0, STOP, 0);
                    motor.Drive(0, CCW, 4095); 
                    motor.Drive(1, CCW, 4095);
                    wait(1);
                }
            }
            else if((rightline == 0 && leftline == 0) && (rightlift == 1 && leftlift == 0)){    //BBBW
                motor.Drive(0, CW, 2200);                     
                motor.Drive(1, STOP, 0);               
                lift.Drive(0, STOP, 0);
                led1 = 1;
                led2 = 1;
                led3 = 1;
                led4 = 0;
            }
            else if((rightline == 0 && leftline == 0) && (rightlift == 0 && leftlift == 1)){    //WBBB
                motor.Drive(0, STOP, 0);                     
                motor.Drive(1, CW, 2200);               
                lift.Drive(0, STOP, 0);
                led1 = 0;
                led2 = 1;
                led3 = 1;
                led4 = 1;
            }
            else if((rightline == 0 && leftline == 0) && (rightlift == 1 && leftlift == 1)){    //WBBW  *
                motor.Drive(0, CW, 2200);                     
                motor.Drive(1, CW, 1100);               
                lift.Drive(0, STOP, 0);
                led1 = 0;
                led2 = 1;
                led3 = 1;
                led4 = 0;
            }
            
            else if((rightline == 1 && leftline == 1) && (rightlift == 1 && leftlift == 0)){    //BWWW  *
                motor.Drive(0, CW, 1500);                     
                motor.Drive(1, CW, 1000);               
                lift.Drive(0, STOP, 0);
                led1 = 1;
                led2 = 0;
                led3 = 0;
                led4 = 0;
            }
            else if((rightline == 1 && leftline == 1) && (rightlift == 0 && leftlift == 1)){    //WWWB
                motor.Drive(0, CW, 500);                     
                motor.Drive(1, CW, 1500);               
                lift.Drive(0, STOP, 0);
                led1 = 0;
                led2 = 0; 
                led3 = 0;
                led4 = 1;
            }
            else if((rightline == 1 && leftline == 1) && (rightlift == 1 && leftlift == 1)){    //WWWW
                motor.Drive(0, CCW, 650);                     
                motor.Drive(1, CW, 1500);               
                lift.Drive(0, STOP, 0);
                led1 = 0;
                led2 = 0;
                led3 = 0;
                led4 = 0;
            }
        
            else if((rightline == 0 && leftline == 1) && (rightlift == 0 && leftlift == 0)){    //BWBB
                 if(a == 0){
                    led1 = 1;
                    led2 = 0;
                    led3 = 1;
                    led4 = 1;
                    motor.Drive(0, CW, 2000);
                    motor.Drive(1, CW, 2000);               
                    lift.Drive(0, CCW, 4095);
                    wait(1.5);
                    lift.Drive(0, STOP, 0);
                    a = 1;
                }
                else{
                    led1 = 1;
                    led2 = 0;
                    led3 = 1;
                    led4 = 1;
                    motor.Drive(0, STOP, 0); 
                    motor.Drive(1, STOP, 0);               
                    lift.Drive(0, CW, 4095);
                    wait(1.5);
                    lift.Drive(0, STOP, 0);
                    motor.Drive(0, CCW, 4095); 
                    motor.Drive(1, CCW, 4095);
                    wait(1);
                }
            }
            else if((rightline == 0 && leftline == 1) && (rightlift == 1 && leftlift == 0)){    //BWBW
                motor.Drive(0, CW, 2200);                     
                motor.Drive(1, STOP, 0);               
                lift.Drive(0, STOP, 0);
                led1 = 1;
                led2 = 0;
                led3 = 1;
                led4 = 0;
            }
            else if((rightline == 0 && leftline == 1) && (rightlift == 0 && leftlift == 1)){    //WWBB
                motor.Drive(0, CW, 1000);                     
                motor.Drive(1, CW, 1500);               
                lift.Drive(0, STOP, 0);
                led1 = 0;
                led2 = 0;
                led3 = 1;
                led4 = 1;
            }
            
            else if((rightline == 1 && leftline == 0) && (rightlift == 0 && leftlift == 0)){    //BBWB
                if(a == 0){
                    led1 = 1;
                    led2 = 1;
                    led3 = 0;
                    led4 = 1;
                    motor.Drive(0, CW, 2000);
                    motor.Drive(1, CW, 2000);               
                    lift.Drive(0, CCW, 4095);
                    wait(1.5);
                    lift.Drive(0, STOP, 0);
                    a = 1;
                }
                else{
                    led1 = 1;
                    led2 = 1;
                    led3 = 0;
                    led4 = 1;
                    motor.Drive(0, STOP, 0); 
                    motor.Drive(1, STOP, 0);               
                    lift.Drive(0, CW, 4095);
                    wait(1.5);
                    lift.Drive(0, STOP, 0);
                    motor.Drive(0, CCW, 4095); 
                    motor.Drive(1, CCW, 4095);
                    wait(1);
                }
            }
            else if((rightline == 1 && leftline == 0) && (rightlift == 1 && leftlift == 0)){    //BBWW
                motor.Drive(0, CW, 2200);                     
                motor.Drive(1, STOP, 0);               
                lift.Drive(0, STOP, 0);
                led1 = 1;
                led2 = 1;
                led3 = 0;
                led4 = 0;
            }
            else if((rightline == 1 && leftline == 0) && (rightlift == 0 && leftlift == 1)){  //WBWB
                motor.Drive(0, STOP, 0);                     
                motor.Drive(1, CW, 2200);               
                lift.Drive(0, STOP, 0);
                led1 = 0;
                led2 = 1;
                led3 = 0;
                led4 = 1;
            }
            else{                                    //WBWW
                motor.Drive(0, CW, 2200);                     
                motor.Drive(1, CW, 1000);               
                lift.Drive(0, STOP, 0);
                led1 = 0;
                led2 = 1;
                led3 = 0;
                led4 = 0;
            }
        }
    }
}