![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Motor programma
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of frdm_Motor_V2_3 by
Diff: main.cpp
- Revision:
- 12:69ab81cf5b7d
- Parent:
- 11:0793a78109a2
- Child:
- 13:a6770307a5d2
--- a/main.cpp Tue Sep 29 14:01:47 2015 +0000 +++ b/main.cpp Tue Sep 29 16:28:55 2015 +0000 @@ -2,16 +2,15 @@ #include "MODSERIAL.h" #include "HIDScope.h" #include "QEI.h" +#include "biquadFilter.h" Serial pc(USBTX, USBRX); // tx, rx DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield) PwmOut motor2speed(D5); -DigitalIn button1(SW3); -DigitalIn encoderA(D3); -DigitalIn encoderB(D2); AnalogIn potmeter2(A5); QEI Encoder(D3, D2, NC, 128); HIDScope scope(3); + Ticker ScopeTime; Ticker myControllerTicker; @@ -30,24 +29,49 @@ scope.send(); } +// Sample time (motor2-step) +const double m2_Ts = 0.01; + // Controller gain -const double m2_Kp = 0.07,m2_Ki = 0.005; -const double m2_Ts = 0.01; -double m2_err_int = 0; +const double m2_Kp = 0.07,m2_Ki = 0.005, m2_Kd = 0.005; +double m2_err_int = 0, m1_prev_err = 0; + +//Derivative filter coeffs +const double m2_f_a1 = 1.0, m2_f_a2 = 1.0, m2_f_b0 = 1.0, m2_f_b1 = 1.0, m2_f_b2 = 1.0; +// Filter variables +double m2_f_v1 = 0, m2_f_v2 = 0; -// Reusable PI controller -double P( double error, const double Kp, const double Ki, double Ts, double &e_int) +// Biquad filter +double biquad( double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2 ) { - e_int = e_int + Ts*error; - return Kp * error + Ki*e_int; + double v = u − a1∗v1 − a2∗v2; + double y = b0∗v + b1∗v1 + b2∗v2; + v2 = v1; v1 = v; + return y; +} + + +// Reusable PID controller +double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, double &f_v2, +const double f_a1,const double f_a2, const double f_b0, const double f_b1, const double f_b3) +{ + // Derivative + double e_der = (e-e_prev)/Ts; + e_der = biquad(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2); + e_prev = e + // Integral + e_int = e_int + Ts*e; + // PID + return Kp * e + Ki*e_int + Kd*e_der; } -// Next task, measure the error and apply the output to the plant +// Motor control void motor2_Controller() { reference = potmeter2.read()*360; position = Encoder.getPulses()*360/(0.5*128*131); // Aantal Degs - double P2 = P( reference - position, m2_Kp, m2_Ki, m2_Ts, m2_err_int); + double P2 = PID( reference − position, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2, + m2_f_b0, m2_f_b1, m2_f_b2); motor2speed = abs(P2); if(P2 > 0) {