group14 - k9 / Mbed 2 deprecated PIDcontroller

PID controller test code

Dependencies:   mbed Encoder HIDScope

main.cpp@0:88e83d97a0ee, 2015-10-12 (annotated)

Committer:
S1lverEagle
Date:
Mon Oct 12 13:58:40 2015 +0000
Revision:
0:88e83d97a0ee
Child:
1:64ec36ba6e76
eerste versie PID controller

Who changed what in which revision?

UserRevisionLine numberNew contents of line
S1lverEagle 0:88e83d97a0ee 1 #include 'mbed.h'
S1lverEagle 0:88e83d97a0ee 2
S1lverEagle 0:88e83d97a0ee 3 Ticker control_ticker;
S1lverEagle 0:88e83d97a0ee 4 PwmOut pwm_motor1(D4); //onderste motor
S1lverEagle 0:88e83d97a0ee 5 DigitalOut dir_motor1(D5);
S1lverEagle 0:88e83d97a0ee 6 AnalogIn potmeter1(A4);
S1lverEagle 0:88e83d97a0ee 7
S1lverEagle 0:88e83d97a0ee 8 // Sample time (motor1−timestep)
S1lverEagle 0:88e83d97a0ee 9 const double m1_Ts = 0.01;
S1lverEagle 0:88e83d97a0ee 10 // Controller gains (motor1−Kp,−Ki,...)
S1lverEagle 0:88e83d97a0ee 11 const double m1_Kp = 2.5, m1_Ki = 1.0, m1_Kd = 0.5;
S1lverEagle 0:88e83d97a0ee 12 double m1_err_int = 0, m1_prev_err = 0;
S1lverEagle 0:88e83d97a0ee 13 // Derivative filter coeicients (motor1−filter−b0,−b1,...)
S1lverEagle 0:88e83d97a0ee 14 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;
S1lverEagle 0:88e83d97a0ee 15 // Filter variables (motor1−filter−v1,−v2)
S1lverEagle 0:88e83d97a0ee 16 double m1_f_v1 = 0, m1_f_v2 = 0;
S1lverEagle 0:88e83d97a0ee 17
S1lverEagle 0:88e83d97a0ee 18 // Biquad filter (See slide 14)
S1lverEagle 0:88e83d97a0ee 19 double biquad( double u, double &v1, double &v2, const double a1, const double a2,
S1lverEagle 0:88e83d97a0ee 20 const double b0, const double b1, const double b2 )
S1lverEagle 0:88e83d97a0ee 21 {
S1lverEagle 0:88e83d97a0ee 22 double v = u − a1∗v1 − a2∗v2;
S1lverEagle 0:88e83d97a0ee 23 double y = b0∗v + b1∗v1 + b2∗v2;
S1lverEagle 0:88e83d97a0ee 24 v2 = v1;
S1lverEagle 0:88e83d97a0ee 25 v1 = v;
S1lverEagle 0:88e83d97a0ee 26 return y;
S1lverEagle 0:88e83d97a0ee 27 }
S1lverEagle 0:88e83d97a0ee 28
S1lverEagle 0:88e83d97a0ee 29 // Reusable PID controller with filter
S1lverEagle 0:88e83d97a0ee 30 double PID( double e, const double Kp, const double Ki, const double Kd, double Ts,
S1lverEagle 0:88e83d97a0ee 31 double &e_int, double &e_prev, double &f_v1, double &f_v2, const double f_a1,
S1lverEagle 0:88e83d97a0ee 32 const double f_a2, const double f_b0, const double f_b1, const double f_b3)
S1lverEagle 0:88e83d97a0ee 33 {
S1lverEagle 0:88e83d97a0ee 34 // Derivative
S1lverEagle 0:88e83d97a0ee 35 double e_der = (e − e_prev)/Ts;
S1lverEagle 0:88e83d97a0ee 36 e_der = biquad( e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2 );
S1lverEagle 0:88e83d97a0ee 37 e_prev = e;
S1lverEagle 0:88e83d97a0ee 38 // Integral
S1lverEagle 0:88e83d97a0ee 39 e_int = e_int + Ts ∗ e;
S1lverEagle 0:88e83d97a0ee 40 // PID
S1lverEagle 0:88e83d97a0ee 41 return Kp ∗ e + Ki ∗ e_int + Kd ∗ e_der;
S1lverEagle 0:88e83d97a0ee 42 }
S1lverEagle 0:88e83d97a0ee 43
S1lverEagle 0:88e83d97a0ee 44 void m1_Controller()
S1lverEagle 0:88e83d97a0ee 45 {
S1lverEagle 0:88e83d97a0ee 46 double reference = potmeter1.read();
S1lverEagle 0:88e83d97a0ee 47 double position = 0.002991∗encoder1.getPosition(); // Don’t use magic numbers!
S1lverEagle 0:88e83d97a0ee 48 motor1 = PID( reference − position, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int,
S1lverEagle 0:88e83d97a0ee 49 m1_prev_err, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2 );
S1lverEagle 0:88e83d97a0ee 50 if(motor1 > 0)
S1lverEagle 0:88e83d97a0ee 51 dir_motor1 = 0;
S1lverEagle 0:88e83d97a0ee 52 else
S1lverEagle 0:88e83d97a0ee 53 dir_motor1 = 1;
S1lverEagle 0:88e83d97a0ee 54
S1lverEagle 0:88e83d97a0ee 55 pwm_motor1.write(abs(motor1));
S1lverEagle 0:88e83d97a0ee 56 } // Attach this function to a Ticker
S1lverEagle 0:88e83d97a0ee 57
S1lverEagle 0:88e83d97a0ee 58 int main()
S1lverEagle 0:88e83d97a0ee 59 {
S1lverEagle 0:88e83d97a0ee 60 control_ticker.attach(m1_Controller, 0.01);
S1lverEagle 0:88e83d97a0ee 61
S1lverEagle 0:88e83d97a0ee 62 while(1);
S1lverEagle 0:88e83d97a0ee 63 }