Pid.h

Go to the documentation of this file.

#ifndef CHASSIS_PID_H_
#define CHASSIS_PID_H_
#include <chrono>
#include <type_traits>
 
namespace rct {
 
 
 
struct PidGain {
  float kp;  
  float ki;  
  float kd;  
};
 
template<class T>
class is_pidable {
  template<class U, class = U>
  struct is_pidable_helper : std::false_type {};
  template<class U>
  struct is_pidable_helper<U, decltype(std::declval<U&>() = std::declval<U>() + std::declval<U>(),
                                       std::declval<U&>() = std::declval<U>() - std::declval<U>(),
                                       std::declval<U&>() = std::declval<U>() * std::declval<float>(),
                                       std::declval<U&>() = std::declval<U>() / std::declval<float>(), U{})>
      : std::true_type {};
 public:
  static constexpr bool value = is_pidable_helper<T>::value;
};
 
template<class T>
constexpr bool is_pidable_v = is_pidable<T>::value;
 
template<class T>
struct Pid {
  static_assert(is_pidable_v<T>, "template parameter T must be PID-able type");
 
  Pid(const PidGain& pid_gain, const T& init = T{}) noexcept(noexcept(T{})) : pid_gain_{pid_gain}, pre_{init} {}
 
  T calc(const T& dst, const T& now, const std::chrono::microseconds& delta_time) {
    const float sec = std::chrono::duration<float>{delta_time}.count();
    const T proportional = dst - now;
    integral_ += proportional * sec;
    const T differential = (proportional - pre_) / sec;
    pre_ = proportional;
    return proportional * pid_gain_.kp + integral_ * pid_gain_.ki + differential * pid_gain_.kd;
  }
 
  void refresh(const T& init = T{}) noexcept(noexcept(T{})) {
    integral_ = T{};
    pre_ = init;
  }
 
  void set_pid_gain(const PidGain& pid_gain) noexcept(noexcept(refresh())) {
    pid_gain_ = pid_gain;
    refresh();
  }
 
 private:
  PidGain pid_gain_;
  T integral_{};
  T pre_;
};
 
 
 
}  // namespace rct
 
#endif  // CHASSIS_PID_H_