Diff: tracer.cpp

Revision:

1:faa0499314d3

Parent:

0:0f3034bec1f8

Child:

2:52a55b50025c

--- a/tracer.cpp	Thu Oct 23 06:55:41 2014 +0000
+++ b/tracer.cpp	Thu Oct 30 11:45:35 2014 +0000
@@ -1,82 +1,108 @@
 #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);
 
 MotorDriver_SU motor(MOTOR_DOUBLE);
 MotorDriver_SU lift(MOTOR_SINGLE);
 
-int f = 2048;
-int s = 1000;
-int hasegawa = 0;
+int height = 0;
+int a = 0;
 
 int main(){
     while(1){
+        while(a == 0){
         //patern1
-        if(sensor1 == 1 && sensor2 == 1 && sensor3 == 0){
-            motor.Drive(0, CW, 1000); //右タイヤ
-            motor.Drive(1, CW, 2408);//左タイヤ
-            
-            hasegawa = 1;
-            }
+            if(sensor1 == 1 && sensor2 == 1 && sensor3 == 0){
+                motor.Drive(0, CW, 300); //右タイヤ
+                motor.Drive(1, CW, 1500);//左タイヤ
+                }
         //patern2
-        else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1){
-            motor.Drive(0, CW, 2048);
-            motor.Drive(1, CW, 2048);
-            
-            hasegawa = 0;//回転方向判定
-            }
+            else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1){
+                motor.Drive(0, CW, 1500);
+                motor.Drive(1, CW, 1500);
+                }
         //patern3
-        else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){
-            motor.Drive(0, CW, 2048);
-            motor.Drive(1, CW, 1000);
-            
-            hasegawa = 0;
-            }
+            else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){
+                motor.Drive(0, CW, 1500);
+                motor.Drive(1, CW, 300);
+                }
         //patern4
-        else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 1){
-            motor.Drive(0, CW, 2048);
-            motor.Drive(1, CW, 1000);
-            
-            hasegawa = 0;
-            }
+            else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 1){
+                motor.Drive(0, CW, 800);
+                motor.Drive(1, CW, 1500);
+                }
         //patern5
         else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 0){
-            motor.Drive(0, CW, 1000);
-            motor.Drive(1, CW, 2048);
-            
-            hasegawa = 1;//回転方向判定
+            motor.Drive(0, CW, 1500);
+            motor.Drive(1, CW, 800);
             }
         //patern6
-        else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 0){
-            motor.Drive(0, CW, 2048);
-            motor.Drive(1, CW, 2048);
-            }
+            else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 0){
+                motor.Drive(0, CW, 1500);
+                motor.Drive(1, CW, 1500);
+                }
         
         //patern7
-        else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 0){
-            motor.Drive(0, CW, 2048);
-            motor.Drive(1, CW, 2048);
-            }
-        
+            else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 0){
+                motor.Drive(0, CW, 1500);
+                motor.Drive(1, CW, 1500);
+                }
+        /*
         //patern8 探索回転方向
         //ラインの右側に出た場合 反時計回り
-        else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && hasegawa == 0){
-            motor.Drive(0, CW, 2048);
-            motor.Drive(1, CW, 1000);
-            }
-        //ラインの左側に出た場合 時計回り
-        else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && hasegawa == 1){
-                motor.Drive(0, CW, 1000);
-                motor.Drive(1, CW, 2048);
-            }
-        else{
-            motor.Drive(0, CW, 2048);
-            motor.Drive(1, CW, 2048);
+            else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && hasegawa == 0){
+                    motor.Drive(0, CW, 2048);
+                    motor.Drive(1, CW, 1000);
+                    }*/
+        /*//ラインの左側に出た場合 時計回り
+            else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && hasegawa == 1){
+                    motor.Drive(0, CW, 1000);
+                    motor.Drive(1, CW, 2048);
+                    }*/
+            
+        //リフト判定
+        /*  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);
+                    lift.Drive(0, STOP, 0);
+                    height = 1; //lift位置
+                     //再スタート
+                    motor.Drive(0, CW, 1500);
+                    motor.Drive(1, CW, 1500);
+                    }
+            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);
+                    lift.Drive(0, STOP, 0);
+            
+                    a = 1;// スルー判定
+                    }*/
+            
+            else{
+                motor.Drive(0, CW, 1500);
+                motor.Drive(1, CW, 1500);
+                }
         }
-        
+        //後退
+        /*while( a == 1){
+                    motor.Drive(0, CCW, 3000);
+                    motor.Drive(1, CCW, 3000);
+                    }*/
+            
     }
     
 }
\ No newline at end of file