Diff: tracer.cpp

Revision:

2:078d6c4bf9ee

Parent:

1:faa0499314d3

Child:

3:cd48c06fac88

--- a/tracer.cpp	Thu Oct 30 11:45:35 2014 +0000
+++ b/tracer.cpp	Wed Nov 05 07:30:35 2014 +0000
@@ -1,108 +1,100 @@
 #include "mbed.h"
 #include "MotorDriver_SU.h"
-
+ 
 // trace L to R
 DigitalIn sensor1(P0_16);
 DigitalIn sensor2(P0_13);
 DigitalIn sensor3(P0_11);
 // lift L to R
-DigitalIn sensor4(P1_27);
-DigitalIn sensor5(P0_4);
-
+DigitalIn sensor4(P0_12);
+DigitalIn sensor5(P0_14);
+ 
 MotorDriver_SU motor(MOTOR_DOUBLE);
 MotorDriver_SU lift(MOTOR_SINGLE);
-
-int height = 0;
-int a = 0;
-
+ 
 int main(){
+    int height = 0;//リフト位置指定
     while(1){
-        while(a == 0){
         //patern1
             if(sensor1 == 1 && sensor2 == 1 && sensor3 == 0){
-                motor.Drive(0, CW, 300); //右タイヤ
-                motor.Drive(1, CW, 1500);//左タイヤ
+                motor.Drive(0, CW, 0); //右タイヤ
+                motor.Drive(1, CW, 2000);//左タイヤ
                 }
         //patern2
             else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1){
-                motor.Drive(0, CW, 1500);
-                motor.Drive(1, CW, 1500);
+                motor.Drive(0, CW, 1000);
+                motor.Drive(1, CW, 1000);
                 }
         //patern3
             else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){
-                motor.Drive(0, CW, 1500);
-                motor.Drive(1, CW, 300);
+                motor.Drive(0, CW, 2000);
+                motor.Drive(1, CW, 0);
                 }
         //patern4
             else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 1){
-                motor.Drive(0, CW, 800);
+                motor.Drive(0, CW, 0);
                 motor.Drive(1, CW, 1500);
                 }
         //patern5
         else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 0){
             motor.Drive(0, CW, 1500);
-            motor.Drive(1, CW, 800);
+            motor.Drive(1, CW, 0);
             }
         //patern6
             else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 0){
-                motor.Drive(0, CW, 1500);
-                motor.Drive(1, CW, 1500);
+                motor.Drive(0, CW, 1000);
+                motor.Drive(1, CW, 1000);
                 }
         
         //patern7
             else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 0){
-                motor.Drive(0, CW, 1500);
-                motor.Drive(1, CW, 1500);
+                motor.Drive(0, CW, 1000);
+                motor.Drive(1, CW, 1000);
                 }
-        /*
+        
         //patern8 探索回転方向
         //ラインの右側に出た場合 反時計回り
-            else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && hasegawa == 0){
+            /*else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1){
                     motor.Drive(0, CW, 2048);
-                    motor.Drive(1, CW, 1000);
+                    motor.Drive(1, STOP, 0);
                     }*/
-        /*//ラインの左側に出た場合 時計回り
-            else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && hasegawa == 1){
-                    motor.Drive(0, CW, 1000);
-                    motor.Drive(1, CW, 2048);
-                    }*/
+        //ラインの左側に出た場合 時計回り
+            else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 ){
+                    motor.Drive(0, STOP, 0);
+                    motor.Drive(1, CW, 1500);
+                    }
             
         //リフト判定
-        /*  else if(sensor4 == 0 && sensor5 == 0, height == 0){
+        else if(sensor4 == 0 && sensor5 == 0, height == 0){
                     //ストップ
                     motor.Drive(0, STOP, 0);
                     motor.Drive(1, STOP, 0);
             
                     lift.Drive(0, CW, 2000); //リフトアップ
-                    wait(4);
+                    wait(2);
                     lift.Drive(0, STOP, 0);
-                    height = 1; //lift位置
+                    height = 1;//lift位置
+                    wait(0.2);
                      //再スタート
-                    motor.Drive(0, CW, 1500);
-                    motor.Drive(1, CW, 1500);
+                    motor.Drive(0, CW, 1000);
+                    motor.Drive(1, CW, 1000);
                     }
             else if(sensor4 == 0 && sensor5 == 0, height == 1){
                     motor.Drive(0, STOP, 0);
                     motor.Drive(1, STOP, 0);
             
                     lift.Drive(0, CCW, 2000);//リフトダウン
-                    wait(4);
+                    wait(2);
                     lift.Drive(0, STOP, 0);
-            
-                    a = 1;// スルー判定
-                    }*/
+                    motor.Drive(0, CCW, 4000);
+                    motor.Drive(1, CCW, 4000);
+                    height = 0;
+                    }
             
             else{
-                motor.Drive(0, CW, 1500);
-                motor.Drive(1, CW, 1500);
+                motor.Drive(0, CW, 1000);
+                motor.Drive(1, CW, 1000);
                 }
-        }
-        //後退
-        /*while( a == 1){
-                    motor.Drive(0, CCW, 3000);
-                    motor.Drive(1, CCW, 3000);
-                    }*/
-            
-    }
+    }            
+}
     
-}
\ No newline at end of file