Motor control, feedback, PI controller, BiQuad filter
Dependencies: FastPWM HIDScope MODSERIAL biquadFilter mbed QEI
Diff: main.cpp
- Revision:
- 17:4a0912c93771
- Parent:
- 15:c2cfab737a4c
- Child:
- 18:e522dfbab4c6
- Child:
- 19:1353ba4d94db
--- a/main.cpp Fri Oct 19 09:30:44 2018 +0000 +++ b/main.cpp Fri Oct 19 12:08:31 2018 +0000 @@ -14,17 +14,15 @@ //Tickers Ticker MeasureControl; Ticker print; -/*Ticker MakeMotorRotate;*/ //Waar zal deze ticker voor dienen? //Global variables volatile double measuredPosition = 0.0; volatile double referencePosition = 0.0; volatile double motorValue= 0.01; -volatile double Kp = 0.0; //dit maken we variabel, dit zorgt voor een grote of kleine overshoot +volatile double Kp = 5.0; //dit maken we variabel, dit zorgt voor een grote of kleine overshoot volatile double Ki = 1.0; //dit moeten we bepalen met een plot bijvoorbeeld +volatile double Kd = 0.0; volatile double Ts = 0.01; -volatile double Error_integral = 0.0; - //------------------------------------------------------------------------------ // Functions @@ -45,14 +43,22 @@ double FeedbackControl(double Error) { + static double Error_integral = 0; + static double Error_prev = Error; + //static BiQuad LowPassFilter(..., ..., ..., ..., ...) // Proportional part: - Kp = 20*potMeter2.read(); //van 0 tot 20, waardes rond de 5 zijn het beste (minder overshoot + minder trilling motor beste combinatie hiervan) + //van 0 tot 20, waardes rond de 5 zijn het beste (minder overshoot + minder trilling motor beste combinatie hiervan) double u_k = Kp * Error; // Integral part: Error_integral = Error_integral + Error * Ts; double u_i = Ki * Error_integral; + // Derivative part + double Error_derivative = (Error - Error_prev)/Ts; + Kd = 20*potMeter2.read(); + double u_d = Kd * Error_derivative; + Error_prev = Error; // Sum all parts and return it - return u_k + u_i; //motorValue + return u_k + u_i + u_d; //motorValue } void SetMotor1(double motorValue) @@ -99,7 +105,7 @@ pc.printf("Motorvalue/Error %f \r\n", motorValue); pc.printf("Proportional gain %f \r\n", Kp); pc.printf("Integral gain %f \r\n", Ki); - pc.printf("Error integral %f \r\n", Error_integral); + pc.printf("Derivative gain %f \r\n", Kd); } //----------------------------------------------------------------------------- int main()