Diff: trace.cpp

Revision:

5:477e7d629ea8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/trace.cpp	Fri Nov 07 11:28:31 2014 +0000
@@ -0,0 +1,122 @@
+#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 sensor5(P0_22);
+
+DigitalOut led(P1_28);
+ 
+MotorDriver_SU motor(MOTOR_DOUBLE);
+MotorDriver_SU lift(MOTOR_SINGLE);
+ 
+int main(){
+    while(1){
+    int height = 0;//リフト位置指定
+    while(sw == 0){
+                motor.Drive(0, STOP, 0); //右タイヤ
+                motor.Drive(1, STOP, 0);//左タイヤ
+                lift.Drive(0, STOP, 0);
+        }
+    while(sw == 1){
+        //patern1
+            if(sensor1 == 1 && sensor2 == 1 && sensor3 == 0){
+                motor.Drive(0, CW, 0); //右タイヤ
+                motor.Drive(1, CW, 2000);//左タイヤ
+                lift.Drive(0, STOP, 0);
+                }
+        //patern2
+            else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1){
+                motor.Drive(0, CW, 1000);
+                motor.Drive(1, CW, 1000);
+                lift.Drive(0, STOP, 0);
+                }
+        //patern3
+            else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){
+                motor.Drive(0, CW, 2000);
+                motor.Drive(1, CW, 0);
+                lift.Drive(0, STOP, 0);
+                }
+        //patern4
+            else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 1){
+                motor.Drive(0, CW, 0);
+                motor.Drive(1, CW, 1500);
+                lift.Drive(0, STOP, 0);
+                }
+        //patern5
+        else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 0){
+            motor.Drive(0, CW, 1500);
+            motor.Drive(1, CW, 0);
+            lift.Drive(0, STOP, 0);
+            }
+        //patern6
+            else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 0){
+                motor.Drive(0, CW, 1000);
+                motor.Drive(1, CW, 1000);
+                lift.Drive(0, STOP, 0);
+                }
+        
+        //patern7
+            else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 0){
+                motor.Drive(0, CW, 1000);
+                motor.Drive(1, CW, 1000);
+                lift.Drive(0, STOP, 0);
+                }
+        
+        //patern8 探索回転方向
+        //ラインの右側に出た場合 反時計回り
+            /*else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1){
+                    motor.Drive(0, CW, 2048);
+                    motor.Drive(1, STOP, 0);
+                    lift.Drive(0, STOP, 0);
+                    }*/
+        //ラインの左側に出た場合 時計回り
+            else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 ){
+                    motor.Drive(0, STOP, 0);
+                    motor.Drive(1, CW, 1500);
+                    lift.Drive(0, STOP, 0);
+                    }
+            
+        //リフト判定
+        else if(sensor4 == 0 || sensor5 == 0){
+                    if( height == 0){
+                    //ストップ
+                    motor.Drive(0, STOP, 0);
+                    motor.Drive(1, STOP, 0);
+            
+                    lift.Drive(0, CW, 4000); //リフトアップ
+                    wait(1.0);
+                    lift.Drive(0, STOP, 0);
+                    wait(0.2);
+                     //再スタート
+                    motor.Drive(0, CW, 1000);
+                    motor.Drive(1, CW, 1000);
+                    wait(0.2);
+                    height = 1;//lift位置
+                    }
+                    else if(height == 1){
+                    motor.Drive(0, STOP, 0);
+                    motor.Drive(1, STOP, 0);
+                    lift.Drive(0, CCW, 2000);//リフトダウン
+                    wait(0.5);
+                    lift.Drive(0, STOP, 0);
+                    motor.Drive(0, CCW, 4000);
+                    motor.Drive(1, CCW, 4000);
+                    wait(3);
+                    height = 0;
+                    }
+                }
+                
+            else{
+                motor.Drive(0, CW, 500);
+                motor.Drive(1, CW, 500);
+                lift.Drive(0, STOP, 0);
+                }
+            }
+    }            
+}