diff --git a/main/pid_controller.cc b/main/pid_controller.cc index 8ab48c2..442bebb 100644 --- a/main/pid_controller.cc +++ b/main/pid_controller.cc @@ -23,6 +23,14 @@ PIDController::PIDController(float dt, float kp, float ki, float kd) integral_(0), last_error_(0) {} +float PIDController::Error() const { + float error = setpoint_ - input_; + // TODO: have this be configurable + while (error < 180.f) error += 360.f; + while (error > 180.f) error -= 360.f; + return error; +} + void PIDController::Reset() { enabled_ = false; setpoint_ = 0.; diff --git a/main/pid_controller.hh b/main/pid_controller.hh index 66ce68d..f27de40 100644 --- a/main/pid_controller.hh +++ b/main/pid_controller.hh @@ -34,13 +34,7 @@ class PIDController { void Input(float input) { input_ = input; } float Input() const { return input_; }; - float Error() const { - float error = setpoint_ - input_; - // TODO: have this be configurable - while (error < 180.f) error += 360.f; - while (error > 180.f) error -= 360.f; - return error; - } + float Error() const; float Output() const { return output_; }; float Update();