group14 - k9 / Mbed 2 deprecated PIDcontroller

PID controller test code

Dependencies:   mbed Encoder HIDScope

main.cpp@4:b8c8b94aa890, 2015-10-13 (annotated)

Committer:
S1lverEagle
Date:
Tue Oct 13 14:56:12 2015 +0000
Revision:
4:b8c8b94aa890
Parent:
3:c4c2d42cedd4 
PID werkt, geen trilling meer

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 3:c4c2d42cedd4 3 #include "HIDScope.h"
S1lverEagle 0:88e83d97a0ee 4
S1lverEagle 0:88e83d97a0ee 5 Ticker control_ticker;
S1lverEagle 2:323606a75d64 6 PwmOut pwm_motor1(D5); //onderste motor
S1lverEagle 2:323606a75d64 7 DigitalOut dir_motor1(D4);
S1lverEagle 0:88e83d97a0ee 8 AnalogIn potmeter1(A4);
S1lverEagle 1:64ec36ba6e76 9 Encoder encoder1(D13,D12,true);
S1lverEagle 1:64ec36ba6e76 10 double pos_1;
S1lverEagle 3:c4c2d42cedd4 11 HIDScope scope(1);
S1lverEagle 0:88e83d97a0ee 12
S1lverEagle 0:88e83d97a0ee 13 // Sample time (motor1−timestep)
S1lverEagle 0:88e83d97a0ee 14 const double m1_Ts = 0.01;
S1lverEagle 0:88e83d97a0ee 15 // Controller gains (motor1−Kp,−Ki,...)
S1lverEagle 3:c4c2d42cedd4 16 const double m1_Kp = 5, m1_Ki = 0.005, m1_Kd = 2;
S1lverEagle 0:88e83d97a0ee 17 double m1_err_int = 0, m1_prev_err = 0;
S1lverEagle 0:88e83d97a0ee 18 // Derivative filter coeicients (motor1−filter−b0,−b1,...)
S1lverEagle 3:c4c2d42cedd4 19 //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 20 // Filter variables (motor1−filter−v1,−v2)
S1lverEagle 0:88e83d97a0ee 21 double m1_f_v1 = 0, m1_f_v2 = 0;
S1lverEagle 0:88e83d97a0ee 22
S1lverEagle 3:c4c2d42cedd4 23 const double BiGain1 = 0.016955;
S1lverEagle 3:c4c2d42cedd4 24 const double m1_f_a1 = -0.96608908283*BiGain1, m1_f_a2 = 0.0*BiGain1, m1_f_b0 = 1.0*BiGain1, m1_f_b1 = 1.0*BiGain1, m1_f_b2 = 0.0*BiGain1;
S1lverEagle 3:c4c2d42cedd4 25
S1lverEagle 0:88e83d97a0ee 26 // Biquad filter (See slide 14)
S1lverEagle 0:88e83d97a0ee 27 double biquad( double u, double &v1, double &v2, const double a1, const double a2,
S1lverEagle 0:88e83d97a0ee 28 const double b0, const double b1, const double b2 )
S1lverEagle 0:88e83d97a0ee 29 {
S1lverEagle 1:64ec36ba6e76 30 double v = u - a1*v1 - a2*v2;
S1lverEagle 1:64ec36ba6e76 31 double y = b0*v + b1*v1 + b2*v2;
S1lverEagle 0:88e83d97a0ee 32 v2 = v1;
S1lverEagle 0:88e83d97a0ee 33 v1 = v;
S1lverEagle 0:88e83d97a0ee 34 return y;
S1lverEagle 0:88e83d97a0ee 35 }
S1lverEagle 0:88e83d97a0ee 36
S1lverEagle 0:88e83d97a0ee 37 // Reusable PID controller with filter
S1lverEagle 1:64ec36ba6e76 38 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 39 {
S1lverEagle 0:88e83d97a0ee 40 // Derivative
S1lverEagle 1:64ec36ba6e76 41 double e_der = (e - e_prev)/Ts;
S1lverEagle 0:88e83d97a0ee 42 e_der = biquad( e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2 );
S1lverEagle 0:88e83d97a0ee 43 e_prev = e;
S1lverEagle 0:88e83d97a0ee 44 // Integral
S1lverEagle 1:64ec36ba6e76 45 e_int = e_int + Ts * e;
S1lverEagle 0:88e83d97a0ee 46 // PID
S1lverEagle 1:64ec36ba6e76 47 return Kp * e + Ki * e_int + Kd * e_der;
S1lverEagle 0:88e83d97a0ee 48 }
S1lverEagle 0:88e83d97a0ee 49
S1lverEagle 3:c4c2d42cedd4 50
S1lverEagle 3:c4c2d42cedd4 51
S1lverEagle 0:88e83d97a0ee 52 void m1_Controller()
S1lverEagle 0:88e83d97a0ee 53 {
S1lverEagle 3:c4c2d42cedd4 54 double reference = (potmeter1.read()-0.5)*4200;
S1lverEagle 3:c4c2d42cedd4 55 double position = encoder1.getPosition(); // Don’t use magic numbers!
S1lverEagle 4:b8c8b94aa890 56 double error = (reference - position)/1000;
S1lverEagle 3:c4c2d42cedd4 57 pos_1 = PID( error, 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 3:c4c2d42cedd4 58 //pos_1 = (reference - position) *0.001;
S1lverEagle 1:64ec36ba6e76 59 if(pos_1 > 0)
S1lverEagle 0:88e83d97a0ee 60 dir_motor1 = 0;
S1lverEagle 0:88e83d97a0ee 61 else
S1lverEagle 0:88e83d97a0ee 62 dir_motor1 = 1;
S1lverEagle 0:88e83d97a0ee 63
S1lverEagle 1:64ec36ba6e76 64 pwm_motor1.write(abs(pos_1));
S1lverEagle 3:c4c2d42cedd4 65
S1lverEagle 3:c4c2d42cedd4 66 scope.set(0,position);
S1lverEagle 3:c4c2d42cedd4 67 scope.send();
S1lverEagle 3:c4c2d42cedd4 68
S1lverEagle 0:88e83d97a0ee 69 } // Attach this function to a Ticker
S1lverEagle 0:88e83d97a0ee 70
S1lverEagle 0:88e83d97a0ee 71 int main()
S1lverEagle 0:88e83d97a0ee 72 {
S1lverEagle 3:c4c2d42cedd4 73 pwm_motor1.period(0.0001);
S1lverEagle 2:323606a75d64 74 control_ticker.attach(&m1_Controller, 0.01);
S1lverEagle 3:c4c2d42cedd4 75
S1lverEagle 3:c4c2d42cedd4 76 while(1) {
S1lverEagle 3:c4c2d42cedd4 77
S1lverEagle 3:c4c2d42cedd4 78 }
S1lverEagle 0:88e83d97a0ee 79 }