Diff: main_demo.cpp

Revision:

6:5431ddb9d881

Parent:

5:cd329205f037

Child:

7:2e0cdf836495

--- a/main_demo.cpp	Thu Nov 01 11:03:23 2018 +0000
+++ b/main_demo.cpp	Thu Nov 01 15:10:25 2018 +0000
@@ -16,6 +16,9 @@
 DigitalOut ledr(LED_RED);
 DigitalOut ledg(LED_GREEN);
 DigitalOut ledb(LED_BLUE);
+AnalogIn  potmeter1(PTC10);
+AnalogIn potmeter2(PTC11);
+
 
 // Tickers
 Ticker Demo;
@@ -35,9 +38,9 @@
 double  point2x = 350.0;
 double  point2y = 200.0;
 double  point3x = 350.0;
-double  point3y = 100.0;
+double  point3y = 0;
 double  point4x = 200.0;
-double  point4y = 100.0;
+double  point4y = 0;
 volatile int track = 1;
 const double x0 = 80.0; //zero x position after homing
 const double y0 = 141.0; //zero y position after homing
@@ -56,7 +59,18 @@
 // Reference angles of the starting position
 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 = q2_0 + q1_0;
+double q2_0_enc = q2_0 - q1_0;
+
+// Controller
+double potwaarde1;
+double pot1;
+double potwaarde2;
+double pot2;
+volatile double Kp1 = 3.95;              // Kp (proportionele controller, nu nog een random waarde)
+volatile double Kp2 = 3.95;              // Kp (proportionele controller, nu nog een random waarde)
+volatile double Kd1 = 1;                 // Kd (demping van de controller)
+volatile double Kd2 = 1;                 // Kd (demping van de controller)
+
 
 // --------------------------------------------------------------------
 // ---------------Read out encoders------------------------------------
@@ -86,7 +100,7 @@
     }
     
     // Van punt 1 naar punt 2. 
-    if (setpointy >= point1y - 0.3 && setpointx >= point1x - 0.3 && setpointy <= point1y + 0.3 && setpointx <= point1x + 0.3 && track == 1){
+    if (fabs(setpointx - point1x) <= 0.3 && fabs(setpointy - point1y) <= 0.3 && (track == 1 || track == 41)) {
         setpointx = setpointx + 0.1;
         track = 12;
     }
@@ -94,34 +108,43 @@
         setpointx = setpointx + 0.2;
     }
     
-    // Van punt 2 naar punt 1. 
+    // Van punt 2 naar punt 3. 
     if (fabs(setpointx - point2x) <= 0.3 && fabs(setpointy - point2y) <= 0.3 && track == 12)
     {
         setpointx = point3x; 
         track = 23;
     }
+    
     if (setpointy > point3y && track == 23)
     {
-        setpointx = point3x;          // Van punt 1 naar punt 2 op dezelfde y blijven. 
+        setpointx = point3x;          // Van punt 1 naar punt 2 op dezelfde x blijven. 
     } 
  
 
     // Van punt 3 naar punt 4. 
-    if (setpointy >= point3y - 0.3 && setpointx >= point3x - 0.3 && setpointy <= point3y + 0.3 && setpointx <= point3x + 0.3)
+    if ((fabs(setpointx - point3x) <= 0.3) && (fabs(setpointy - point3y) <= 0.3) && (track == 23))
     {
         setpointx = setpointx - 0.1;          // Van punt 1 naar punt 2 op dezelfde y blijven. 
         track = 34;
     }
     
-    if (setpointy > point3y && track == 34)
+    if (setpointx > point4x && track == 34)
     {
         setpointx = setpointx - 0.1;
     }
-        
-    if (setpointy >= point4y - 0.3 && setpointx >= point4x - 0.3 && setpointy <= point4y + 0.3 && setpointx <= point4x + 0.3 && track == 34)
+    
+    // Van punt 4 naar punt 1.        
+    if ((fabs(setpointx - point4x) <= 0.3) && (fabs(setpointy - point4y) <= 0.3) && (track == 34))
     {
-        track = 1;
+        setpointx = point4x;
+        track = 41;
     }
