Diff: main.cpp

Revision:

11:94a4dd7ed05c

Parent:

10:b871d1b05787

Child:

12:23b94b5dcc60

--- a/main.cpp	Mon Oct 14 14:28:37 2019 +0000
+++ b/main.cpp	Fri Oct 18 10:19:02 2019 +0000
@@ -35,7 +35,7 @@
 bool motordir;
 double motorpwm;
 double premotorpwm;
-float u1= 0;
+float refangle= 0;
 double u2;
 double potValue;
 double pi2= 6.283185;
@@ -45,6 +45,13 @@
 float u_k;
 float u_i;
 
+//Windup control
+float ux;
+float up;
+float ek;
+float ei= 0;
+float Ka= 1;
+
 //Hidscope
 HIDScope scope(3); //Going to send 3 channels of data. To access data go to 'http:/localhost:18082/' after starting HIDScope application.
 // PC connection
@@ -69,7 +76,7 @@
 
 float PID_controller(float e){
     static float error_integral=0;
-    static float e_prev=e;
+    //static float e_prev=e;
     
     //Proportional part:
     Kp=potmeter1.read();
@@ -77,11 +84,24 @@
     
     //Integral part
     Ki=potmeter2.read();
-    error_integral=error_integral+e*Ts;
+    error_integral=error_integral+ei*Ts;
     u_i=Ki*error_integral;
     
-    // Sum and return
-    return u_k+u_i;    
+    // Sum and limit
+    u2= u_k+u_i;
+    if (u2>1){
+        up=1;}
+    else if (u2<-1){
+        up=-1;}
+    else {
+        up=u2;}
+    
+    // To prevent windup
+    ux= u2-up;
+    ek= Ka*ux;
+    ei= e-ek;
+     //Return
+     return up; 
 }
 
 
@@ -94,20 +114,22 @@
 }
 
 void togglehoek(){
-    static float t = 0;
-    u1= pi2/3.0f*sin(5.0f*t)*motor1Toggle;
-    t+=0.01;
+    
+    refangle= 0.5*pi2+refangle;
+    // static float t = 0;
+    // refangle= pi2/3.0f*sin(5.0f*t)*motor1Toggle;
+    //t+=0.01;
     }
     
 void motorControl()
 {
-    togglehoek();
-    //button1.fall(&togglehoek);
+    //togglehoek();
+    button1.fall(&togglehoek);
     angle = counts * factorin / gearratio; // Angle of motor shaft in rad
     omega = deltaCounts / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
     potValue= potmeter1.read();
-    //u1= (potValue*2*pi2)-pi2;
-    e=u1-angle;
+    //refangle= (potValue*2*pi2)-pi2;
+    e=refangle-angle;
     
     u2=PID_controller(e);
     
@@ -125,7 +147,7 @@
 }
 
 void Plotje(){
-    scope.set(0,u1); //gewenste hoek
+    scope.set(0,refangle); //gewenste hoek
     scope.set(1,angle); //Gemeten hoek
     scope.set(2,e); //verschil in gewenste en gemeten hoek
 
@@ -154,7 +176,7 @@
         //pc.printf("Potmeter: %d \r\n", potValue,);
         //pc.printf("Counts: %i   DeltaCounts: %i\r\n", counts, deltaCounts);
         pc.printf("Angle:  %f   Omega:       %f\r\n", angle, omega);
-        pc.printf("U1: %f   Error:  %f     \r\n",u1, e);
+        pc.printf("refangle: %f   Error:  %f     \r\n",refangle, e);
         pc.printf("Kp: %f   Ki:    %f \r\n", Kp, Ki);
         
         wait(0.5);