Diff: main.cpp

Revision:

4:84bd5ead83f9

Parent:

3:0a4bfcb3f339

Child:

5:9b5edadc023b

--- a/main.cpp	Fri Oct 07 17:48:34 2016 +0000
+++ b/main.cpp	Fri Oct 07 18:29:28 2016 +0000
@@ -1,22 +1,27 @@
 #include "mbed.h"
-#include "MODSERIAL.h'
+#include "MODSERIAL.h"
+#include "Servo.h"
 
 DigitalOut Direcion_M1(D4);
 DigitalOut Speed_M1(D5);
-//DigitalOut Speed_M2(D6);                  
-//DigitalOut Direction_M2(D7);
+DigitalOut Speed_M2(D6);                  
+DigitalOut Direction_M2(D7);
+
+Servo Gripper_servo(A3);
 
-InterruptIn Switch_1(D8);
-InterruptIn Switch_2(D9);
-InterruptIn Switch_3(D10);
-int counter_extension=1;
+InterruptIn Switch_1(D8);       //To control the translation of the arm
+InterruptIn Switch_2(D9);       //To control the rotation to the left
+InterruptIn Switch_3(D10);      //To control the rotation to the right
+InterruptIn Switch_4(D11);      //To control the gripper
+int counter_translation=1;
 int counter_rotation_left=1;
 int counter_rotation_right=1;
+int counter_gripper=1;
 
 MODSERIAL pc(USBTX, USBRX);  
 
-void extension (){
-    switch (counter_extension){
+void translation (){
+    switch (counter_translation){
         case 1:
             digitalWrite(Direction_M1, 1);   //The arm will get longer  
             analogWrite(Speed_M1, 255);      //The motor is turned on
@@ -44,25 +49,25 @@
     }     
 }                 
 
-void switch_counter_extension (){
-    counter_extension++;
-    if (counter_extension > 4){
-        counter_extension=1;
+void switch_counter_translation (){
+    counter_translation++;
+    if (counter_translation > 4){
+        counter_translation=1;
     }
-    extension();
+    translation();
 }
 
 void rotation_left (){
     switch (counter_rotation_left){
         case 1:
-            digitalWrite(Direction_M1, 1);   //The arm will rotate to the left  
-            analogWrite(Speed_M1, 255);      //The motor is turned on
+            digitalWrite(Direction_M2, 1);   //The arm will rotate to the left  
+            analogWrite(Speed_M2, 255);      //The motor is turned on
             pc.printf("The arm will now rotate to the left");
             wait(0.5f);
             break;
         case 2:
-            digitalWrite(Direction_M1, 1);   //The arm will rotate to the left  
-            analogWrite(Speed_M1, 0);        //The motor is turned off
+            digitalWrite(Direction_M2, 1);   //The arm will rotate to the left  
+            analogWrite(Speed_M2, 0);        //The motor is turned off
             pc.printf("The arm will now stop");
             wait(0.5f);
             break;
@@ -80,14 +85,14 @@
 void rotation_right (){
     switch (counter_rotation_right){
         case 1:
-            digitalWrite(Direction_M1, 0);   //The arm will rotate to the right 
-            analogWrite(Speed_M1, 255);      //The motor is turned on
+            digitalWrite(Direction_M2, 0);   //The arm will rotate to the right 
+            analogWrite(Speed_M2, 255);      //The motor is turned on
             pc.printf("The arm will now rotate to the right");
             wait(0.5f);
             break;
         case 2:
-            digitalWrite(Direction_M1, 0);   //The arm will rotate to the right
-            analogWrite(Speed_M1, 0);        //The motor is turned off
+            digitalWrite(Direction_M2, 0);   //The arm will rotate to the right
+            analogWrite(Speed_M2, 0);        //The motor is turned off
             pc.printf("The arm will now stop");
             wait(0.5f);
             break;
@@ -102,6 +107,29 @@
     rotation_right();
 }
 
+void gripper (){
+    switch (counter_gripper){
+        case 1:
+            Gripper_servo(0);     //The gripper is now closed
+            pc.printf("The gripper will now close");
+            wait(0.5f);
+            break;
+        case 2:
+            Gripper_servo(0.5);     //The gripper is now closed
+            pc.printf("The gripper will now open");
+            wait(0.5f);
+            break;
+    }     
+}
+
+void switch_counter_gripper (){
+    counter_gripper++;
+    if (counter_gripper> 2){
+        counter_gripper=1;
+    }
+    gripper();
+}
+
 int main(){
     pc.baud(115200);
     pc.printf("RESET \n");
@@ -110,8 +138,10 @@
     analogWrite(Speed_M1, 0);        //The motor is initially turned off
     digitalWrite(Direction_M1, 1);   //The arm will initially get longer  
     analogWrite(Speed_M1, 0);        //The motor is initially turned off
+    Gripper_servo(0.5);              //The gripper is initially open
     
-    switch_1.rise(&switch_counter_extension);  
+    switch_1.rise(&switch_counter_translation);  
     switch_2.rise(&switch_counter_rotation_left);
     switch_3.rise(&switch_counter_rotation_right);
+    switch_4.rise(&switch_counter_gripper);
 }
\ No newline at end of file