shinnosuke hanamura
1
Dependencies: MotorDriver_SU mbed
Fork of
finalcenter
by 
Naoki Hasegawa
Diff: center.cpp
- Revision:
- 1:cd2d0caaa312
- Parent:
- 0:cc0c03ee56b3
--- a/center.cpp Wed Nov 19 13:08:25 2014 +0000
+++ b/center.cpp Sat Dec 13 03:12:11 2014 +0000
@@ -8,7 +8,9 @@
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);
@@ -20,7 +22,7 @@
//int height = 0;//lift position
int check1 = 0;//convert count
- int check2 = 0;
+ /*int check2 = 0;*/
int count = 0;//count liftsensor B&B
while(sw == 0){
motor.Drive(0, STOP, 0); //R
@@ -36,39 +38,41 @@
motor.Drive(1, CW, 1500);
}
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, 2000);
+ motor.Drive(1, CW, 2000);
lift.Drive(0, STOP, 0);
}
- else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 0 && sensor5 == 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, STOP, 0); //stop
- motor.Drive(1, STOP, 0);
- wait(1);
- motor.Drive(0, CW, 1000); //restart
- motor.Drive(1, CW, 1000);
+ motor.Drive(0, CW, 1300); //restart
+ motor.Drive(1, CW, 1300);
+ /*motor.Drive(0, STOP, 0); //stop
+ motor.Drive(1, STOP, 0);*/
+ wait(1.8);
+ 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);
+ 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++;
- }
- else if(count == 2){
- count++;
+ count = 3;
}
else if(count == 3){
motor.Drive(0, STOP, 0);
motor.Drive(1, STOP, 0);
- lift.Drive(0, CCW, 2000);//lift down
+ lift.Drive(0, CCW, 3000);//lift down
wait(1.3);
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
@@ -79,19 +83,19 @@
}
else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){
motor.Drive(0, CW, 1500);
- motor.Drive(1, STOP, 500);
+ motor.Drive(1, CW, 500);
}
else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 1){
motor.Drive(0, CW, 1500);
- motor.Drive(1, CW, 500);
+ motor.Drive(1, CW, 1200);
}
else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 0){
- motor.Drive(0, CW, 500);
+ motor.Drive(0, CW, 1200);
motor.Drive(1, CW, 1500);
}
else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 0){
- motor.Drive(0, CCW, 500);
- motor.Drive(1, CCW, 500);
+ 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);
@@ -105,30 +109,49 @@
lift.Drive(0, STOP, 0);
}
//select course condition
- else if(sensor2 == 0 && sensor4 == 1 && sensor5 == 0){
+ 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(1);
+ 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){
+ /*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, 1500);//change both speed
- motor.Drive(1, CW, 500);
+ 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);