Diff: main_demo.cpp

Revision:

8:088904ebb85b

Parent:

7:2e0cdf836495

--- a/main_demo.cpp	Thu Nov 01 15:54:44 2018 +0000
+++ b/main_demo.cpp	Thu Nov 01 17:02:30 2018 +0000
@@ -33,6 +33,10 @@
 const double  pi = 3.14159265359;
 volatile double  error1;
 volatile double  error2;
+volatile double error1_final = 10.0;
+volatile double error2_final = 10.0;
+volatile double q1_diff_final;
+volatile double q2_diff_final;
 double  point1x = 200.0;
 double  point1y = 200.0;
 double  point2x = 350.0;
@@ -48,11 +52,13 @@
 volatile double  setpointy = y0;
 volatile double  U1;
 volatile double  U2;
+volatile double xfinal;
+volatile double yfinal;
 
 // Determine demo setpoints
 const double stepsize1 = 0.15;
 const double stepsize2 = 0.25;
-const double setpoint_error = 0.3;
+const double setpoint_error = 0.03;
 
 // Inverse Kinematics
 volatile double q1_diff;
@@ -67,10 +73,6 @@
 double q2_0_enc = q2_0 - q1_0;
 
 // Controller
-double potwaarde1;
-double pot1;
-double potwaarde2;
-double pot2;
 volatile double Kp1 = 20.0;                 // Kp (proportionele controller, nu nog een random waarde)
 volatile double Kp2 = 20.0;                 // Kp (proportionele controller, nu nog een random waarde)
 volatile double Ki1 = 6.0;                  // Ki (I-actie van de controller)
@@ -99,124 +101,90 @@
 // -------------------------------------------------------------------------
 // -------------- Determine Setpoints --------------------------------------
 // -------------------------------------------------------------------------
