1113
Dependencies: MotorDriver_SU mbed
Fork of tracer1106 by
Diff: trace.cpp
- Revision:
- 9:db0dcd64bb93
- Parent:
- 8:8e263a17efdf
diff -r 8e263a17efdf -r db0dcd64bb93 trace.cpp --- a/trace.cpp Thu Nov 13 08:09:02 2014 +0000 +++ b/trace.cpp Mon Nov 17 10:56:17 2014 +0000 @@ -6,7 +6,7 @@ DigitalIn sensor1(P0_16); DigitalIn sensor2(P0_13); DigitalIn sensor3(P0_11); -// lift r to l +// lift L to R DigitalIn sensor4(P0_21); DigitalIn sensor5(P0_22); @@ -26,39 +26,43 @@ while(sw == 1){ //patern1 if(sensor1 == 1 && sensor2 == 1 && sensor3 == 0){ - motor.Drive(0, CW, 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, 1000); - motor.Drive(1, CW, 1000); + 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(1.5); + wait(0.7); lift.Drive(0, STOP, 0); - wait(0.2); + //wait(0.1); //再スタート - motor.Drive(0, CW, 1500); - motor.Drive(1, CW, 1500); + 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); + wait(1.3); + motor.Drive(0, CCW, 4095); + motor.Drive(1, CCW, 4095); lift.Drive(0, STOP, 0); - motor.Drive(0, CCW, 4000); - motor.Drive(1, CCW, 4000); + wait(2); height = 0; } @@ -66,7 +70,7 @@ //patern3 else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){ motor.Drive(0, CW, 2000); - motor.Drive(1, CW, 0); + motor.Drive(1, STOP, 0); lift.Drive(0, STOP, 0); } //patern4 @@ -105,43 +109,46 @@ //ラインの左側に出た場合 時計回り else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 ){ motor.Drive(0, STOP, 0); - motor.Drive(1, CW, 1500); + motor.Drive(1, CW, 2000); lift.Drive(0, STOP, 0); } //リフト判定 - else if(sensor4 == 0 && sensor5 == 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(1.5); + wait(0.7); lift.Drive(0, STOP, 0); - wait(0.2); + //wait(0.2); //再スタート - motor.Drive(0, CW, 1500); - motor.Drive(1, CW, 1500); + 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); + wait(1.3); + motor.Drive(0, CCW, 4095); + motor.Drive(1, CCW, 4095); lift.Drive(0, STOP, 0); - motor.Drive(0, CCW, 4000); - motor.Drive(1, CCW, 4000); wait(2); height = 0; } } else{ - motor.Drive(0, CW, 500); - motor.Drive(1, CW, 500); + motor.Drive(0, CW, 1000); + motor.Drive(1, CW, 1000); lift.Drive(0, STOP, 0); } }