Diff: main.cpp

Revision:

10:cf579c3eaf01

Parent:

9:cca4d4084775

Child:

11:b1ad5267a6bd

--- a/main.cpp	Tue Oct 11 11:00:18 2016 +0000
+++ b/main.cpp	Fri Oct 14 18:00:27 2016 +0000
@@ -3,23 +3,23 @@
 #include "Servo.h"
 #include "QEI.h"
 
-QEI encoder_M1 (D9, D10, NC, 8400);
-QEI encoder_M2 (D11, D12, NC, 8400);
+QEI encoder_M1 (D9, D10, NC, 8400);     //Define an encoder for motor 1 called encoder_M1
+QEI encoder_M2 (D11, D12, NC, 8400);    //Define an encoder for motor 2 called encoder_M2
 
-Ticker encoder_M1_ticker;
-Ticker encoder_M2_ticker;
+Ticker encoder_M1_ticker;       //Create a ticker for reading the encoder data of encoder_M1
+Ticker encoder_M2_ticker;       //Create a ticker for reading the encoder data of encoder_M2
 
 DigitalOut Direction_M1(D4);    //To control the rotation direction of the arm
 PwmOut Speed_M1(D5);            //To control the rotation speed of the arm
 PwmOut Speed_M2(D6);            //To control the translation direction of the arm
 DigitalOut Direction_M2(D7);    //To control the translation speed of the arm
 
-Servo gripper_servo(D13);        //To control the gripper (Note: D8=PTC12)
+Servo gripper_servo(D13);       //To control the gripper
 
-InterruptIn Switch_1(SW2);     //To control the rotation to the left
-InterruptIn Switch_2(SW3);     //To control the rotation to the right
-InterruptIn Switch_3(D2);      //To control the translation of the arm
-InterruptIn Switch_4(D3);      //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_3(D2);       //Switch 3 to control the translation of the arm
+InterruptIn Switch_4(D3);       //Switch 4 to control the gripper
 
 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
@@ -28,54 +28,54 @@
 
 MODSERIAL pc(USBTX, USBRX);     //To make connection with the PC
 
-const double pi = 3.1415926535897;
+const double pi = 3.1415926535897;  //Declare the value of pi
 
-double speed_rotation=pi/5;       //in rad/sec
-double speed_translation=pi/5;    //in rad/sec
-double speedM1=speed_rotation/8.4;
-double speedM2=speed_translation/8.4;
+double speed_rotation=pi/5;             //Set the rotation speed in rad/sec -> NOTE: this has to be below 8.4 rad/sec
+double speed_translation=pi/5;          //Set the translation speed in rad/sec -> NOTE: this has to be below 8.4 rad/sec
+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
 
