File content as of revision 0:a66927a110ab:

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


DigitalIn sw(P0_1, PullUp);
DigitalOut led(P1_28);
AnalogIn   sw1(P0_22);
AnalogIn   sw2(P0_12);
AnalogIn   sw3(P0_14);
AnalogIn   sw4(P0_11);
AnalogIn   sw5(P0_13);

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


int main( ){
    float hoge1;
    float hoge2;
    float hoge3;
    float hoge4;
    float hoge5;
    int AI = 0;
    int updown = 0;
        
        while(1){
        
        hoge1 = sw1.read();
        hoge2 = sw2.read();
        hoge3 = sw3.read();
        hoge4 = sw4.read();
        hoge5 = sw5.read();
        
        
        /************************************/
        /*ラインに沿って走るためのプログラム*/
        /************************************/
        ///////////まっすぐ走るためのプログラム//////////
        /*黒＿白＿白＿白＿白*/
        if(sw1  <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4  > 0.5 && sw5 > 0.5){     
            motor.Drive(0, CW, 500);           
            motor.Drive(1, CW, 2000);             
            
        }
        
        
        /*白＿黒＿白＿白＿白*/
        else if(sw1  > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){   
            motor.Drive(0, CW, 800);           
            motor.Drive(1, CW, 2000);           
            
        }
        /*白＿黒＿黒＿白＿白*/
        else if(sw1  > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){   
            motor.Drive(0, CW, 1000);         
            motor.Drive(1, CW, 2000);        
            
        }
        
        
        /*白＿白＿黒＿白＿白*/
        else if(sw1  > 0.5  && sw2  > 0.5 && sw3 <= 0.3 && sw4  > 0.5 && sw5 > 0.5){  
            motor.Drive(0, CW, 1600);        
            motor.Drive(1, CW, 1600);     
            
        }
        /*白＿白＿黒＿黒＿白*/
        else if(sw1  > 0.5 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){   
            motor.Drive(0, CW, 2000);         
            motor.Drive(1, CW, 1000);        
            
        }
        
        
        
        /*白＿白＿白＿黒＿白*/
        else if(sw1  > 0.5 && sw2  > 0.5 && sw3  > 0.5 && sw4 <= 0.5 && sw5 > 0.5){  
            motor.Drive(0, CW, 2000);       
            motor.Drive(1, CW, 800);           
            
        }
        
        
        /*白＿白＿白＿白＿黒*/
        else if(sw1  > 0.5 && sw2  > 0.5 && sw3  > 0.5 && sw4  > 0.5 && sw5 <= 0.3){  
            motor.Drive(0, CW, 2000);        
            motor.Drive(1, CW, 500);       
        
        }
        
        
        
        
        ///////////大きい右カーブを曲がるためのプログラム//////////
        /*1.2.1 センサ2-5が黒（センサ5によって右のカーブであることを判定する）*/
        else if(sw1  > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 &&sw5  <= 0.3){   
            AI = 1;
        }
        
        /*1.2.2 センサ3-5が黒（センサ5によって右のカーブであることを判定する）*/
        else if(sw1  > 0.5 && sw2 > 0.5 && sw3 <= 0.3 &&  sw4 > 0.5 && sw5  <= 0.3){   
            AI = 1;
        }
        /*1.2.3 センサ4-5が黒（センサ5によって右のカーブであることを判定する）*/
        else if(sw1  > 0.5 && sw2 > 0.5 && sw3 > 0.5 &&  sw4 <= 0.3 && sw5  <= 0.3){   
            AI = 1;
        }
        
        /*1.2..3 センサ2-3-5が黒（センサ5によって右のカーブであることを判定する）*/
        else if(sw1  > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.3){   
            AI = 1;
        }
        
        /*1.2.4 センサ3-4-5が黒（センサ5によって右のカーブであることを判定する）*/
        else if(sw1  > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 <= 0.3 && sw5 <= 0.3){   
            AI = 1;
        }
        
        /*1.2.5 全部白かつAI = 1*/
        else if(AI == 1 && sw1  > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){   
            motor.Drive(0, CW, 1800);         
            motor.Drive(1, CCW, 1800);        
            
        }
        
        ///////////大きい左カーブを曲がるためのプログラム//////////
        /*1.3.1 センサ1-2が黒（センサ1によって右のカーブであることを判定する）*/
        else if(sw1  <= 0.3 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){   
            AI = 2;
        }
        
        /*1.3.2 センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/
        else if(sw1  <= 0.3 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){   
            AI = 2;
        }
        
        /*1.3.3 センサ1-4が黒（センサ1によって右のカーブであることを判定する）*/
        else if(sw1  <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 > 0.5){   
            AI = 2;
        }
        
        /*1.3.4 センサ1-2-3が黒（センサ1によって右のカーブであることを判定する）*/
        else if(sw1  <= 0.3 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){   
            AI = 2;
        }
        
        /*1.3.5 センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/
        else if(sw1  <= 0.3 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){   
            AI = 2;
        }
        
        /*1.3.6 全部白かつAI = 2*/
        else if(AI == 2 && sw1  > 0.5 && sw2 > 0.5 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){   
            motor.Drive(0, CCW, 1800);         
            motor.Drive(1, CW, 1800);        
            
        }
        
        /**********************************/
        /*停止位置で止まるためのプログラム*/
        /**********************************/
        
        /*1.4.1 センサ1-5が黒*/
        else if(sw1  <= 0.5 && sw5 <= 0.5){     
            motor.Drive(0, STOP, 0);           
            motor.Drive(1, STOP, 0);             
            if( updown == 0 ){
                ///////////停止//////////
                motor.Drive(0, STOP, 0);           
                motor.Drive(1, STOP, 0);
                wait(0.1);
                ///////////リフト上げる//////////
                lift.Drive(0, CCW, 4095);
                wait(1.0);
                ///////////リフト停止//////////
                lift.Drive(0, STOP, 0);
                wait(0.1);
                ///////////停止位置を超えるために少し前進//////////
                motor.Drive(0, CW, 1500);           
                motor.Drive(1, CW, 1500);
                wait(1.0);
                updown = 1;
            }
            
            else if( updown == 1 ){
                ///////////停止//////////
                motor.Drive(0, STOP, 0);           
                motor.Drive(1, STOP, 0);
                wait(0.1);
                ///////////リフト下げる//////////
                lift.Drive(0, CW, 4095);
                wait(1.0);
                ///////////リフト停止//////////
                lift.Drive(0, STOP, 0);
                wait(0.1);
                ///////////バック//////////
                motor.Drive(0, CCW, 4095);           
                motor.Drive(1, CCW, 4095);
                wait(3.0);
                ///////////モータ停止//////////
                motor.Drive(0, STOP, 0);           
                motor.Drive(1, STOP, 0);
                wait(1.0);
                updown = 0;
                break;
            }
        }
        
}
}