|
|
|
@ -23,6 +23,14 @@ PIDController::PIDController(float dt, float kp, float ki, float kd)
@@ -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.; |
|
|
|
|