-void read_position_M1(){
-    int pulses_M1 = encoder_M1.getPulses();
-    float angle_M1 = float(pulses_M1)/4200*2.0*pi;
-    pc.printf("%i \t%f \t", pulses_M1, angle_M1);
+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
+    float 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(){
-    int pulses_M2 = encoder_M2.getPulses();
-    float angle_M2 = float(pulses_M2)/4200*2.0*pi;
-    pc.printf("%i \t%f \n", pulses_M2, angle_M2);
+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
+    float 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 rotation_left (){
-    switch (counter_rotation_left){
-        case 1:                              //For activating the rotation 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
+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 = 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.5f);
             break;
-        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
+        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 (){   //To count the number of times the rotation_left switch (switch_1) has been pushed
-    counter_rotation_left++;
-    if (counter_rotation_left > 2){
+void switch_counter_rotation_left (){       //To count the number of times the rotation_left switch (switch_1) has been pushed
+    counter_rotation_left++;                //Increase the counter_rotation_left
+    if (counter_rotation_left > 2){         //Because there are only 2 cases in the switch statement, case 3 = case 1 etc.
         counter_rotation_left=1;
     }
-    rotation_left();
+    rotation_left();                        //After increasing the counter, execute the rotation_left function
 }
 
-void rotation_right (){
-    switch (counter_rotation_right){
-        case 1:                              //For activation the rotation to the right
-            Direction_M1 = 0;                //The arm will rotate to the right 
+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 = 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.5f);
@@ -90,18 +90,18 @@
 }
 
 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++;               //Increase the counter_rotation_right
+    if (counter_rotation_right> 2){         //Because there are only 2 cases in the switch statement, case 3 = case 1
         counter_rotation_right=1;
     }
-    rotation_right();
+    rotation_right();                       //After increasing the counter, execute the rotation_right function
 }
 
-void translation (){
-    switch (counter_translation){
+void translation (){                            //Function to control the translation
+    switch (counter_translation){               //Create a switch statement
         case 1:                                 //For activating the elongation of the arm
             Direction_M2 = 1;                   //The arm will get longer  
-            Speed_M2 = speedM2;   //The motor is turned on at speed_rotation rad/sec
+            Speed_M2 = speedM2;                 //The motor is turned on at speed_translation rad/sec
             pc.printf("The arm will now get longer \n");
             wait(0.5f);
             break;
@@ -113,7 +113,7 @@
             break;
         case 3:                                 //For activating the shortening of the arm
             Direction_M2 = 0;                   //The arm will get shorter  
-            Speed_M2 = speedM2;   //The motor is turned on at speed_rotation rad/sec
+            Speed_M2 = speedM2;                 //The motor is turned on at speed_translation rad/sec
             pc.printf("The arm will now get shorter \n");
             wait(0.5f);
             break;
@@ -127,22 +127,22 @@
 }                 
 
 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++;              //Increase the counter_translation
+    if (counter_translation > 4){       //Because there are 4 cases in the switch statement, case 5 = case 1
         counter_translation=1;
     }
-    translation();
+    translation();                      //After increasing the counter, execute the translation function
 }
 
-void gripper (){
-    switch (counter_gripper){
-        case 1:
-            gripper_servo = 0;     //The gripper is now closed
+void gripper (){                        //Function to control the gripper
+    switch (counter_gripper){           //Create a switch statement
+        case 1:                         //For closing the gripper
+            gripper_servo = 0;          //The gripper is now closed
             pc.printf("The gripper will now close \n");
             wait(0.5f);
             break;
-        case 2:
-            gripper_servo = 1;     //The gripper is now open
+        case 2:                         //For opening the gripper
+            gripper_servo = 1;          //The gripper is now open
             pc.printf("The gripper will now open \n");
             wait(0.5f);
             break;
@@ -150,31 +150,32 @@
 }
 
 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++;              //Increase the couter_gripper
+    if (counter_gripper> 2){        //Because there are only 2 cases in the switch statement, case 3 = case 1
         counter_gripper=1;
     }
-    gripper();
+    gripper();                      //After increasing the counter, execute the gripper function
 }
 
 int main(){
-    pc.baud(115200);
-    pc.printf("RESET \n");
+    pc.baud(115200);                //Set the boud rate for serial communication
+    pc.printf("RESET \n");          //Print "RESET"
     
     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_M2 = 0;                   //The second motor is initially turned off
     gripper_servo = 1;              //The gripper is initially open
-    encoder_M1.reset();
-    encoder_M2.reset();
-    encoder_M1_ticker.attach(&read_position_M1,0.5);
-    encoder_M2_ticker.attach(&read_position_M2,0.5);
+    encoder_M1.reset();             //Reset the encoder for motor 1
+    encoder_M2.reset();             //Reset the encoder for motor 2
     
-    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);
+    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(&switch_counter_rotation_left);       //Connect switch_1 to the counter_rotation_left
+    Switch_2.rise(&switch_counter_rotation_right);      //Connect switch_2 to the counter_rotation_right
+    Switch_3.rise(&switch_counter_translation);         //Connect switch_3 to the counter_translation
+    Switch_4.rise(&switch_counter_gripper);             //Connect switch_4 to the counter_gripper
 
     while (true);
 }
\ No newline at end of file