Diff: main.cpp

Revision:

5:9b5edadc023b

Parent:

4:84bd5ead83f9

Child:

6:98121d2d76a6

--- a/main.cpp	Fri Oct 07 18:29:28 2016 +0000
+++ b/main.cpp	Sat Oct 08 15:34:37 2016 +0000
@@ -2,79 +2,43 @@
 #include "MODSERIAL.h"
 #include "Servo.h"
 
-DigitalOut Direcion_M1(D4);
-DigitalOut Speed_M1(D5);
-DigitalOut Speed_M2(D6);                  
-DigitalOut Direction_M2(D7);
-
-Servo Gripper_servo(A3);
+DigitalOut Direction_M1(D4);    //To control the rotation direction of the arm
+DigitalOut Speed_M1(D5);        //To control the rotation speed of the arm
+DigitalOut Speed_M2(D6);        //To control the translation direction of the arm
+DigitalOut Direction_M2(D7);    //To control the translation speed of the arm
 
-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);  
+Servo gripper_servo(D3);        //To control the gripper (Note: doesn't work on D8, unknown why)
 
-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
-            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;
-    }     
-}                 
+InterruptIn Switch_1(D9);       //To control the rotation to the left
+InterruptIn Switch_2(D10);      //To control the rotation to the right
+InterruptIn Switch_3(D11);      //To control the translation of the arm
+InterruptIn Switch_4(D12);      //To control the gripper
 
-void switch_counter_translation (){
-    counter_translation++;
-    if (counter_translation > 4){
-        counter_translation=1;
-    }
-    translation();
-}
+int counter_rotation_left=0;    //To count the number of times the rotation_left switch (switch_1) has been pushed
+int counter_rotation_right=0;   //To count the number of times the rotation_right switch (switch_2) has been pushed
+int counter_translation=0;      //To count the number of times the translation switch (switch_3) has been pushed
+int counter_gripper=0;          //To count the number of times the gripper switch (switch_4) has been pushed
+
+MODSERIAL pc(USBTX, USBRX);     //To make connection with the PC
 
 void rotation_left (){
     switch (counter_rotation_left){
-        case 1:
-            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");
+        case 1:                              //For activating the rotation to the left
+            Direction_M1 = 1;                //The arm will rotate to the left  
+            Speed_M1 = 1;                    //The motor is turned on
+            pc.printf("The arm will now rotate to the left \n");
             wait(0.5f);
             break;
-        case 2:
-            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");
+        case 2:                              //For stopping the rotation to the left
+            Direction_M1 = 1;                //The arm will rotate to the left  
+            Speed_M1 = 0;                    //The motor is turned off
+            pc.printf("The arm will now stop rotating to the left \n");
             wait(0.5f);
             break;
     }
 }                 
 
-void switch_counter_rotation_left (){
+void switch_counter_rotation_left (){   //To count the number of times the rotation_left switch (switch_1) has been pushed
     counter_rotation_left++;
     if (counter_rotation_left > 2){
         counter_rotation_left=1;
@@ -84,22 +48,22 @@
 
 void rotation_right (){
     switch (counter_rotation_right){
-        case 1:
-            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");
+        case 1:                              //For activation the rotation to the right
+            Direction_M1 = 0;                //The arm will rotate to the right 
+            Speed_M1 = 1;                    //The motor is turned on
+            pc.printf("The arm will now rotate to the right \n");
             wait(0.5f);
             break;
-        case 2:
-            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");
+        case 2:                              //For stopping the rotation to the right
+            Direction_M1 = 0;                //The arm will rotate to the right
+            Speed_M1 = 0;                    //The motor is turned off
+            pc.printf("The arm will now stop rotating to the right \n");
             wait(0.5f);
             break;
     }
 }
 
-void switch_counter_rotation_right (){
+void switch_counter_rotation_right (){      //To count the number of times the rotation_right switch (switch_2) has been pushed
     counter_rotation_right++;
     if (counter_rotation_right> 2){
         counter_rotation_right=1;
@@ -107,22 +71,59 @@
     rotation_right();
 }
 
+void translation (){
+    switch (counter_translation){
+        case 1:                              //For activating the elongation of the arm
+            Direction_M2 = 1;                //The arm will get longer  
+            Speed_M2 = 1;                    //The motor is turned on
+            pc.printf("The arm will now get longer \n");
+            wait(0.5f);
+            break;
+        case 2:                              //For stopping the elongation of the arm
+            Direction_M2 = 1;                //The arm will get longer  
+            Speed_M2 = 0;                    //The motor is turned off
+            pc.printf("The arm will now stop getting longer \n");
+            wait(0.5f);
+            break;
+        case 3:                              //For activating the shortening of the arm
+            Direction_M2 = 0;                //The arm will get shorter  
+            Speed_M2 = 1;                    //The motor is turned off
+            pc.printf("The arm will now get shorter \n");
+            wait(0.5f);
+            break;
+        case 4:                              //For stopping the shortening of the arm
+            Direction_M2 = 0;                //The arm will get shorter 
+            Speed_M2 = 0;                    //The motor is turned off
+            pc.printf("The arm will now stop getting shorter \n");
+            wait(0.5f);
+            break;
+    }     
+}                 
+
+void switch_counter_translation (){     //To count the number of times the translation switch (switch_3) has been pushed
+    counter_translation++;
+    if (counter_translation > 4){
+        counter_translation=1;
+    }
+    translation();
+}
+
 void gripper (){
     switch (counter_gripper){
         case 1:
-            Gripper_servo(0);     //The gripper is now closed
-            pc.printf("The gripper will now close");
+            gripper_servo = 0;     //The gripper is now closed
+            pc.printf("The gripper will now close \n");
             wait(0.5f);
             break;
         case 2:
-            Gripper_servo(0.5);     //The gripper is now closed
-            pc.printf("The gripper will now open");
+            gripper_servo = 1;     //The gripper is now open
+            pc.printf("The gripper will now open \n");
             wait(0.5f);
             break;
     }     
 }
 
-void switch_counter_gripper (){
+void switch_counter_gripper (){     //To count the number of times the gripper switch (switch_4) has been pushed
     counter_gripper++;
     if (counter_gripper> 2){
         counter_gripper=1;
@@ -134,14 +135,16 @@
     pc.baud(115200);
     pc.printf("RESET \n");
     
-    digitalWrite(Direction_M1, 1);   //The arm will initially get longer  
-    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
+    Direction_M1 = 1;               //The arm will initially get longer  
+    Speed_M1 = 0;                   //The first motor is initially turned off
+    Direction_M2 = 255;             //The arm will initially turn left  
+    Speed_M1 = 0;                   //The second motor is initially turned off
+    gripper_servo = 1;              //The gripper is initially open
     
-    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);
+    Switch_1.rise(&switch_counter_rotation_left);
+    Switch_2.rise(&switch_counter_rotation_right);
+    Switch_3.rise(&switch_counter_translation); 
+    Switch_4.rise(&switch_counter_gripper);
+
+    while (true);
 }
\ No newline at end of file