File content as of revision 0:cc0c03ee56b3:

#include "mbed.h"
#include "MotorDriver_SU.h"
 
DigitalIn sw(P0_1, PullUp); 
// trace L to R
DigitalIn sensor1(P0_16);
DigitalIn sensor2(P0_13);
DigitalIn sensor3(P0_11);
// lift L to R
DigitalIn sensor4(P0_21);
DigitalIn sensor5(P0_22);

DigitalOut led(P1_28);
 
MotorDriver_SU motor(MOTOR_DOUBLE);
MotorDriver_SU lift(MOTOR_SINGLE);
 
int main(){
    while(1){
        
    //int height = 0;//lift position
    int check1 = 0;//convert count
    int check2 = 0;
    int count = 0;//count liftsensor B&B
    while(sw == 0){
                motor.Drive(0, STOP, 0); //R
                motor.Drive(1, STOP, 0);//L
                lift.Drive(0, STOP, 0);
        }
//////////////////////////////////////////////////////////        
        //need to change some numerical value after test
//////////////////////////////////////////////////////////
    while(sw == 1){
            if(sensor1 == 1 && sensor2 == 1 && sensor3 == 0){
                motor.Drive(0, CW, 500);
                motor.Drive(1, CW, 1500);
                }
            else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1){
                motor.Drive(0, CW, 1000);
                motor.Drive(1, CW, 1000);
                lift.Drive(0, STOP, 0);
                }
            else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 0 && sensor5 == 0){
                if( count == 0){
                    lift.Drive(0, CW, 4095); //lift up
                    motor.Drive(0, STOP, 0); //stop
                    motor.Drive(1, STOP, 0);
                    wait(1);
                    motor.Drive(0, CW, 1000); //restart
                    motor.Drive(1, CW, 1000); 
                    lift.Drive(0, CW, 4095); //lift up more
                    wait(0.7);
                    lift.Drive(0, STOP, 0);
                    //height = 1;//lift position high
                    count++;
                    }
                    //need to change count[1,2,3...] after test
                    else if(count == 1){
                            count++;
                            }
                    else if(count == 2){
                            count++;
                            }
                    else if(count == 3){
                    motor.Drive(0, STOP, 0);
                    motor.Drive(1, STOP, 0);
                    lift.Drive(0, CCW, 2000);//lift down
                    wait(1.3);
                    motor.Drive(0, CCW, 4095);//back for 2 seconds
                    motor.Drive(1, CCW, 4095);
                    lift.Drive(0, STOP, 0);
                    wait(2);
                    //height = 0;//lift position low
                    count = 0;//reset
                    }
                    else{
                            ;
                        }
                }
            else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){
                motor.Drive(0, CW, 1500);
                motor.Drive(1, STOP, 500);
                }
            else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 1){
                motor.Drive(0, CW, 1500);
                motor.Drive(1, CW, 500);
                }
            else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 0){
                motor.Drive(0, CW, 500);
                motor.Drive(1, CW, 1500);
                }
            else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 0){
                motor.Drive(0, CCW, 500);
                motor.Drive(1, CCW, 500);
                }
            /*else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1){
                motor.Drive(0, CW, 2048);
                motor.Drive(1, STOP, 0);
                lift.Drive(0, STOP, 0);
                }*/
        //all body on line's outside
            else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 ){
                    motor.Drive(0, CCW, 200);
                    motor.Drive(1, CW, 2000);
                    lift.Drive(0, STOP, 0);
                    }
        //select course condition
            else if(sensor2 == 0 && sensor4 == 1 && sensor5 == 0){
                        if(check1 == 0){
                            check1 = 1;
                        //enter the right courses
                            motor.Drive(0, CW, 500);//change both speed
                            motor.Drive(1, CW, 1500);
                            wait(1);
                            }
                        else{
                                ;//response only once
                            }
                    }
            else if(sensor2 == 0 && sensor4 == 0 && sensor5 == 1){
                        if(check2 == 0){
                            check2 = 1;
                    //enter the center course
                            motor.Drive(0, CW, 1500);//change both speed
                            motor.Drive(1, CW, 500);
                            wait(1);
                            }
                        else{
                                ;//response only once
                                }
                    }
            else{
                motor.Drive(0, CW, 2500);//spin to search correct way
                motor.Drive(1, CW, 500);
                }
            }
    }            
}