Diff: main.cpp

Revision:

13:ea065d364277

Parent:

12:35a81d6c6505

--- a/main.cpp	Wed Oct 19 11:54:55 2016 +0000
+++ b/main.cpp	Mon Oct 24 11:42:49 2016 +0000
@@ -15,11 +15,13 @@
 DigitalOut Direction_M1(D7);    //To control the translation speed of the arm
 Servo gripper_servo(D13);       //To control the gripper
 
-InterruptIn Switch_1(SW2);      //Switch 1 to control the rotation to the left
-InterruptIn Switch_2(SW3);      //Switch 2 to control the rotation to the right
+InterruptIn Switch_1(SW3);      //Switch 1 to control the rotation to the left
+InterruptIn Switch_2(SW2);      //Switch 2 to control the rotation to the right
 InterruptIn Switch_3(D2);       //Switch 3 to control the translation of the arm
 InterruptIn Switch_4(D3);       //Switch 4 to control the gripper
 
+Ticker      check_goflags_ticker;               //Create a ticker for checking if the go-flags are set true
+
 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
@@ -39,19 +41,19 @@
 double speedM1=speed_rotation/8.4;      //Map the rotation speed from (0-8.4) to (0-1) by dividing by 8.4
 double speedM2=speed_translation/8.4;   //Map the translation speed from (0-8.4) to (0-1) by dividing by 8.4
 
