![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
tralalal
main.cpp@0:297b00643040, 2018-10-30 (annotated)
- Committer:
- Marle
- Date:
- Tue Oct 30 18:57:03 2018 +0000
- Revision:
- 0:297b00643040
PID controller;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Marle | 0:297b00643040 | 1 | #include "mbed.h" |
Marle | 0:297b00643040 | 2 | |
Marle | 0:297b00643040 | 3 | double Kp = 17.5; |
Marle | 0:297b00643040 | 4 | double Ki = 1.02; |
Marle | 0:297b00643040 | 5 | double Kd = 23.2; |
Marle | 0:297b00643040 | 6 | double Ts = 0.005f; |
Marle | 0:297b00643040 | 7 | // Sample time in seconds= tickertijd |
Marle | 0:297b00643040 | 8 | |
Marle | 0:297b00643040 | 9 | double PID_controller(double error) |
Marle | 0:297b00643040 | 10 | { |
Marle | 0:297b00643040 | 11 | static double error_integral = 0; |
Marle | 0:297b00643040 | 12 | static double error_prev = error; // initialization with this value only done once! |
Marle | 0:297b00643040 | 13 | static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); //to eliminate sensor noise to get accurate D-action calculation |
Marle | 0:297b00643040 | 14 | |
Marle | 0:297b00643040 | 15 | // Proportional part: |
Marle | 0:297b00643040 | 16 | double u_k = Kp * e; |
Marle | 0:297b00643040 | 17 | |
Marle | 0:297b00643040 | 18 | // Integral part: |
Marle | 0:297b00643040 | 19 | error_integral = error_integral + error * Ts; |
Marle | 0:297b00643040 | 20 | double u_i = Ki * error_integral; |
Marle | 0:297b00643040 | 21 | |
Marle | 0:297b00643040 | 22 | // Derivative part: |
Marle | 0:297b00643040 | 23 | double error_derivative = (error - error_prev)/Ts; |
Marle | 0:297b00643040 | 24 | double filtered_error_derivative = LowPassFilter.step(error_derivative); |
Marle | 0:297b00643040 | 25 | double u_d = Kd * filtered_error_derivative; error_prev = error; |
Marle | 0:297b00643040 | 26 | error_prev = error; |
Marle | 0:297b00643040 | 27 | |
Marle | 0:297b00643040 | 28 | // Sum all parts and return it |
Marle | 0:297b00643040 | 29 | return u_k + u_i + u_d; |