PidController.cpp
Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include <cstdio>
00037 #include "generic/PidController.hpp"
00038
00039 namespace youbot {
00040
00041 PidController::PidController(double P, double I, double D, double I1, double I2) :
00042 p_gain_(P), i_gain_(I), d_gain_(D), i_max_(I1), i_min_(I2)
00043 {
00044 p_error_last_ = 0.0;
00045 p_error_ = 0.0;
00046 d_error_ = 0.0;
00047 i_error_ = 0.0;
00048 cmd_ = 0.0;
00049 last_i_error = 0.0;
00050 }
00051
00052 PidController::~PidController()
00053 {
00054 }
00055
00056 void PidController::initPid(double P, double I, double D, double I1, double I2)
00057 {
00058 p_gain_ = P;
00059 i_gain_ = I;
00060 d_gain_ = D;
00061 i_max_ = I1;
00062 i_min_ = I2;
00063
00064 reset();
00065 }
00066
00067 void PidController::reset()
00068 {
00069 p_error_last_ = 0.0;
00070 p_error_ = 0.0;
00071 d_error_ = 0.0;
00072 i_error_ = 0.0;
00073 cmd_ = 0.0;
00074 }
00075
00076 void PidController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
00077 {
00078 p = p_gain_;
00079 i = i_gain_;
00080 d = d_gain_;
00081 i_max = i_max_;
00082 i_min = i_min_;
00083 }
00084
00085 void PidController::setGains(double P, double I, double D, double I1, double I2)
00086 {
00087 p_gain_ = P;
00088 i_gain_ = I;
00089 d_gain_ = D;
00090 i_max_ = I1;
00091 i_min_ = I2;
00092 }
00093
00094
00095 double PidController::updatePid(double error, boost::posix_time::time_duration dt)
00096 {
00097 double p_term, d_term, i_term;
00098 p_error_ = error;
00099 double deltatime = (double)dt.total_microseconds()/1000.0;
00100
00101
00102 if (deltatime == 0.0 || isnan(error) || isinf(error))
00103 return 0.0;
00104
00105
00106 p_term = p_gain_ * p_error_;
00107
00108
00109
00110 i_error_ = last_i_error + deltatime * p_error_;
00111 last_i_error = deltatime * p_error_;
00112
00113
00114 i_term = i_gain_ * i_error_;
00115
00116
00117 if (i_term > i_max_)
00118 {
00119 i_term = i_max_;
00120 i_error_=i_term/i_gain_;
00121 }
00122 else if (i_term < i_min_)
00123 {
00124 i_term = i_min_;
00125 i_error_=i_term/i_gain_;
00126 }
00127
00128
00129 if (deltatime != 0)
00130 {
00131 d_error_ = (p_error_ - p_error_last_) / deltatime;
00132 p_error_last_ = p_error_;
00133 }
00134
00135 d_term = d_gain_ * d_error_;
00136 cmd_ = -p_term - i_term - d_term;
00137
00138
00139
00140 return cmd_;
00141 }
00142
00143
00144 double PidController::updatePid(double error, double error_dot, boost::posix_time::time_duration dt)
00145 {
00146 double p_term, d_term, i_term;
00147 p_error_ = error;
00148 d_error_ = error_dot;
00149 double deltatime = (double)dt.total_microseconds()/1000.0;
00150
00151 if (deltatime == 0.0 || isnan(error) || isinf(error) || isnan(error_dot) || isinf(error_dot))
00152 return 0.0;
00153
00154
00155
00156 p_term = p_gain_ * p_error_;
00157
00158
00159 i_error_ = last_i_error + deltatime * p_error_;
00160 last_i_error = deltatime * p_error_;
00161
00162
00163
00164
00165
00166 i_term = i_gain_ * i_error_;
00167
00168
00169 if (i_term > i_max_)
00170 {
00171 i_term = i_max_;
00172 i_error_=i_term/i_gain_;
00173 }
00174 else if (i_term < i_min_)
00175 {
00176 i_term = i_min_;
00177 i_error_=i_term/i_gain_;
00178 }
00179
00180
00181 d_term = d_gain_ * d_error_;
00182 cmd_ = -p_term - i_term - d_term;
00183
00184 return cmd_;
00185 }
00186
00187
00188
00189 void PidController::setCurrentCmd(double cmd)
00190 {
00191 cmd_ = cmd;
00192 }
00193
00194 double PidController::getCurrentCmd()
00195 {
00196 return cmd_;
00197 }
00198
00199 void PidController::getCurrentPIDErrors(double& pe, double& ie, double& de)
00200 {
00201 pe = p_error_;
00202 ie = i_error_;
00203 de = d_error_;
00204 }
00205
00206 }