File content as of revision 9:db0dcd64bb93:

#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;//リフト位置指定
    while(sw == 0){
                motor.Drive(0, STOP, 0); //右タイヤ
                motor.Drive(1, STOP, 0);//左タイヤ
                lift.Drive(0, STOP, 0);
        }
    while(sw == 1){
        //patern1
            if(sensor1 == 1 && sensor2 == 1 && sensor3 == 0){
                motor.Drive(0, CCW, 200); //右タイヤ
                motor.Drive(1, CW, 2000);//左タイヤ
                lift.Drive(0, STOP, 0);
                }
        //patern2
            else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1){
                motor.Drive(0, CW, 2500);
                motor.Drive(1, CW, 2500);
                lift.Drive(0, STOP, 0);
                }
            else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 0 && sensor5 == 0){
                if( height == 0){
                    //ストップ
                    lift.Drive(0, CW, 4095); //リフトアップ
                    motor.Drive(0, STOP, 0);
                    motor.Drive(1, STOP, 0);
                    wait(1);
                    motor.Drive(0, CW, 2000);
                    motor.Drive(1, CW, 2000); 
                    lift.Drive(0, CW, 4095); //リフトアップ
                    wait(0.7);
                    lift.Drive(0, STOP, 0);
                    //wait(0.1);
                     //再スタート
                    motor.Drive(0, CW, 2500);
                    motor.Drive(1, CW, 2500);
                    height = 1;//lift位置
                    }
                    else if(height == 1){
                    motor.Drive(0, STOP, 0);
                    motor.Drive(1, STOP, 0);
                    lift.Drive(0, CCW, 2000);//リフトダウン
                    wait(1.3);
                    motor.Drive(0, CCW, 4095);
                    motor.Drive(1, CCW, 4095);
                    lift.Drive(0, STOP, 0);

                    wait(2);
                    height = 0;
                    }
                }
        //patern3
            else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){
                motor.Drive(0, CW, 2000);
                motor.Drive(1, STOP, 0);
                lift.Drive(0, STOP, 0);
                }
        //patern4
            else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 1){
                motor.Drive(0, CW, 0);
                motor.Drive(1, CW, 1500);
                lift.Drive(0, STOP, 0);
                }
        //patern5
        else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 0){
            motor.Drive(0, CW, 1500);
            motor.Drive(1, CW, 0);
            lift.Drive(0, STOP, 0);
            }
        //patern6
            else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 0){
                motor.Drive(0, CW, 1000);
                motor.Drive(1, CW, 1000);
                lift.Drive(0, STOP, 0);
                }
        
        //patern7
            else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 0){
                motor.Drive(0, CW, 1000);
                motor.Drive(1, CW, 1000);
                lift.Drive(0, STOP, 0);
                }
        
        //patern8 探索回転方向
        //ラインの右側に出た場合 反時計回り
            /*else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1){
                    motor.Drive(0, CW, 2048);
                    motor.Drive(1, STOP, 0);
                    lift.Drive(0, STOP, 0);
                    }*/
        //ラインの左側に出た場合 時計回り
            else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 ){
                    motor.Drive(0, STOP, 0);
                    motor.Drive(1, CW, 2000);
                    lift.Drive(0, STOP, 0);
                    }
            
        //リフト判定

        else if( (sensor4 == 0 && sensor2 == 0) || (sensor5 == 0 && sensor2 == 0)) {
                    if( height == 0){
                    //ストップ
                    lift.Drive(0, CW, 4095); //リフトアップ
                    motor.Drive(0, STOP, 0);
                    motor.Drive(1, STOP, 0);
                    wait(1);
                    motor.Drive(0, CW, 2000);
                    motor.Drive(1, CW, 2000);           
                    lift.Drive(0, CW, 4095); //リフトアップ
                    wait(0.7);
                    lift.Drive(0, STOP, 0);
                    //wait(0.2);
                     //再スタート
                    motor.Drive(0, CW, 2500);
                    motor.Drive(1, CW, 2500);
                    height = 1;//lift位置
                    }
                    else if(height == 1){
                    motor.Drive(0, STOP, 0);
                    motor.Drive(1, STOP, 0);
                    lift.Drive(0, CCW, 2000);//リフトダウン
                    wait(1.3);
                    motor.Drive(0, CCW, 4095);
                    motor.Drive(1, CCW, 4095);
                    lift.Drive(0, STOP, 0);
                    wait(2);
                    height = 0;
                    }
                }
                
            else{
                motor.Drive(0, CW, 1000);
                motor.Drive(1, CW, 1000);
                lift.Drive(0, STOP, 0);
                }
            }
    }            
}