![](/media/cache/group/9e3cc099b3b04bca937a1cca1da81b19.jpg.50x50_q85.jpg)
Motor control
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 3:07fedd2e252c
- Parent:
- 2:d7286c36595f
- Child:
- 4:e7d50c6a7c53
diff -r d7286c36595f -r 07fedd2e252c main.cpp --- a/main.cpp Fri Oct 11 11:49:34 2019 +0000 +++ b/main.cpp Fri Oct 11 12:02:22 2019 +0000 @@ -1,8 +1,8 @@ #include "mbed.h" -//#include "HIDScope.h" +#include "HIDScope.h" #include "QEI.h" #include "MODSERIAL.h" -//#include "BiQuad.h" +#include "BiQuad.h" #include "FastPWM.h" // Button and potmeter control @@ -33,12 +33,13 @@ double potValue; double pi2= 6.283185; float e; //e = error -float Kp=0.1; -float Ki=0.1; -float Kd=0.7; +float Kp=17.5; +float Ki=1; +float Kd=21.5; float u_k; float u_i; float u_d; + // PC connection MODSERIAL pc(USBTX, USBRX); @@ -60,6 +61,8 @@ float PID_controller(float e){ static float error_integral=0; + static float e_prev=e; + static BiQuad LowPassFilter(0.0640,0.1279,0.0640,-1.1683,0.4241); //Proportional part: u_k=Kp*e; @@ -68,8 +71,14 @@ error_integral=error_integral+e*Ts; u_i=Ki*error_integral; + //Derivative part + float error_derivative =(e-e_prev)/Ts; + float filtered_error_derivative = LowPassFilter.step(error_derivative); + u_d=Kd*filtered_error_derivative; + e_prev=e; + // Sum and return - return u_k+u_i; + return u_k+u_i+u_d; } void motorControl()