pid.cpp

00001 #include "pid.h"
00002 
00003 PID::PID() {
00004     Kp = 0;
00005     Ki = 0;
00006     Kd = 0;
00007     error_integral = 0;
00008     error_previous = 0;
00009     pid_period = 0;
00010     first_update = true;
00011     
00012     // Setup the low pass filter.
00013     // The values are from the slides (PES, lecture 5)
00014     B[0] = 0.0640; // b0
00015     B[1] = 0.1279; // b1
00016     B[2] = 0.0640; // b2
00017     // a0 = 1
00018     A[0] = -1.1683; // a1
00019     A[1] = 0.4241; // a2
00020 }
00021 
00022 void PID::set_period(double period) {
00023     pid_period = period;
00024 }
00025 
00026 void PID::set_k_values(double Kp, double Ki, double Kd) {
00027     this->Kp = Kp;
00028     this->Ki = Ki;
00029     this->Kd = Kd;
00030 }
00031 
00032 void PID::clear_state() {
00033     first_update = true;
00034     error_previous = 0;
00035     error_integral = 0;
00036     
00037     // Cleart the low pass filter.
00038     wz[0] = 0;
00039     wz[1] = 0;
00040 }
00041 
00042 double PID::update(double error) {
00043     if (first_update) {
00044         // Make sure the previous error has a sensible value (not 0).
00045         error_previous = error;
00046         first_update = false;
00047     }
00048     
00049     // Proportional part
00050     double u_p = Kp * error;
00051     
00052     // Integral part
00053     error_integral += error * pid_period;
00054     double u_i = Ki * error_integral;
00055     
00056     // Derivative part (needs filter)
00057     double error_der = (error-error_previous)/pid_period;
00058     double filtered_error_der = biquad_step(error_der);
00059     double u_d = Kd * filtered_error_der;
00060     error_previous = error;
00061     
00062     // Add all components and return.
00063     return u_p + u_d + u_i;
00064 }
00065 
00066 double PID::biquad_step(double x) {
00067 
00068     double y,w;
00069 
00070     /* Direct form II */
00071     w =      x - A[0]*wz[0] - A[1]*wz[1];
00072     y = B[0]*w + B[1]*wz[0] + B[2]*wz[1];
00073 
00074     /* Shift */
00075     wz[1] = wz[0];
00076     wz[0] = w;
00077 
00078     return y;
00079 }