-float angle_M1=0;
-float angle_M2=0;
+float angle_M1=0;       //The measured angle of motor 1 is initially zero
+float angle_M2=0;       //The measured angle of motor 2 is initially zero
 
 void read_position_M1(){                            //Function to read the position of motor 1
-    int pulses_M1 = -encoder_M1.getPulses();         //Read the encoder data and store it in pulses_M1
-    angle_M1 = float(pulses_M1)/4200*2.0*pi;  //Calculate the angle that corresponds with the measured encoder pulses
-    pc.printf("%i \t%f \t", pulses_M1, angle_M1);
+    int pulses_M1 = -encoder_M1.getPulses();        //Read the encoder data and store it in pulses_M1
+    angle_M1 = float(pulses_M1)/4200*2.0*pi;        //Calculate the angle that corresponds with the measured encoder pulses
+//    pc.printf("%i \t%f \t", pulses_M1, angle_M1);
 }
 
 void read_position_M2(){                            //Function to read the position of motor 2
-    int pulses_M2 = -encoder_M2.getPulses();         //Read the encoder data and store it in pulses_M2
-    angle_M2 = float(pulses_M2)/4200*2.0*pi;  //Calculate the angle that corresponds with the measured encoder pulses
-    pc.printf("%i \t%f \n", pulses_M2, angle_M2);
+    int pulses_M2 = -encoder_M2.getPulses();        //Read the encoder data and store it in pulses_M2
+    angle_M2 = float(pulses_M2)/4200*2.0*pi;        //Calculate the angle that corresponds with the measured encoder pulses
+//    pc.printf("%i \t%f \n", pulses_M2, angle_M2);
 }
 
 void activate_rotation_left (){             //To activate the rotation_left
@@ -65,13 +67,13 @@
 void rotation_left (){                      //Function to control the rotation to the left
     switch (counter_rotation_left){         //Create a switch statement
         case 1:                             //For activating the rotation to the left
-            Direction_M1 = 0;               //The arm will rotate to the left  
+            Direction_M1 = 1;               //The arm will rotate to the left  
             Speed_M1 = speedM1;             //The motor is turned on at speed_rotation rad/sec
             pc.printf("The arm will now rotate to the left with %f rad/sec \n", speedM1);
             wait(0.1f);
             break;
         case 2:                             //For stopping the rotation to the left
-            Direction_M1 = 0;               //The arm will rotate 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.1f);
@@ -90,13 +92,13 @@
 void rotation_right (){                     //Function to control the rotation to the left
     switch (counter_rotation_right){        //Create a switch statement
         case 1:                             //For activation the rotation to the right
-            Direction_M1 = 1;               //The arm will rotate to the right 
+            Direction_M1 = 0;               //The arm will rotate to the right 
             Speed_M1 = speedM1;             //The motor is turned on at speed_rotation rad/sec
             pc.printf("The arm will now rotate to the right with %f rad/sec \n", speedM1);
             wait(0.1f);
             break;
         case 2:                              //For stopping the rotation to the right
-            Direction_M1 = 1;                //The arm will rotate 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.1f);
@@ -157,13 +159,32 @@
             wait(0.1f);
             break;
         case 2:                         //For opening the gripper
-            gripper_servo = 1;          //The gripper is now open
+            gripper_servo = 0.3;        //The gripper is now open
             pc.printf("The gripper will now open \n");
             wait(0.1f);
             break;
     }     
 }
 
+void check_goflags (){          //Function to check if the go-flags are activated
+    if (rotation_left_go == true) {     //If the rotation_left go-flag is true
+        rotation_left_go = false;       //Set the rotation_left go-flag to false
+        rotation_left();                //Execute the rotation_left function
+    }
+    if (rotation_right_go == true) {    //If the rotation_right go-flag is true
+        rotation_right_go = false;      //Set the rotation_right go-flag to false
+        rotation_right();               //Execute the rotation_right function
+    }
+    if (translation_go == true) {       //If the translation go-flag is true
+        translation_go = false;         //Set the translation go-flag to false
+        translation();                  //Execute the translation function
+    }
+    if (gripper_go == true) {           //If the gripper go-flag is true
+        gripper_go = false;             //Set the gripper go-flag to false
+        gripper();                      //Execute the gripper function
+    }  
+}
+
 int main(){
     pc.baud(115200);                //Set the boud rate for serial communication
     pc.printf("RESET \n");          //Print "RESET"
@@ -172,34 +193,19 @@
     Speed_M1 = 0;                   //The first motor is initially turned off
     Direction_M2 = 255;             //The arm will initially turn left  
     Speed_M2 = 0;                   //The second motor is initially turned off
-    gripper_servo = 1;              //The gripper is initially open
+    gripper_servo = 0.3;            //The gripper is initially open
     encoder_M1.reset();             //Reset the encoder for motor 1
     encoder_M2.reset();             //Reset the encoder for motor 2
     
     encoder_M1_ticker.attach(&read_position_M1,0.01);   //Connect the encoder_M1_ticker to the read_position_M1 function and execute at 100Hz
     encoder_M2_ticker.attach(&read_position_M2,0.01);   //Connect the encoder_M2_ticker to the read_position_M2 function and execute at 100Hz
     
-    Switch_1.rise(&activate_rotation_left);             //Use switch_1 to activate the rotation_left go-flag
+    Switch_1.rise(&activate_rotation_left);             //Use switch_1 to activate the counter_rotation_left go-flag
     Switch_2.rise(&activate_rotation_right);            //Use switch_2 to activate the counter_rotation_right go-flag
     Switch_3.rise(&activate_translation);               //Use switch_3 to activate the counter_translation go-flag
     Switch_4.rise(&activate_gripper);                   //Use switch_4 to activate the counter_gripper go-flag
 
-    while (true){
-        if (rotation_left_go == true) {                         //If the rotation_left go-flag is true
-            rotation_left_go = false;                       //Set the rotation_left go-flag to false
-            rotation_left();                                //Execute the rotation_left function
-        }
-        if (rotation_right_go == true) {                        //If the rotation_right go-flag is true
-            rotation_right_go = false;                      //Set the rotation_right go-flag to false
-            rotation_right();                               //Execute the rotation_right function
-        }
-        if (translation_go == true) {                           //If the translation go-flag is true
-            translation_go = false;                         //Set the translation go-flag to false
-            translation();                                  //Execute the translation function
-        }
-        if (gripper_go == true) {                               //If the gripper go-flag is true
-            gripper_go = false;                                 //Set the gripper go-flag to false
-            gripper();                                          //Execute the gripper function
-        }
-    }
+    check_goflags_ticker.attach(&check_goflags, 0.01);         //Connect the check_goflags_ticker to the check_goflags
+    
+    while (true){}
 }
\ No newline at end of file