-double determinedemosetx(double setpointx, double setpointy)
+void determinedemoset()
 {
-
-    if (setpointx < point1x && track == 1){ 
-        setpointx = setpointx + stepsize1;    
+    if (track == 1){
+        xfinal = point1x;
+        yfinal = point1y;
     }
+        
+    if ((setpointx < point1x) && (track == 1)){ 
+        setpointx = setpointx + stepsize2;    
+    }
+    if ((setpointy < point1y) && (track == 1)){
+        setpointy = setpointy + stepsize1;
+    } 
     
     // Van punt 1 naar punt 2. 
-    if (fabs(setpointx - point1x) <= setpoint_error && fabs(setpointy - point1y) <= setpoint_error && (track == 1 || track == 41)) {
+    if ((error1_final <= setpoint_error) && (error2_final <= setpoint_error) && (track == 1)){
+        //setpointx = point2x;
+        //setpointy = setpointy + (-0.2);          // Van punt 1 naar punt 2 op dezelfde y blijven. 
+        ledr = !ledr;                                // Aangeven met een ROOD lampje dat hij op de plaats van bestemming is
         track = 12;
     }
     
-    if (setpointx < point2x && track == 12){
-        setpointx = setpointx + stepsize2;
-    }
-    
-    // Van punt 2 naar punt 3. 
-    if (fabs(setpointx - point2x) <= setpoint_error && fabs(setpointy - point2y) <= setpoint_error && track == 12)
-    {
-        setpointx = point3x; 
-        track = 23;
+    if (track == 12){
+        xfinal = point2x;
+        yfinal = point2y;
     }
     
-    if (setpointy > point3y && track == 23)
-    {
-        setpointx = point3x;          // Van punt 1 naar punt 2 op dezelfde x blijven. 
-    } 
- 
-
-    // Van punt 3 naar punt 4. 
-    if ((fabs(setpointx - point3x) <= setpoint_error) && (fabs(setpointy - point3y) <= setpoint_error) && (track == 23))
-    {
-        setpointy = point4y;
-        track = 34;
-    }
-    
-    if (setpointx > point4x && track == 34)
-    {
-        setpointx = setpointx - stepsize2;
-    }
-    
-    // Van punt 4 naar punt 1.        
-    if ((fabs(setpointx - point4x) <= setpoint_error) && (fabs(setpointy - point4y) <= setpoint_error) && (track == 34))
-    {
-        setpointx = point4x;
-        track = 41;
+    if ((setpointy > point2y) && (track == 12)) {
+        setpointx = point2x;
+        //setpointx = setpointx + 0.2;
+        setpointy = setpointy + (-stepsize2);
     }
     
-    if (setpointy < point1y && track == 41)
-    {
-        setpointx = point4x;        // Van punt 4 naar punt 2 op dezelfde x blijven.
+    if ((error1_final <= setpoint_error) && (error2_final <= setpoint_error) && (track == 12)){
+        ledr = !ledr;
+        ledg = !ledg;
+        track = 23;
+        }
+    
+    
+    /**
+    if (setpointy > point2y && track == 12){
+        setpointx = point2x;
+        setpointy = setpointy + (-0.2);
     }
     
-
-      return setpointx;
-}  
-
-double determinedemosety(double setpointx, double setpointy)
-{
-    // Van reference positie naar punt 1.
-    if(setpointy < point1y && track == 1){
-        setpointy = setpointy + (stepsize2);
-    } 
-
-    // Van punt 1 naar punt 2. 
-    if (fabs(setpointx - point1x) <= setpoint_error && fabs(setpointy - point1y) <= setpoint_error && (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;
-    }
-    if (setpointx < point2x && track == 12){
-        setpointy = point2y;
-    }
     
     // Van punt 2 naar punt 3. 
-    if (fabs(setpointx - point2x) <= setpoint_error && fabs(setpointy - point2y) <= setpoint_error && (track == 12)){
-        ledr = 1; ledg = 0; ledb = 1;   // Turns the Led GREEN
-        setpointx = point3x;
+    if (fabs(setpointx - point2x) <= 0.3 && fabs(setpointy - point2y) <= 0.3)
+    {
+        //setpointx = setpointx - 0.2; 
+        //setpointy = setpointy;
+        ledr = 1;
+        ledg = 0;
         track = 23;
     }
-    if ((setpointy > point3y) && (track == 23))
+    if (setpointy > point3y && track == 23)
     {
-        setpointy = setpointy + (-stepsize2);
+        //setpointx = setpointx - 0.2;          // Van punt 1 naar punt 2 op dezelfde y blijven. 
+        //setpointy = setpointy;
         track = 23;
-    }    
+    } 
     
+        
     // Van punt 3 naar punt 4. 
-    if ((fabs(setpointx - point3x) <= setpoint_error) && (fabs(setpointy - point3y) <= setpoint_error) && (track == 23))
+    if (setpointy >= point3y - 0.3 && setpointx >= point3x - 0.3 && setpointy <= point3y + 0.3 && setpointx <= point3x + 0.3)
     {
-        ledg = 1; ledr = 1; ledb = 0;   // Turns the Led BLUE
+        setpointx = setpointx - 0.1;          // Van punt 1 naar punt 2 op dezelfde y blijven. 
         setpointy = setpointy;
         track = 34;
     }
-    if (setpointx > point4x && track == 34)
+    if (setpointy > point3y && track == 34)
     {
+        setpointx = setpointx - 0.1;
         setpointy = setpointy;
-    }     
-    
-    // Van punt 4 naar punt 1.  
-    if ((fabs(setpointx - point4x) <= setpoint_error) && (fabs(setpointy - point4y) <= setpoint_error) && (track == 34))
-    {
-        ledg = 0; ledr = 0; ledb = 0;   // Turns the LED WHITE
-        track = 41;
+
     }
-    
-    if (setpointy < point1y && track == 41)
-    {
-        setpointy = setpointy + (stepsize2);        // Van punt 4 naar punt 2 op dezelfde x blijven.
-    }
+    */
 
-    return setpointy;
-    
-}
-
+}  
 // -----------------------------------------------------------------    
 // --------------------------- PI controllers ----------------------
 // -----------------------------------------------------------------
-double PI_controller1(double error1)
+double PID_controller1(double error1)
 {
     static double error_integral1 = 0;
     static double error_prev1 = error1; // initialization with this value only done once!
@@ -241,7 +209,7 @@
     // Return
     return U1;      
 }
-double PI_controller2(double error2)
+double PID_controller2(double error2)
 {
     static double error_integral2 = 0;
     static double error_prev2 = error2; // initialization with this value only done once!
@@ -289,18 +257,23 @@
 // -----------------------------------------------
 void motoraansturing()
 {
-    setpointx = determinedemosetx(setpointx, setpointy);
-    setpointy = determinedemosety(setpointx, setpointy);
+    determinedemoset();
     q1_diff = makeAngleq1(setpointx, setpointy);
     q2_diff = makeAngleq2(setpointx, setpointy);
+    q1_diff_final = makeAngleq1(xfinal, yfinal);
+    q2_diff_final = makeAngleq2(xfinal, yfinal);
 
     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);   
+    //errors die de setpoint bepalen
+    error1_final = q1_diff_final - theta1;
+    error2_final = q2_diff_final - theta2;
+    
+    U1 = PID_controller1(error1);       // Voltage dat naar de motor gestuurd wordt. 
+    U2 = PID_controller2(error2);   
     
     motor1_pwm.write(fabs(U1));         // Motor aansturen
     directionM1 = U1 > 0.0f;            // Richting van de motor bepalen
@@ -330,13 +303,14 @@
     
     while (true) {
         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 van home 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");
         }
@@ -348,6 +322,6 @@
         }
         */
         
-        wait(0.1f);
+        wait(1.0f);
     }
 }    
\ No newline at end of file