PID motor controll for the biorobotics project.
Dependents: PID_example Motor_calibration Demo_mode Demo_mode ... more
Diff: pid.cpp
- Revision:
- 0:009e84d7af32
- Child:
- 3:f1067b5bb5af
diff -r 000000000000 -r 009e84d7af32 pid.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/pid.cpp Fri Oct 19 10:57:39 2018 +0000 @@ -0,0 +1,49 @@ +#include "pid.h" + +PID::PID() { + Kp = 0; + Ki = 0; + Kd = 0; + error_integral = 0; + error_previous = 0; + pid_period = 0; + first_update = true; + + // Setup the low pass filter. + // The values are from the slides (PES, lecture 5) + low_pass_filter = BiQuad(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); +} + +void PID::set_period(double period) { + pid_period = period; +} + +void PID::set_k_values(double Kp, double Ki, double Kd) { + this->Kp = Kp; + this->Ki = Ki; + this->Kd = Kd; +} + +double PID::update(double error) { + if (first_update) { + // Make sure the previous error has a sensible value (not 0). + error_previous = error; + first_update = false; + } + + // Proportional part + double u_p = Kp * error; + + // Integral part + error_integral += error * pid_period; + double u_i = Ki * error_integral; + + // Derivative part + double error_der = (error-error_previous)/pid_period; + double filtered_error_der = low_pass_filter.step(error_der); + double u_d = Kd * filtered_error_der; + error_previous = error; + + // Add all components and return. + return u_p + u_d + u_i; +} \ No newline at end of file