Motor control
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 11:94a4dd7ed05c
- Parent:
- 10:b871d1b05787
- Child:
- 12:23b94b5dcc60
diff -r b871d1b05787 -r 94a4dd7ed05c main.cpp --- 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);