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 #ifndef PIDCONTROL_H
00036 #define PIDCONTROL_H
00037
00038 #include <math.h>
00039
00040 namespace dbw_mkz_twist_controller {
00041
00042 class PidControl {
00043 public:
00044 PidControl() {
00045 last_error_ = 0.0; int_val_ = 0.0; last_int_val_ = 0.0;
00046 kp_ = 0.0; ki_ = 0.0; kd_ = 0.0; min_ = -INFINITY; max_ = INFINITY;
00047 }
00048 PidControl(double kp, double ki, double kd, double min, double max) {
00049 last_error_ = 0.0; int_val_ = 0.0; last_int_val_ = 0.0;
00050 kp_ = kp; ki_ = ki; kd_ = kd; min_ = std::min(min,max); max_ = std::max(min,max);
00051 }
00052
00053 void setGains(double kp, double ki, double kd) { kp_ = kp; ki_ = ki; kd_ = kd; }
00054 void setRange(double min, double max) { min_ = std::min(min,max); max_ = std::max(min,max); }
00055 void setParams(double kp, double ki, double kd, double min, double max) { setGains(kp,ki,kd); setRange(min,max); }
00056 void resetIntegrator() { int_val_ = 0.0; last_int_val_ = 0.0; }
00057 void revertIntegrator() { int_val_ = last_int_val_; }
00058
00059 double step(double error, double sample_time) {
00060 last_int_val_ = int_val_;
00061
00062 double integral = int_val_ + error * sample_time;
00063 double derivative = (error - last_error_) / sample_time;
00064
00065 double y = kp_ * error + ki_ * int_val_ + kd_ * derivative;
00066 if (y > max_) {
00067 y = max_;
00068 } else if (y < min_) {
00069 y = min_;
00070 } else {
00071 int_val_ = integral;
00072 }
00073 last_error_ = error;
00074 return y;
00075 }
00076
00077 private:
00078 double last_error_;
00079 double int_val_, last_int_val_;
00080 double kp_, ki_, kd_;
00081 double min_, max_;
00082 };
00083
00084 }
00085
00086 #endif // PIDCONTROL_H
00087