PID controller voor 1 motor die een hoek van 20 graden draait, niet werkend.
Dependencies: MODSERIAL QEI mbed biquadFilter
Inverse Kinematics + PID Controller
Diff: main.cpp
- Revision:
- 0:26ce65a63616
- Child:
- 1:e5d2d638eaf4
diff -r 000000000000 -r 26ce65a63616 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Oct 14 09:26:14 2016 +0000 @@ -0,0 +1,96 @@ +#include "stdio.h" +#include "math.h" +#include "mbed.h" +#include "QEI.h" +#include "MODSERIAL.h" + +MODSERIAL pc(USBTX, USBRX); +QEI wheel_M1 (D10, D11, NC, 64); +QEI wheel_M2 (D13, D12, NC, 64); +PwmOut pwm_M1 (D5); +PwmOut pwm_M2 (D6); +DigitalOut dir_M1 (D4); +DigitalOut dir_M2 (D7); + +Ticker PIDcontrol; + +volatile double q1; +volatile double q2; +volatile double q1_prev = 0; +volatile double q2_prev = 0; +volatile double pwm_M1_ref = 0; +volatile double pwm_M2_ref = 0; +volatile double Pulses_M1; +volatile double Pulses_M2; + +void initialize() +{ + dir_M1 = 0; //ccw + dir_M2 = 1; //cw + while (q1 > -20*2*3.1415/360) { + Pulses_M1 = wheel_M1.getPulses(); //1334.355 counts/rad + q1 = Pulses_M1 / (1334.355/3); // rad + pwm_M1_ref = 0.1; + } + pwm_M1_ref = 0; + + //while (q2 < 45*2*3.1415/360) { + // Pulses_M2 = wheel_M2.getPulses(); + // q2 = Pulses_M2 / (1334.355/3); + // pwm_M2_ref = 0.1; + //} + //pwm_M2_ref = 0; + +const double M1_TS = 0.002; +const double M1_Kp = 4.348, M1_Ki = 36.632, M1_Kd = 0.126; +double M1_err_int = 0, n1_prev_err = 0; +const double M1_F_A1 = 1.0, M1_F_A2 = 2.0, M1_F_B0 = 1.0, M1_F_B1 = 3.0, M1_F_B2 = 4.0; +double M2_f_v2 = 0, m2_f_v2 = 0; + +//Biquad filter + +//volatile double u; +//volatile double v; +//volatile double v1; +//volatile double v2; +//volatile double y; +//double a1; +//double a2; +//double b0; +//double b1; +//double b2; + +double biquad(double u, double &v1, double &v2, double &v2,const double a1,const double a2,const double b0,const double b1,const double b2); +{ + double v = u - a1*v1 - a2*v2; + double y = b0*v + b1*v1 + b2*v2; + v2 = v1;v1=v; + return y; +} + +//PID filter +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_b2){ + // 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; +} + +void M1_controller(){ + double reference = pwm_M1; + q1 = wheel_M1.getPulses() / (1334.355/3); + double velocity = (q1 - q1_prev) / 0.002; + q1_prev = q1; + pwm_M1 = PID(reference - velocity, M1_Kp, M1_Ki, M1_Kd, M1_TS, M1_err_int, M1_prev_err, M1_f_v1, M1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2); +} + +int main() +{ + pc.baud(115200); + PIDcontrol.attach(&M1_controller, 0.002f); + initialize(); +} \ No newline at end of file