uas-ugv/main/pid_controller.hh

#pragma once

#include <math.h>

class PIDController {
 public:
  explicit PIDController(float dt, float kp = 0., float ki = 0., float kd = 0.);

  void DeltaT(float dt) {
    dt_ = dt;
    Reset();
  }
  float DeltaT() { return dt_; }

  float GetP() { return kp_; }
  float GetI() { return ki_; }
  float GetD() { return kd_; }

  void SetPID(float kp, float ki, float kd) {
    kp_ = kp;
    ki_ = ki;
    kd_ = kd;
  }

  void  MaxOutput(float max_output) { max_output_ = max_output; }
  float MaxOutput() const { return max_output_; }

  void  MaxIError(float max_i_error) { max_i_error_ = max_i_error; }
  float MaxIError() const { return max_i_error_; }

  void  Setpoint(float setpoint) { setpoint_ = setpoint; }
  float Setpoint() const { return setpoint_; }

  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 Output() const { return output_; };

  float Update();
  float Update(float input);
  float Update(float input, float setpoint);

  void Reset();
  void Enable(bool enable = true) { enabled_ = enable; }
  void Disable() { Enable(false); }
  bool Enabled() const { return enabled_; }

 private:
  static float clamp_mag(float x, float mag);

  float dt_;
  float kp_;
  float ki_;
  float kd_;
  float max_output_;
  float max_i_error_;

  bool  enabled_;
  float setpoint_;
  float input_;
  float output_;

  float integral_;
  float last_error_;
};