Diff: main.cpp

Revision:

5:cf58a6bfdd7a

Parent:

4:0ed46a6cdf66

Child:

6:18052a74ae56

--- a/main.cpp	Thu Oct 16 19:16:54 2014 +0000
+++ b/main.cpp	Sun Oct 19 23:46:08 2014 +0000
@@ -5,86 +5,62 @@
 #include "mbed.h"
 #include "Motor.h"
 #include "Servo.h"
- 
+
+Motor kate(p26, p29, p30);
 Servo arm(p21);
+Servo scoop(p22);
 DigitalIn sw1(p16); //up
 DigitalIn sw2(p17); //down
-Servo scoop(p22);
 DigitalIn sw3(p18); //dump
-Motor kate(p26, p29, p30);//movement of structure
 DigitalIn sw4(p19);
 DigitalIn sw5(p20);
 
 
-int main(){
+int main()
+{
 
     float armpos;
     float servopos;
     float mspeed;
-    
-    while(1){
-       
-       if((sw1 == 1) && (sw2 == 0)){
-           armpos += .05;
-           arm = armpos;
-           wait(.1);
-        }
-        else if((sw1 == 0) && (sw2 == 0)){
-            arm = armpos;
-            wait(.1);
-        }
-        if((sw2 == 1) && (sw1 == 0)){
-            armpos -= .01;
-            arm = armpos;
-            wait(.05);
-        }
-        else if((sw2 == 0) && (sw1 == 0)){
-            arm = armpos;
-            wait(.1);
-        }
-         if (sw3 == 1) {//dump command
-            for (servopos = 0; servopos <= 1.0; servopos += 1.0) {
-                scoop = servopos;
-            }
-            wait (1);
-        } else if (sw3 == 0) {
-            for (servopos = 1.0; servopos >= 0; servopos -=1.0) {
-                scoop = servopos;
-            }
-            wait (1);
-        }
-        if( sw4 == 1 ) {
+
+    while(1) {
+        if( sw4 == 1 && sw5 == 0 && sw1 == 0 && sw2 == 0 ) {
             mspeed = 0.35;
             kate.speed(mspeed);
             wait(.03);
             mspeed = 0.0;
             kate.speed(mspeed);
             wait(.05);
-        } 
-        else if (sw4 == 0) {
-            mspeed = 0.0;
-            kate.speed(mspeed);
-        }
-        else if(( sw4 == 1 && sw5 == 1)){ 
-            mspeed = 0.0;
-            kate.speed(mspeed);
-        }
-        if( sw5 == 1 ) {
+        } else if( sw5 == 1 && sw4 == 0 && sw1 == 0 && sw2 == 0) {
             mspeed = -0.35;
             kate.speed(mspeed);
             wait(.03);
             mspeed = 0.0;
             kate.speed(mspeed);
             wait(.05);
-            
-        } else if (sw5 == 0) {
+        } else if (sw3 == 1 && sw2 == 0 && sw1 == 0 && sw4 == 0 && sw5 == 0 ) {
+            for (servopos = 0; servopos <= 1.0; servopos += 1.0) {
+                scoop = servopos;
+            }
+            wait (1);
+        } else if(sw1 == 1 && sw2 == 0 && sw5 == 0 && sw4 == 0 ) {
+            armpos += .01;
+            arm = armpos;
+            wait(.05);
+        } else if(sw2 == 1 && sw1 == 0 && sw4 == 0 && sw5 == 0) {
+            armpos -= .01;
+            arm = armpos;
+            wait(.05);
+        } else if (sw4 == 0 && sw1 == 0 && sw2 == 0 && sw3 == 0 && sw5 == 0) {
             mspeed = 0.0;
             kate.speed(mspeed);
+            arm = armpos;
+            wait(.1);
+            for (servopos = 1.0; servopos >= 0; servopos -=1.0) {
+                scoop = servopos;
+            }
         }
-        else if(( sw4 == 1 && sw5 == 1)){ 
-            mspeed = 0.0;
-            kate.speed(mspeed);
-        }
-    }//end while
+    }
+
 }//end main