Diff: a.cpp

Revision:

4:02c4de3930a2

Parent:

3:e54506c893b7

--- a/a.cpp	Fri Nov 07 06:55:00 2014 +0000
+++ b/a.cpp	Mon Nov 17 05:40:32 2014 +0000
@@ -5,6 +5,7 @@
 DigitalIn sensor2(P0_21);
 DigitalIn sensor3(P0_12);
 DigitalIn sensor4(P0_13);
+DigitalIn sw(P0_1,PullUp);
 MotorDriver_SU motor(MOTOR_DOUBLE);
 MotorDriver_SU lift(MOTOR_SINGLE);
  
@@ -12,9 +13,59 @@
     int x = 0;
     
     while(1){
+        while ( sw == 0 )
+        {
+             motor.Drive(0,STOP,0);
+            motor.Drive(1,STOP,0);
+            }
         if(sensor1 == 0 && sensor2 == 0){
             motor.Drive(0,CW,1500);
             motor.Drive(1,CW,1500);
+            }
+        else if(sensor3 == 0 && sensor4 == 0){
+             lift.Drive(0,CCW,4000);
+                    motor.Drive(0,STOP,0);
+                    motor.Drive(1,STOP,0);
+                    wait (2.0);
+                    lift.Drive(0,STOP,0);
+                    break;
+                    } 
+        else if(sensor1 == 0 && sensor2 == 1){
+                x = 1;
+                motor.Drive(0,CW,0);
+                motor.Drive(1,CW,1500);
+                } 
+        else if(sensor1 == 1 && sensor2 == 0){
+                    x = 2;
+                    motor.Drive(0,CW,1500);
+                    motor.Drive(1,CW,0);
+                    }
+         else  if(x == 1 && sensor1 == 1 && sensor2 == 1){
+         
+                    motor.Drive(0,CW,0);
+                    motor.Drive(1,CW,1500);
+                    }
+         else  if(x == 2 && sensor1 == 1 && sensor2 == 1){
+                    motor.Drive(0,CW,1500);
+                    motor.Drive(1,CW,0);
+                    }
+                    }
+             motor.Drive(0,CW,1500);
+            motor.Drive(1,CW,1500);
+            wait(0.5);
+                    
+    while(1){
+        if(sensor1 == 0 && sensor2 == 0){
+            motor.Drive(0,CW,1500);
+            motor.Drive(1,CW,1500);
+            }
+        else if(sensor3 == 0 && sensor4 == 0){
+             lift.Drive(0,CW,4000);
+                    motor.Drive(0,STOP,0);
+                    motor.Drive(1,STOP,0);
+                    wait (2.0);
+            lift.Drive(0,STOP,0);
+                    break;
             } 
         else if(sensor1 == 0 && sensor2 == 1){
                 x = 1;
@@ -35,42 +86,12 @@
                     motor.Drive(0,CW,1500);
                     motor.Drive(1,CW,0);
                     }
-                    else  if(sensor3 == 0 && sensor4 == 0){
-                    lift.Drive(0,CW,1500);
-                    motor.Drive(0,CW,0);
-                    motor.Drive(1,CW,0);
-                    break;
                     }
-                    }
-    while(1){
-        if(sensor1 == 0 && sensor2 == 0){
-            motor.Drive(0,CW,1500);
-            motor.Drive(1,CW,1500);
-            } 
-        else if(sensor1 == 0 && sensor2 == 1){
-                x = 1;
-                motor.Drive(0,CW,0);
-                motor.Drive(1,CW,1500);
-                } 
-        else if(sensor1 == 1 && sensor2 == 0){
-                    x = 2;
-                    motor.Drive(0,CW,1500);
-                    motor.Drive(1,CW,0);
-                    }
-         else  if(x == 1 && sensor1 == 1 && sensor2 == 1){
-         
-                    motor.Drive(0,CW,0);
-                    motor.Drive(1,CW,1500);
-                    }
-         else  if(x == 2 && sensor1 == 1 && sensor2 == 1){
-                    motor.Drive(0,CW,1500);
-                    motor.Drive(1,CW,0);
-                    }
-                    else  if(sensor3 == 0 && sensor4 == 0){
-                    lift.Drive(0,CCW,1500);
-                    motor.Drive(0,CW,0);
-                    motor.Drive(1,CW,0);
-                    }
-                    }
+                        motor.Drive(0,CCW,1500);
+                    motor.Drive(1,CCW,1500);
+                    wait (1.0);
+                    motor.Drive(0,STOP,0);
+                    motor.Drive(1,STOP,0);
+                    
                     }
                     
\ No newline at end of file