a

Dependencies:   MotorDriver_SU mbed

Fork of tracer by Naoki Hasegawa

tracer.cpp

Committer:
12F2041G
Date:
2014-10-23
Revision:
0:0f3034bec1f8
Child:
1:a9d9cd966fa4

File content as of revision 0:0f3034bec1f8:

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

DigitalIn sensor1(P0_16);
DigitalIn sensor2(P0_13);
DigitalIn sensor3(P0_11);

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

int f = 2048;
int s = 1000;
int hasegawa = 0;

int main(){
    while(1){
        //patern1
        if(sensor1 == 1 && sensor2 == 1 && sensor3 == 0){
            motor.Drive(0, CW, 1000); //右タイヤ
            motor.Drive(1, CW, 2408);//左タイヤ
            
            hasegawa = 1;
            }
        //patern2
        else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1){
            motor.Drive(0, CW, 2048);
            motor.Drive(1, CW, 2048);
            
            hasegawa = 0;//回転方向判定
            }
        //patern3
        else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){
            motor.Drive(0, CW, 2048);
            motor.Drive(1, CW, 1000);
            
            hasegawa = 0;
            }
        //patern4
        else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 1){
            motor.Drive(0, CW, 2048);
            motor.Drive(1, CW, 1000);
            
            hasegawa = 0;
            }
        //patern5
        else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 0){
            motor.Drive(0, CW, 1000);
            motor.Drive(1, CW, 2048);
            
            hasegawa = 1;//回転方向判定
            }
        //patern6
        else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 0){
            motor.Drive(0, CW, 2048);
            motor.Drive(1, CW, 2048);
            }
        
        //patern7
        else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 0){
            motor.Drive(0, CW, 2048);
            motor.Drive(1, CW, 2048);
            }
        
        //patern8 探索回転方向
        //ラインの右側に出た場合 反時計回り
        else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && hasegawa == 0){
            motor.Drive(0, CW, 2048);
            motor.Drive(1, CW, 1000);
            }
        //ラインの左側に出た場合 時計回り
        else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && hasegawa == 1){
                motor.Drive(0, CW, 1000);
                motor.Drive(1, CW, 2048);
            }
        else{
            motor.Drive(0, CW, 2048);
            motor.Drive(1, CW, 2048);
        }
        
    }
    
}