Script for controlling 2 DC-motors and a gripper-servo using buttons

Dependencies:   MODSERIAL QEI Servo mbed

Revision:
2:b20570f160c6
Parent:
1:0d55a4bf2269
Child:
3:0a4bfcb3f339
--- a/main.cpp	Fri Oct 07 17:30:26 2016 +0000
+++ b/main.cpp	Fri Oct 07 17:40:28 2016 +0000
@@ -6,10 +6,47 @@
 //DigitalOut Speed_M2(D6);                  
 //DigitalOut Direction_M2(D7);
 
-DigitalIn Switch_1(D8);
-int counter=1;
+InterruptIn Switch_1(D8);
+int counter_extension=1;
+
+MODSERIAL pc(USBTX, USBRX);  
 
-MODSERIAL pc(USBTX, USBRX);                   
+void extension (){
+    switch (counter_extension){
+        case 1:
+            digitalWrite(Direction_M1, 1);   //The arm will get longer  
+            analogWrite(Speed_M1, 255);      //The motor is turned on
+            pc.printf("The arm will now get longer");
+            wait(0.5f);
+            break;
+        case 2:
+            digitalWrite(Direction_M1, 1);   //The arm will get longer  
+            analogWrite(Speed_M1, 0);        //The motor is turned off
+            pc.printf("The arm will now stop");
+            wait(0.5f);
+            break;
+        case 3:
+            digitalWrite(Direction_M1, 0);   //The arm will get shorter  
+            analogWrite(Speed_M1, 255);      //The motor is turned off
+            pc.printf("The arm will now get shorter");
+            wait(0.5f);
+            break;
+        case 4:
+            digitalWrite(Direction_M1, 0);   //The arm will get shorter 
+            analogWrite(Speed_M1, 0);        //The motor is turned off
+            pc.printf("The arm will now stop");
+            wait(0.5f);
+            break;
+    }     
+}                 
+
+void switch_counter_extension (){
+    counter_extension++;
+    if (counter_extension > 4){
+        counter_extension=1;
+    }
+    extension();
+}
 
 int main(){
     pc.baud(115200);
@@ -18,41 +55,5 @@
     digitalWrite(Direction_M1, 1);   //The arm will initially get longer  
     analogWrite(Speed_M1, 0);        //The motor is initially turned off
     
-    while (true){
-        if (switch_1 == 0){
-            switch (counter){
-                case 1:
-                    counter++;
-                    digitalWrite(Direction_M1, 1);   //The arm will get longer  
-                    analogWrite(Speed_M1, 255);      //The motor is turned on
-                    pc.printf("The arm will now get longer");
-                    wait(0.5f);
-                    break;
-                case 2:
-                    counter++;
-                    digitalWrite(Direction_M1, 1);   //The arm will get longer  
-                    analogWrite(Speed_M1, 0);        //The motor is turned off
-                    pc.printf("The arm will now stop");
-                    wait(0.5f);
-                    break;
-                case 3:
-                    counter++;
-                    digitalWrite(Direction_M1, 0);   //The arm will get shorter  
-                    analogWrite(Speed_M1, 255);      //The motor is turned off
-                    pc.printf("The arm will now get shorter");
-                    wait(0.5f);
-                    break;
-                case 4:
-                    counter++;
-                    digitalWrite(Direction_M1, 0);   //The arm will get shorter 
-                    analogWrite(Speed_M1, 0);        //The motor is turned off
-                    pc.printf("The arm will now stop");
-                    wait(0.5f);
-                    break;
-                case 5:
-                    counter = 1;
-                    break;
-            }     
-        } 
-    }
+    switch_1.rise(&switch_counter_extension);  
 }
\ No newline at end of file