Diff: trace.cpp

Revision:

9:db0dcd64bb93

Parent:

8:8e263a17efdf

--- 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);
                 }
             }