File content as of revision 0:0db8fdcf026f:

#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_16);
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 position = 0;
    int AI = 0;//カーブの方向判定
    int updown = 0;//リフトの向き判定
    int R = 0;
    int L = 0;
    
        while(1){
        
        hoge1 = sw1.read();
        hoge2 = sw2.read();
        hoge3 = sw3.read();
        hoge4 = sw4.read();
        hoge5 = sw5.read();
        
        while(sw == 0){
                motor.Drive(0, STOP, 0); 
                motor.Drive(1, STOP, 0);       
        }
        /*黒＿白＿白＿白＿白*/
        if(sw1  <= 0.6 && sw2 > 0.6 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){   
                motor.Drive(0, CW, 500);           
                motor.Drive(1, CW, 2000);
        }
        /*白＿黒＿白＿白＿白*/
        else if(sw1  > 0.6 && sw2 <= 0.3 && sw3 > 0.6 && sw4 > 0.6 && sw5 > 0.6){   
                motor.Drive(0, CW, 1500);           
                motor.Drive(1, CW, 2000);
        }
        /*白＿黒＿黒＿白＿白*/
        else if(sw1  > 0.6 && sw2 <= 0.6 && sw3 <= 0.6 && sw4 > 0.6 && sw5 > 0.6){   
                motor.Drive(0, CW, 1800);           
                motor.Drive(1, CW, 2000);
        }
        /*白＿白＿黒＿白＿白*/
        else if(sw1  > 0.6  && sw2  > 0.6 && sw3 <= 0.3 && sw4  > 0.6 && sw5 > 0.6){
                motor.Drive(0, CW, 2000);           
                motor.Drive(1, CW, 2000); 
        }
                
        /*白＿白＿黒＿黒＿白*/
        else if(sw1  > 0.6 && sw2 > 0.6 && sw3 <= 0.6 && sw4 <= 0.6 && sw5 > 0.6){   
                motor.Drive(0, CW, 2000);           
                motor.Drive(1, CW, 1800);
        }
        /*白＿白＿白＿黒＿白*/
        else if(sw1  > 0.6 && sw2  > 0.6 && sw3  > 0.6 && sw4 <= 0.3 && sw5 > 0.6){  
                motor.Drive(0, CW, 2000);           
                motor.Drive(1, CW, 1500);
        }
        /*白＿白＿白＿白＿黒*/
        else if(sw1  > 0.6 && sw2  > 0.6 && sw3  > 0.6 && sw4 > 0.6 && sw5 <= 0.6){  
                motor.Drive(0, CW, 2000);           
                motor.Drive(1, CW, 500);
        }
        
        ///////////大きい右カーブを曲がるためのプログラム//////////
        /*1センサ2-5が黒（センサ5によって右のカーブであることを判定する）*/
        else if(sw1  > 0.5 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 &&sw5  <= 0.3){   
            AI = 1;
        }
        
        /*2センサ3-5が黒（センサ5によって右のカーブであることを判定する）*/
        else if(sw1  > 0.5 && sw2 > 0.5 && sw3 <= 0.3 &&  sw4 > 0.5 && sw5  <= 0.3){   
            AI = 1;
        }
        /*3センサ4-5が黒（センサ5によって右のカーブであることを判定する）*/
        else if(sw1  > 0.5 && sw2 > 0.5 && sw3 > 0.5 &&  sw4 <= 0.3 && sw5  <= 0.3){   
            AI = 1;
        }
        
        /*4センサ2-3-5が黒（センサ5によって右のカーブであることを判定する）*/
        else if(sw1  > 0.5 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 <= 0.3){   
            AI = 1;
        }
        
        /*5センサ3-4-5が黒（センサ5によって右のカーブであることを判定する）*/
        else if(sw1  > 0.5 && sw2 > 0.5 && sw3 <= 0.3 && sw4 <= 0.3 && sw5 <= 0.3){   
            AI = 1;
        }
        
        ///////////大きい左カーブを曲がるためのプログラム//////////
        /*6センサ1-2が黒（センサ1によって右のカーブであることを判定する）*/
        else if(sw1  <= 0.3 && sw2 <= 0.3 && sw3 > 0.5 && sw4 > 0.5 && sw5 > 0.5){   
            AI = 2;
        }
        
        /*7センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/
        else if(sw1  <= 0.3 && sw2 > 0.5 && sw3 <= 0.3 && sw4 > 0.5 && sw5 > 0.5){   
            AI = 2;
        }
        
        /*8センサ1-4が黒（センサ1によって右のカーブであることを判定する）*/
        else if(sw1  <= 0.3 && sw2 > 0.5 && sw3 > 0.5 && sw4 <= 0.3 && sw5 > 0.5){   
            AI = 2;
        }
        
        /*9センサ1-2-3が黒（センサ1によって右のカーブであることを判定する）*/
        else if(sw1  <= 0.3 && sw2 <= 0.5 && sw3 <= 0.5 && sw4 > 0.5 && sw5 > 0.5){   
            AI = 2;
        }
        
        /*10センサ1-3が黒（センサ1によって右のカーブであることを判定する）*/
        else if(sw1  <= 0.3 && sw2 > 0.5 && sw3 <= 0.5 && sw4 <= 0.5 && sw5 > 0.5){   
            AI = 2;
        }
        
        
        
        /*11全部白かつ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, 1200);         
            motor.Drive(1, CCW, 1200); 
        }
        /*12全部白かつ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, 1200);         
            motor.Drive(1, CW, 1200);  
        }
        
        /**********************************/
        /*停止位置で止まるためのプログラム*/
        /**********************************/
        
        /*1.4.1 センサ1-5が黒*/
        else if(sw1  <= 0.5 && sw5 <= 0.5){
               
            
                      
            if( updown == 0 ){
                ///////////停止//////////
                motor.Drive(0, STOP, 0);           
                motor.Drive(1, STOP, 0);
                wait(0.1);
                
                ///////////リフト上げる//////////
                lift.Drive(0, CCW, 2000);
                wait(1.5);
                ///////////リフト停止//////////
                lift.Drive(0, STOP, 0);
                wait(0.1);
                /*
                ///////////停止位置を超えるために少し前進//////////
                
                motor.Drive(0, CW, 1200);           
                motor.Drive(1, CW, 1200);
                wait(0.1);
                */
                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(1.2);
                ///////////モータ停止//////////
                motor.Drive(0, STOP, 0);           
                motor.Drive(1, STOP, 0);
                wait(1.0);
                updown = 0;
                break;
            }
        }
            
        
        
            
            
}
}