Diff: liftver2.0.cpp

Revision:

0:57da1a182e99

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/liftver2.0.cpp	Thu Nov 27 04:22:11 2014 +0000
@@ -0,0 +1,84 @@
+#include "mbed.h"
+#include "MotorDriver_SU.h"
+DigitalIn sw(P0_1,PullUp);
+AnalogIn sensorleft(P0_23),sensorcenter(P0_14),sensorright(P0_12),sensorlift1(P0_22),sensorlift2(P0_21);
+MotorDriver_SU motor(MOTOR_DOUBLE);
+MotorDriver_SU lift(MOTOR_SINGLE);
+int main() {
+    float hoge,pool,fink,coen,ronin;
+    while(sw == 1){
+        ;
+        }
+        wait(0.1);
+        while(sw == 0){
+            ;
+            }
+    while(1){
+        hoge = sensorleft.read();
+        pool = sensorcenter.read();
+        fink = sensorright.read();
+        coen = sensorlift1.read();
+        ronin = sensorlift2.read();
+    if(coen < 0.7 && ronin < 0.7){
+        motor.Drive(0,STOP,0);
+        motor.Drive(1,STOP,0);
+        lift.Drive(0,CW,3000);
+        wait(1.5);
+        while(1){
+            lift.Drive(0,STOP,0);
+            motor.Drive(0,CW,1200);
+            motor.Drive(1,CW,1200);
+            wait(0.3);
+        while(1){
+        hoge = sensorleft.read();
+        pool = sensorcenter.read();
+        fink = sensorright.read();
+        coen = sensorlift1.read();
+        ronin = sensorlift2.read();
+        if(coen < 0.7 && ronin < 0.7){
+            motor.Drive(0,STOP,0);
+            motor.Drive(1,STOP,0);
+            lift.Drive(0,CCW,3000);
+            wait(1.5);
+            while(1){
+            lift.Drive(0,STOP,0);
+            motor.Drive(0,CCW,1500);
+            motor.Drive(1,CCW,1500);
+            }
+        }
+        else if(fink < 0.7){
+                motor.Drive(0,CW,0);
+                motor.Drive(1,CW,1100);
+        }
+        else if(hoge < 0.7){
+                motor.Drive(0,CW,1100);
+                motor.Drive(1,CW,0);
+        }
+        else if(pool < 0.7){
+                motor.Drive(0,CW,1150);
+                motor.Drive(1,CW,1200);
+        }
+        else{
+                lift.Drive(0,STOP,0);
+        }
+    }
+    }
+    }
+    else if(fink < 0.7){
+            motor.Drive(0,CW,0);
+            motor.Drive(1,CW,850);
+        }
+    else if(hoge < 0.7){
+            motor.Drive(0,CW,850);
+            motor.Drive(1,CW,0);
+    }
+    else if(pool < 0.7){
+            motor.Drive(0,CW,850);
+            motor.Drive(1,CW,850);
+    }
+    else{
+        lift.Drive(0,STOP,0);
+          
+    }
+}
+}
\ No newline at end of file