Diff: main.cpp

Revision:

13:a2e281d5de89

Parent:

12:3e084e1a699e

Child:

14:059fd8f6cbfd

--- a/main.cpp	Thu Nov 01 17:53:13 2018 +0000
+++ b/main.cpp	Thu Nov 01 19:39:06 2018 +0000
@@ -57,6 +57,8 @@
 float theta1_prev = 0.0;
 float theta2_prev = 0.0;
 const float pi = 3.14159265359;
+volatile double  error1;
+volatile double  error2;
 float tijd = 0.005;
 float time_in_state;
 int need_to_move_1;                     // Does the robot needs to move in the first direction?
@@ -84,9 +86,9 @@
 const double L3 = 350.0; //length of the second link
 
 // Reference angles of the starting position
-double q2_0 = -acos((pow(x0,sq)+pow(y0,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3));
+double q2_0 = pi + acos((pow(x0,sq)+pow(y0,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3));
 double q1_0 = atan(y0/x0)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x0,sq)+pow(y0,sq))/(2.0*L1*sqrt(pow(x0,sq)+pow(y0,sq))));
-double q2_0_enc = (pi-q2_0) - q1_0;
+double q2_0_enc = q2_0 - q1_0;
 
 // DEMO
 double  point1x = 200.0;
@@ -124,6 +126,20 @@
     theta2_prev = theta2;                        // Define theta_prev
 }
 
+double  counts2angle1()
+{
+    counts1 = Encoder1.getPulses();         // Counts of outputshaft of motor 1
+    theta1 = -(double(counts1)/4200) * 2*pi;       // Angle of outputshaft of motor 1
+    return theta1;
+}
+
+double counts2angle2()
+{
+    counts2 = Encoder2.getPulses();         // Counts of outputshaft of motor 2
+    theta2 = (double(counts2)/4200) * 2*pi;       // Angle of outputshaft of motor 2
+    return theta2;
+}
+
 // Motor calibration
 void MotorAngleCalibrate()                  // Function that drives motor 1 and 2.
 {
@@ -166,7 +182,7 @@
 // Inverse Kinematics
 double makeAngleq1(double x, double y){
     double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration
-    q1_diff = -(2.0*(q1-q1_0));                                                                                    //the actual amount of radians that the motor has to turn in total to reach the setpoint
+    q1_diff = -2.0*(q1-q1_0);                                                                                    //the actual amount of radians that the motor has to turn in total to reach the setpoint
     return q1_diff;
 }
 
@@ -288,7 +304,7 @@
         setpointx = point3x; 
         track = 23;
     }
-    
+
     if (setpointy > point3y && track == 23){
         setpointx = point3x;          // Van punt 1 naar punt 2 op dezelfde x blijven. 
     } 
@@ -362,16 +378,16 @@
     
 }
 void motoraansturingdemo()
-{
+{    
     setpointx = determinedemosetx(setpointx, setpointy);
     setpointy = determinedemosety(setpointx, setpointy);
     q1_diff = makeAngleq1(setpointx, setpointy);
     q2_diff = makeAngleq2(setpointx, setpointy);
 
-    ReadEncoder1();
-    ReadEncoder2();          
-    double error2 = q2_diff - theta2; 
-    double error1 = q1_diff - theta1;          // Setpoint error, te behalen setpoint minus de huidige positie van de as.
+    theta2 = counts2angle2();           
+    error2 = q2_diff - theta2;
+    theta1 = counts2angle1();  
+    error1 = q1_diff - theta1;          // Setpoint error, te behalen setpoint minus de huidige positie van de as.
 
     U1 = PI_controller1(error1);       // Voltage dat naar de motor gestuurd wordt. 
     U2 = PI_controller2(error2);   
@@ -380,7 +396,28 @@
     directionM1 = U1 > 0.0f;            // Richting van de motor bepalen
     motor2_pwm.write(fabs(U2));
     directionM2 = U2 > 0.0f; 
-} 
+}
+
+void motoraansturinghoming()
+{    
+    setpointx = x0;
+    setpointy = y0;
+    q1_diff = makeAngleq1(setpointx, setpointy);
+    q2_diff = makeAngleq2(setpointx, setpointy);
+
+    theta2 = counts2angle2();           
+    error2 = q2_diff - theta2;
+    theta1 = counts2angle1();  
+    error1 = q1_diff - theta1;          // Setpoint error, te behalen setpoint minus de huidige positie van de as.
+
+    U1 = PI_controller1(error1);       // Voltage dat naar de motor gestuurd wordt. 
+    U2 = PI_controller2(error2);   
+    
+    motor1_pwm.write(fabs(U1));         // Motor aansturen
+    directionM1 = U1 > 0.0f;            // Richting van de motor bepalen
+    motor2_pwm.write(fabs(U2));
+    directionM2 = U2 > 0.0f; 
+}  
 // ---------------------------------------------------
 // --------STATEMACHINE------------------------------- 
 // --------------------------------------------------- 
