group14 - k9 / Mbed 2 deprecated PIDcontroller

PID controller test code

Dependencies:   mbed Encoder HIDScope

main.cpp@2:323606a75d64, 2015-10-13 (annotated)

Committer:
S1lverEagle
Date:
Tue Oct 13 13:42:53 2015 +0000
Revision:
2:323606a75d64
Parent:
1:64ec36ba6e76
Child:
3:c4c2d42cedd4
Motor werkt, code niet

Who changed what in which revision?

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