Motor control, feedback, PI controller, BiQuad filter
Dependencies: FastPWM HIDScope MODSERIAL biquadFilter mbed QEI
Diff: main.cpp
- Revision:
- 18:e522dfbab4c6
- Parent:
- 17:4a0912c93771
--- a/main.cpp Fri Oct 19 12:08:31 2018 +0000 +++ b/main.cpp Fri Oct 19 14:18:38 2018 +0000 @@ -3,6 +3,8 @@ #include "MODSERIAL.h" #include "QEI.h" #include <math.h> + +// Defining input and output pins MODSERIAL pc(USBTX, USBRX); DigitalOut motor1DirectionPin(D7); FastPWM motor1MagnitudePin(D6); @@ -16,13 +18,15 @@ Ticker print; //Global variables + // Measure volatile double measuredPosition = 0.0; volatile double referencePosition = 0.0; -volatile double motorValue= 0.01; -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 motorValue= 0.00; + // Control +volatile double Kp = 0.0; // Proportional gain. Dit maken we variabel, dit zorgt voor een grote of kleine overshoot +volatile double Ki = 0.0; // Integral gain. Dit moeten we bepalen met een plot bijvoorbeeld +volatile double Kd = 0.0; // Diverential gain. +volatile double Ts = 0.01; // Sample time in FeedbackControl //------------------------------------------------------------------------------ // Functions @@ -43,20 +47,29 @@ double FeedbackControl(double Error) { - static double Error_integral = 0; + static double Error_integral = 0.0; static double Error_prev = Error; - //static BiQuad LowPassFilter(..., ..., ..., ..., ...) + /*static BiQuad LowPassFilter(..., ..., ..., ..., ...); */ + + // Read potmeter + Kd = 20*potMeter2.read(); + // Proportional part: - //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 + + // Derivative part: double Error_derivative = (Error - Error_prev)/Ts; - Kd = 20*potMeter2.read(); double u_d = Kd * Error_derivative; + /* Filtered_Error_derivative = LowPassFilter(Error_derivative); + double u_d = Kd * Filtered_Error_derivative; */ Error_prev = Error; + // Sum all parts and return it return u_k + u_i + u_d; //motorValue } @@ -118,7 +131,5 @@ //Other initializations - while(true) - { - } + while(true) { } }