1113
Dependencies: MotorDriver_SU mbed
Fork of tracer1106 by
trace.cpp
- Committer:
- 12F2041G
- Date:
- 2014-11-12
- Revision:
- 7:b2adb4c5124b
- Parent:
- 6:477e7d629ea8
- Child:
- 8:8e263a17efdf
File content as of revision 7:b2adb4c5124b:
#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 r to l 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, CW, 0); //右タイヤ 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, 1000); motor.Drive(1, CW, 1000); lift.Drive(0, STOP, 0); } else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 0 && sensor5 == 0){ if( height == 0){ //ストップ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); lift.Drive(0, CW, 4095); //リフトアップ wait(3); lift.Drive(0, STOP, 0); wait(0.2); //再スタート motor.Drive(0, CW, 1000); motor.Drive(1, CW, 1000); wait(0.2); height = 1;//lift位置 } else if(height == 1){ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); lift.Drive(0, CCW, 2000);//リフトダウン wait(0.5); lift.Drive(0, STOP, 0); motor.Drive(0, CCW, 4000); motor.Drive(1, CCW, 4000); wait(1); height = 0; } } //patern3 else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){ motor.Drive(0, CW, 2000); motor.Drive(1, CW, 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, 1500); lift.Drive(0, STOP, 0); } //リフト判定 else if((sensor4 == 0 && sensor5 == 0) || (sensor5 == 0 && sensor2 == 0)) { if( height == 0){ //ストップ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); lift.Drive(0, CW, 4095); //リフトアップ wait(3); lift.Drive(0, STOP, 0); wait(0.2); //再スタート motor.Drive(0, CW, 1000); motor.Drive(1, CW, 1000); wait(0.2); height = 1;//lift位置 } else if(height == 1){ motor.Drive(0, STOP, 0); motor.Drive(1, STOP, 0); lift.Drive(0, CCW, 2000);//リフトダウン wait(0.5); lift.Drive(0, STOP, 0); motor.Drive(0, CCW, 4000); motor.Drive(1, CCW, 4000); wait(1); height = 0; } } else{ motor.Drive(0, CW, 500); motor.Drive(1, CW, 500); lift.Drive(0, STOP, 0); } } } }