@@ -491,20 +528,20 @@
         // Robot moves to the home starting configuration
         pc.printf("HOMING \r\n");
         led_red = 0; led_green = 1; led_red = 0;                  // Colouring the led PURPLE
-
+        motor1_pwm.period_us(60);                // Period is 60 microseconde
+        motor2_pwm.period_us(60);
+        
         // Actions
         timer.start();                              //Start timer to get time in the state "MOTOR_ANGLE_CLRBRT" 
-        if (stateChanged)
-            {
-                MotorAngleCalibrate();                      // Actuate motor 1 and 2.
-                ReadEncoder1();                     // Get velocity of motor 1    
-                ReadEncoder2();                     // Get velocity of motor 2
-                stateChanged = true;                        // Keep this loop going, until the transition conditions are satisfied.    
-            }
+        if(stateChanged){
+            motoraansturinghoming();
+            stateChanged = true;
+         }   
+        
     
         // State transition logic
         time_in_state = timer.read();                       // Determine if this state has run for long enough.    
-        if(time_in_state > 2.0f && vel_1 < 1.1f && vel_2 < 1.1f)
+        if(time_in_state > 5.0f && vel_1 < 1.1f && vel_2 < 1.1f)
             {
                 pc.printf("Homing has ended. We are now in reference position. \n\r");
                 timer.stop();                              // Stop timer for this state
@@ -513,6 +550,7 @@
                 motor2_pwm.write(fabs(0.0));
                 Encoder1.reset();                          // Reset Encoders when arrived at zero-position
                 Encoder2.reset();
+                track = 1;
         
                 currentState = WAITING4SIGNAL;                   // Switch to next state (EMG_CLRBRT).
                 stateChanged = true;                       
@@ -522,6 +560,8 @@
                 currentState = FAILURE_MODE;
                 stateChanged = true;
             }
+             
+            
         break;
                
         case WAITING4SIGNAL:
@@ -540,14 +580,14 @@
             stateChanged = true;
             pc.printf("Starting Calibration \n\r");
         }
-        else if (button_Demo == 1) 
+        else if (button_Demo == 0) 
         {
             currentState = MOVE_W_DEMO;
             stateChanged = true;
             pc.printf("DEMO mode \r\n");
             wait(1.0f);
         }
-        else if (button_Emg == 1) 
+        else if (button_Emg == 0) 
         {
             currentState = MOVE_W_EMG;
             stateChanged = true;
@@ -567,7 +607,9 @@
         // Description:
         // In this state the robot follows a preprogrammed shape, e.g. 
         // a square.
-                    
+        motor1_pwm.period_us(60);                // Period is 60 microseconde
+        motor2_pwm.period_us(60);
+           
         led_red = 1; led_green = 1; led_blue = 0;   // Colouring the led GREEN
                 
         // Requirements to move to the next state:
@@ -710,6 +752,26 @@
     directiony.fall(ChangeDirectionY); //change the direction of the setpoint in y direction
         
     while (true) {
+        
+        if (currentState == MOVE_W_DEMO) {
+            pc.printf("Setpointx: %0.2f, Setpointy: %0.2f, q1_diff: %0.2f, q2_diff: %0.2f, error1: %0.2f, error2: %0.2f, U1: %0.2f, U2: %0.2f\r\n", setpointx,setpointy,q1_diff,q2_diff,error1,error2,U1,U2);
+            
+            if (track == 1) {
+                pc.printf("Gaat naar positie 1\r\n");
+            }
+            else if (track == 12) {
+                pc.printf("Gaat naar positie 2\r\n");
+            }
+        
+            else if (track == 23) {
+                pc.printf("Gaat naar positie 3\r\n");
+            }
+            else if (track == 34) {
+                pc.printf("Gaat naar positie 4\r\n");
+            }
+        }
+
+        wait(0.5f);
     }
 }