Diff: main.cpp

Revision:

8:9c58ca13076e

Parent:

7:9a3809a9ab3e

Child:

9:cca4d4084775

--- a/main.cpp	Mon Oct 10 11:09:35 2016 +0000
+++ b/main.cpp	Mon Oct 10 12:09:49 2016 +0000
@@ -2,10 +2,10 @@
 #include "MODSERIAL.h"
 #include "Servo.h"
 
-//DigitalIn Encoder_M1A(D9); 
-//DigitalIn Encoder_M1B(D10);
-//DigitalIn Encoder_M2A(D11); 
-//DigitalIn Encoder_M2B(D12);
+DigitalIn Encoder_M1A(D9); 
+DigitalIn Encoder_M1B(D10);
+DigitalIn Encoder_M2A(D11); 
+DigitalIn Encoder_M2B(D12);
 
 DigitalOut Direction_M1(D4);    //To control the rotation direction of the arm
 PwmOut Speed_M1(D5);            //To control the rotation speed of the arm
@@ -26,15 +26,18 @@
 
 MODSERIAL pc(USBTX, USBRX);     //To make connection with the PC
 
-float speed_translation=0.1; //0.075
-float speed_rotation=0.1; //0.075
+const double pi = 3.1415926535897;
+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;
 
 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 = speed_rotation;                    //The motor is turned on
-            pc.printf("The arm will now rotate to the left \n");
+            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
@@ -58,8 +61,8 @@
     switch (counter_rotation_right){
         case 1:                              //For activation the rotation to the right
             Direction_M1 = 0;                //The arm will rotate to the right 
-            Speed_M1 = speed_rotation;                    //The motor is turned on
-            pc.printf("The arm will now rotate to the right \n");
+            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);
             break;
         case 2:                              //For stopping the rotation to the right
@@ -81,27 +84,27 @@
 
 void translation (){
     switch (counter_translation){
-        case 1:                              //For activating the elongation of the arm
-            Direction_M2 = 1;                //The arm will get longer  
-            Speed_M2 = speed_translation;                    //The motor is turned on
+        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
             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
+        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 = speed_translation;                    //The motor is turned off
+        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
             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
+        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;