SpeedController.cpp

00001 #include "SpeedController.h"
00002 
00003 
00004 PIDController::PIDController(float kp, float ki, float kd, float max_windup,
00005                              float start_time, float umin, float umax)
00006 {
00007 //    The PID controller can be initalized with a specific kp value
00008 //    ki value, and kd value
00009     this->kp = kp;
00010     this->ki = ki;
00011     this->kd = kd;
00012 
00013     this->max_windup = max_windup;
00014 
00015     this->umin = umin;
00016     this->umax = umax;
00017 
00018     //Store relevant data
00019     this->m_last_timestamp = 0.0;
00020     this->m_set_point = 0.0;
00021     this->m_start_time = start_time;
00022     this->m_error_sum = 0.0;
00023     this->m_last_error = 0.0;
00024 }
00025 
00026 
00027 
00028 float PIDController::update(float measured_value, float timestamp)
00029 {
00030     // TODO
00031     // Fill in the blank
00032     
00033     float delta_time = timestamp - m_last_timestamp;
00034     float error = m_set_point - measured_value;
00035     
00036     m_last_timestamp = timestamp;
00037     
00038     
00039     m_error_sum += error*delta_time;
00040     
00041     float delta_error = error - m_last_error;
00042     
00043     if(delta_error > 10.0) {
00044         delta_error = 0.0;
00045     }
00046     
00047     m_last_error = error;
00048 
00049     if(m_error_sum > max_windup) {
00050         m_error_sum = max_windup;
00051     } else if(m_error_sum < -1.0*max_windup) {
00052         m_error_sum = max_windup*-1.0;
00053     }
00054 
00055     float p = kp*error;
00056     float i = ki*m_error_sum;
00057     float d = kd*(delta_error/delta_time);
00058 
00059     float u=p+i+d;
00060 
00061     if(u > umax) {
00062         u = umax;
00063     } else if(u < umin) {
00064         u = umin;
00065     }
00066 
00067     return u;
00068 }
00069 
00070 void PIDController::setTarget(float target)
00071 {
00072     this->m_set_point = target;
00073 }
00074 
00075 void PIDController::setKp(float kp)
00076 {
00077     this->kp = kp;
00078 }
00079 
00080 void PIDController::setKi(float ki)
00081 {
00082     this->ki = ki;
00083 }
00084 
00085 void PIDController::setKd(float kd)
00086 {
00087     this->kd = kd;
00088 }
00089 
00090 void PIDController::setMaxWindup(float max_windup)
00091 {
00092     this->max_windup = max_windup;
00093 }
00094 
00095 float PIDController::getLastTimeStamp()
00096 {
00097     return this->m_last_timestamp;
00098 }