hidaka sato / PIDController

PID Controller

pid_controller.hpp@0:fca62f1974fa, 2021-08-23 (annotated)

Committer:
sgrsn
Date:
Mon Aug 23 17:02:09 2021 +0000
Revision:
0:fca62f1974fa
one

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sgrsn 0:fca62f1974fa 1 #ifndef PID_CONTROLLER_HPP
sgrsn 0:fca62f1974fa 2 #define PID_CONTROLLER_HPP
sgrsn 0:fca62f1974fa 3
sgrsn 0:fca62f1974fa 4 #include <vector>
sgrsn 0:fca62f1974fa 5
sgrsn 0:fca62f1974fa 6 class PIDControl
sgrsn 0:fca62f1974fa 7 {
sgrsn 0:fca62f1974fa 8 public:
sgrsn 0:fca62f1974fa 9 struct Bound
sgrsn 0:fca62f1974fa 10 {
sgrsn 0:fca62f1974fa 11 double upper;
sgrsn 0:fca62f1974fa 12 double lower;
sgrsn 0:fca62f1974fa 13 };
sgrsn 0:fca62f1974fa 14 struct Parameter
sgrsn 0:fca62f1974fa 15 {
sgrsn 0:fca62f1974fa 16 double kp;
sgrsn 0:fca62f1974fa 17 double ki;
sgrsn 0:fca62f1974fa 18 double kd;
sgrsn 0:fca62f1974fa 19 Bound bound;
sgrsn 0:fca62f1974fa 20 Bound threshold;
sgrsn 0:fca62f1974fa 21 };
sgrsn 0:fca62f1974fa 22
sgrsn 0:fca62f1974fa 23 PIDControl(const Parameter& param) : param_(param)
sgrsn 0:fca62f1974fa 24 {
sgrsn 0:fca62f1974fa 25 err_.resize(3,0);
sgrsn 0:fca62f1974fa 26 control_input_.resize(2,0);
sgrsn 0:fca62f1974fa 27 };
sgrsn 0:fca62f1974fa 28 ~PIDControl() = default;
sgrsn 0:fca62f1974fa 29
sgrsn 0:fca62f1974fa 30 const double& calculate(const double& ref, const double& cur);
sgrsn 0:fca62f1974fa 31
sgrsn 0:fca62f1974fa 32 const Parameter& parameter(void) const { return param_; };
sgrsn 0:fca62f1974fa 33 void parameter(const Parameter& param) { param_ = param; };
sgrsn 0:fca62f1974fa 34
sgrsn 0:fca62f1974fa 35 double calculate(double ref, double cur, double time_step)
sgrsn 0:fca62f1974fa 36 {
sgrsn 0:fca62f1974fa 37 err_[2] = ( (ref - cur)-err_[0] ) / time_step;
sgrsn 0:fca62f1974fa 38 err_[0] = ref - cur;
sgrsn 0:fca62f1974fa 39
sgrsn 0:fca62f1974fa 40 // calculate control input
sgrsn 0:fca62f1974fa 41 control_input_[0] = param_.kp*err_[0] + param_.ki*err_[1] + param_.kd*err_[2];
sgrsn 0:fca62f1974fa 42
sgrsn 0:fca62f1974fa 43 // limit of control input
sgrsn 0:fca62f1974fa 44 control_input_[0] = min(control_input_[0], param_.bound.upper);
sgrsn 0:fca62f1974fa 45 control_input_[0] = max(control_input_[0], param_.bound.lower);
sgrsn 0:fca62f1974fa 46
sgrsn 0:fca62f1974fa 47 // update
sgrsn 0:fca62f1974fa 48 control_input_[1] = control_input_[0];
sgrsn 0:fca62f1974fa 49 err_[1] += err_[0] * time_step;
sgrsn 0:fca62f1974fa 50
sgrsn 0:fca62f1974fa 51 // limit of i gain
sgrsn 0:fca62f1974fa 52 err_[1] = min(err_[1], param_.bound.upper);
sgrsn 0:fca62f1974fa 53 err_[1] = max(err_[1], param_.bound.lower);
sgrsn 0:fca62f1974fa 54
sgrsn 0:fca62f1974fa 55 if( (ref - cur < param_.threshold.upper) && (ref - cur > param_.threshold.lower) )
sgrsn 0:fca62f1974fa 56 return 0;
sgrsn 0:fca62f1974fa 57
sgrsn 0:fca62f1974fa 58 return control_input_[0];
sgrsn 0:fca62f1974fa 59 }
sgrsn 0:fca62f1974fa 60
sgrsn 0:fca62f1974fa 61 private:
sgrsn 0:fca62f1974fa 62 Parameter param_;
sgrsn 0:fca62f1974fa 63
sgrsn 0:fca62f1974fa 64 std::vector<double> err_; //[current_error, error_integral, error_diff]
sgrsn 0:fca62f1974fa 65 std::vector<double> control_input_;
sgrsn 0:fca62f1974fa 66 };
sgrsn 0:fca62f1974fa 67
sgrsn 0:fca62f1974fa 68 #endif