Marleen van Hoorn / Mbed 2 deprecated PID_controller

tralalal

Dependencies:   mbed

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?

UserRevisionLine numberNew 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;