a
Dependencies: MotorDriver_SU mbed
Diff: c.cpp
- Revision:
- 0:a421b8dde3ab
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/c.cpp Thu Dec 04 06:49:04 2014 +0000 @@ -0,0 +1,162 @@ +#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 L to R +DigitalIn sensor4(P0_21); +DigitalIn sensor6(P0_14); +DigitalIn sensor5(P0_22); +DigitalIn sensor7(P0_23); + +DigitalOut led(P1_28); + +MotorDriver_SU motor(MOTOR_DOUBLE); +MotorDriver_SU lift(MOTOR_SINGLE); + +int main(){ + while(1){ + + //int height = 0;//lift position + int check1 = 0;//convert count + /*int check2 = 0;*/ + int count = 0;//count liftsensor B&B + while(sw == 0){ + motor.Drive(0, STOP, 0); //R + motor.Drive(1, STOP, 0);//L + lift.Drive(0, STOP, 0); + } +////////////////////////////////////////////////////////// + //need to change some numerical value after test +////////////////////////////////////////////////////////// + while(sw == 1){ + if(sensor1 == 1 && sensor2 == 1 && sensor3 == 0){ + motor.Drive(0, CW, 500); + motor.Drive(1, CW, 1500); + } + else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1){ + 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)/*||(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 0 && sensor7 == 0)||(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor6 == 0 && sensor5 == 0)||(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor5 == 0 && sensor7 == 0)*/){ + led = 1;//count kakuninn + if( count == 0){ + lift.Drive(0, CW, 4095); //lift up + motor.Drive(0, CW, 1300); //restart + motor.Drive(1, CW, 1300); + /*motor.Drive(0, STOP, 0); //stop + motor.Drive(1, STOP, 0);*/ + wait(1.6); + + led = 0;//count kakuninn + /* motor.Drive(0, CW, 2000); //restart + motor.Drive(1, CW, 2000); + lift.Drive(0, CW, 4095); //lift up more + wait(0.7);*/ + lift.Drive(0, STOP, 0); + //height = 1;//lift position high + count++; + } + //need to change count[1,2,3...] after test + else if(count == 1){ + count = 3; + } + else if(count == 3){ + motor.Drive(0, STOP, 0); + motor.Drive(1, STOP, 0); + lift.Drive(0, CCW, 3000);//lift down + wait(1.2); + motor.Drive(0, CCW, 4095);//back for 2 seconds + motor.Drive(1, CCW, 4095); + lift.Drive(0, STOP, 0); + led = 0;//count kakunin + wait(2); + //height = 0;//lift position low + count = 0;//reset + } + else{ + ; + } + } + else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){ + motor.Drive(0, CW, 1500); + motor.Drive(1, CW, 500); + } + else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 1){ + motor.Drive(0, CW, 1875); + motor.Drive(1, CW, 1500); + } + else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 0){ + motor.Drive(0, CW, 1500); + motor.Drive(1, CW, 1875); + } + else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 0){ + motor.Drive(0, CW, 500);//ccw change cw + motor.Drive(1, CW, 500);//ccw change cw + } + /*else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1){ + motor.Drive(0, CW, 2048); + motor.Drive(1, STOP, 0); + lift.Drive(0, STOP, 0); + }*/ + //all body on line's outside + else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 ){ + motor.Drive(0, CCW, 200); + motor.Drive(1, CW, 2000); + lift.Drive(0, STOP, 0); + } + //select course condition + else if((sensor2 == 0 && sensor4 == 1 && sensor5 == 0 )||(sensor2 == 0 && sensor4 == 1 && sensor7 == 0 )||(sensor2 == 0 && sensor6 == 1 && sensor5 == 0)||(sensor2 == 0 && sensor6 == 1 && sensor7 == 0)){ + if(check1 == 0){ + check1 = 1; + motor.Drive(0, CW, 1500);//adjust + motor.Drive(1, CW, 1500); + wait(0.5);//adjust + //enter the right courses + motor.Drive(0, CW, 500);//change both speed + motor.Drive(1, CW, 2500); + wait(1); + motor.Drive(0, CW, 1500);//adjust + motor.Drive(1, CW, 1500); + wait(0.15);//adjust + motor.Drive(0, CW, 1200);//change both speed + motor.Drive(1, CCW, 200); + wait(0.6); + /*motor.Drive(0, CW, 1500);//adjust + motor.Drive(1, CW, 1500); + wait(0.1);//adjust*/ + } + else{ + ;//response only once + } + } + /*else if((sensor2 == 0 && sensor4 == 0 && sensor5 == 1)||(sensor2 == 0 && sensor4 == 0 && sensor7 == 1)||(sensor2 == 0 && sensor6 == 0 && sensor5 == 1)||(sensor2 == 0 && sensor6 == 0 && sensor7 == 1)){ + if(check2 == 0){ + check2 = 1; + //enter the center course + motor.Drive(0, CW, 800);//change both speed + motor.Drive(1, CCW, 500); + wait(1); + } + else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 0 ) { + motor.Drive(0, CW, 500); + motor.Drive(1, CW, 500); + }//tuika + + + else{ + ;//response only once + } + } + */ + else{ + motor.Drive(0, CW, 2500);//spin to search correct way + motor.Drive(1, CW, 500); + } + } + } +} \ No newline at end of file