pid.h

/*
 * Classic P/PI/PID control
 */

#pragma once

#include <limits>
#include <algorithm>

#include "control/filter/biquad.h"
#include "control/system/type.h"

namespace control::classic {

template<typename T>
constexpr T max() {
  return std::numeric_limits<T>::has_infinity
         ? std::numeric_limits<T>::infinity() : std::numeric_limits<T>::max();
}

template<typename T>
class AbstractController : public system::SISO<T> {
  static_assert(std::numeric_limits<T>::is_signed, "Signed type required");
 public:
  explicit AbstractController(T Limit_ = max<T>()) : Limit(Limit_) {};

  virtual ~AbstractController() = default;

  // Limit
  T Limit;

  // Clipping status
  bool clipping = false;

  T step(T e) final {
    T u;

    // Get the control effort
    u = control(e);

    // Clip the output
    u = clip(u);

    // Return the output
    return u;
  };

  void setLimit(T limit) {
    Limit = limit;
  }

  virtual void reset() {};

 protected:

  T clip(T u) {
    if (Limit == std::numeric_limits<T>::infinity()) {
      return u;
    }

    // Determine if the signal is in range
    if (!clipping && (u > Limit || u < -Limit)) {
      clipping = true;
    } else if (clipping && (u >= -Limit && u <= Limit)) {
      clipping = false;
    }

    // If limiting, then clip to min / max
    if (clipping) {
      u = std::max(-Limit, std::min(Limit, u));
    }

    return u;
  }

  virtual T control(T e) = 0;

};

template<typename T>
class P : public AbstractController<T> {
 public:

  explicit P(T Kp_ = 1.0, T Limit_ = max<T>()) : AbstractController<T>(Limit_), Kp(Kp_) {};

  ~P() = default;;

  // Proportional gain
  const T Kp;

 protected:
  T control(T e) {
    // The output of the controller
    T u;

    // Proportional gain
    u = Kp * e;

    return u;
  };
};

template<typename T>
class PID : public AbstractController<T> {
 public:

  explicit PID(T Ts = 1.0, T Kp = 1.0, T Ti = max<T>(), T Td = 0.0, T N = max<T>(), T Limit = max<T>())
      : AbstractController<T>(Limit), B(
      (Kp * (4 * Td / N + 2 * Td * Ts / Ti / N + Ts * Ts / Ti + 4 * Td + 2 * Ts)) / (4 * Td / N + 2 * Ts),
      -(Kp * (-Ts * Ts / Ti + 4 * Td / N + 4 * Td)) / (2 * Td / N + Ts),
      (Kp * (4 * Td / N - 2 * Td * Ts / Ti / N + Ts * Ts / Ti + 4 * Td - 2 * Ts)) / (4 * Td / N + 2 * Ts),
      -(4 * Td / N) / (2 * Td / N + Ts),
      (2 * Td / N - Ts) / (2 * Td / N + Ts)
  ) {};

  filter::TCS<T> poles() {
    return B.poles();
  }

  void reset() {
    B.reset();
  }

 protected:

  filter::Biquad<T> B;

  T control(T e) {
    return B.step(e);
  }

};

template<typename T>
class PI : public PID<T> {
 public:

  explicit PI(T Ts_ = 1, T Kp_ = 1, T Ti_ = max<T>(), T Limit_ = max<T>()) : PID<T>(Ts_, Kp_, Ti_, 0, max<T>(),
                                                                           Limit_) {};
};

template<typename T>
class PD : public PID<T> {
 public:

  explicit PD(T Ts_ = 1, T Kp_ = 1, T Td_ = 0, T N_ = max<T>(), T Limit_ = max<T>()) : PID<T>(Ts_, Kp_, max<T>(), Td_,
                                                                                     N_, Limit_) {};
};

}