+    
+    if (setpointy < point1y && track == 41)
+    {
+        setpointx = point4x;        // Van punt 4 naar punt 2 op dezelfde x blijven.
+    }
+    
 
       return setpointx;
 }  
@@ -134,9 +157,8 @@
     } 
 
     // Van punt 1 naar punt 2. 
-    if (fabs(setpointx - point1x) <= 0.3 && fabs(setpointy - point1y) <= 0.3 && track == 1){
-        ledg = 1;
-        ledr = 0;
+    if (fabs(setpointx - point1x) <= 0.3 && fabs(setpointy - point1y) <= 0.3 && (track == 1 || track == 41)){
+        ledg = 1; ledr = 0; ledb = 1;   // Turns the Led RED
         setpointy = point2y;          // Van punt 1 naar punt 2 op dezelfde y blijven. 
         track = 12;
     }
@@ -145,9 +167,8 @@
     }
     
     // Van punt 2 naar punt 3. 
-    if (fabs(setpointx - point2x) <= 0.3 && fabs(setpointy - point2y) <= 0.3){
-        ledr = 1;
-        ledg = 0;
+    if (fabs(setpointx - point2x) <= 0.3 && fabs(setpointy - point2y) <= 0.3 && (track == 12)){
+        ledr = 1; ledg = 0; ledb = 1;   // Turns the Led GREEN
         setpointx = point3x;
         track = 23;
     }
@@ -160,24 +181,25 @@
     // Van punt 3 naar punt 4. 
     if ((fabs(setpointx - point3x) <= 0.3) && (fabs(setpointy - point3y) <= 0.3) && (track == 23))
     {
-        ledg = 1;
-        ledr = 1;
-        ledb = 0;
+        ledg = 1; ledr = 1; ledb = 0;   // Turns the Led BLUE
         setpointy = setpointy;
         track = 34;
     }
-    if (setpointy > point3y && track == 34)
+    if (setpointx > point4x && track == 34)
     {
         setpointy = setpointy;
     }     
     
-    
+    // Van punt 4 naar punt 1.  
     if ((fabs(setpointx - point4x) <= 0.3) && (fabs(setpointy - point4y) <= 0.3) && (track == 34))
     {
-        ledg = 1; 
-        ledr = 1; 
-        ledb = 1;
-        track = 1;
+        ledg = 0; ledr = 0; ledb = 0;   // Turns the LED WHITE
+        track = 41;
+    }
+    
+    if (setpointy < point1y && track == 41)
+    {
+        setpointy = setpointy + (0.2);        // Van punt 4 naar punt 2 op dezelfde x blijven.
     }
 
     return setpointy;
@@ -190,9 +212,10 @@
 double PI_controller1(double error1)
 {
     static double error_integral1 = 0;
+    static double error_prev1 = error1; // initialization with this value only done once!
     
     // Proportional part
-    double Kp1 = 3.95;              // Kp (proportionele controller, nu nog een random waarde)
+    //double Kp1 = 3.95;              // Kp (proportionele controller, nu nog een random waarde)
     double u_p1 = Kp1*error1;        // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
     
     // Integral part
@@ -201,8 +224,13 @@
     error_integral1 = error_integral1 + error1 * Ts1;  
     double u_i1 = Ki1 * error_integral1;
     
+    // Derivative part
+    double error_derivative1 = (error1 - error_prev1)/Ts1; 
+    double u_d1 = Kd1 * error_derivative1; 
+    error_prev1 = error1;
+    
     // Sum 
-    U1 = u_p1 + u_i1;
+    U1 = u_p1 + u_i1 + u_d1;
     
     // Return
     return U1;      
@@ -210,9 +238,10 @@
 double PI_controller2(double error2)
 {
     static double error_integral2 = 0;
+    static double error_prev2 = error2; // initialization with this value only done once!
     
     // Proportional part
-    double Kp2 = 3.95;              // Kp (proportionele controller, nu nog een random waarde)
+    //double Kp2 = 3.95;              // Kp (proportionele controller, nu nog een random waarde)
     double u_p2 = Kp2*error2;        // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
     
     // Integral part
@@ -221,8 +250,13 @@
     error_integral2 = error_integral2 + error2 * Ts2;  
     double u_i2 = Ki2 * error_integral2;
     
+    // Derivative part
+    double error_derivative2 = (error2 - error_prev2)/Ts2; 
+    double u_d2 = Kd2 * error_derivative2; 
+    error_prev2 = error2;
+    
     // Sum +
-    U2 = u_p2 + u_i2;
+    U2 = u_p2 + u_i2 + u_d2;
     
     // Return
     return U2;      
@@ -239,9 +273,9 @@
 double makeAngleq2(double x, double y){
     double q2 = -acos((pow(x,sq)+pow(y,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3)); //angle of the second joint in setpoint configuration
     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
-    double q2_motor = (pi - q2)+q1; //because q2 represents the angle at joint two and not at the motor a calculation has to be done
-    q2_diff = (2.0*(q2_motor - q2_0_enc))/(2.0*pi); //the actual amount of radians that the motor has to turn in total to reach the setpoint
-    return -q2_diff;
+    double q2_motor = (pi - q2) - q1; //because q2 represents the angle at joint two and not at the motor a calculation has to be done
+    q2_diff = (2.0*(q2_motor - q2_0_enc)); //the actual amount of radians that the motor has to turn in total to reach the setpoint
+    return q2_diff;
 }
 
 
@@ -283,14 +317,17 @@
     motor2_pwm.period_us(60);
     Demo.attach(&rundemo, 0.005f); 
     
+    pc.printf("\r\n_____ DEMO _____\r\n\r\n");
+    
     ledr = 1;
     ledg = 1;
     ledb = 1;
     
     while (true) {
-        pc.printf("Setpointx: %0.2f, Setpointy: %02f, 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);
+        //pc.printf("Setpointx: %0.2f, Setpointy: %02f, 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);
+        pc.printf("Kp-waarde: %0.3f en Kd-waarde: %0.3f\r\n",Kp1,Kd1);
         if (track == 1) {
-            pc.printf("Gaat naar positie 1\r\n");
+            pc.printf("Gaat van home naar positie 1\r\n");
         }
         else if (track == 12) {
             pc.printf("Gaat naar positie 2\r\n");
@@ -301,6 +338,37 @@
         else if (track == 34) {
             pc.printf("Gaat naar positie 4\r\n");
         }
+        else if (track == 41) {
+            pc.printf("Gaat naar positie 1\r\n");
+        }
+        
+        
+        potwaarde1 = potmeter1.read();  // Lees de potwaardes uit
+        pot1 = potwaarde1*1.1;
+        if (pot1 <= 0.10) 
+        {
+            Kp1 = 3.95;
+            Kp2 = 3.95;
+        } 
+        else if (pot1 >= 0.10)
+        {
+            Kp1 = 3.95 + 40*(pot1-0.1);
+            Kp2 = 3.95 + 40*(pot1-0.1);
+        }
+        potwaarde2 = potmeter2.read();  // Lees de potwaardes uit
+        pot2 = potwaarde2*1.1;
+        
+        if (pot2 <= 0.10)
+        {
+            Kd1 = 0;
+            Kd2 = 0;
+        }
+        else if (pot2 >= 0.10)
+        {
+            Kd1 = 10*(pot2-0.1);
+            Kd2 = 10*(pot2-0.1);
+        }
+        
         wait(0.5f);
     }
 }    
\ No newline at end of file