C++のヘッダオンリーライブラリ 不完全微分を使用した速度型PID制御器

main.cpp

#include <chrono>
#include <iostream>

#include "VelPid.hpp"

using namespace std::literals;

// Kp = 0.5, Ki = 0.2, Kd = 0.05
// min  = -8, max = 8
VelPid pid{{{0.5, 0.2, 0.05}, -8, 8}};

int main() {
  // target = 10, actual = 0, dt = 1[s]
  std::cout << pid.calc(10, 0, 1s) << std::endl;
  std::cout << pid.calc(10, 1, 1s) << std::endl;
  std::cout << pid.calc(10, 3, 1s) << std::endl;
  std::cout << pid.calc(10, 6, 1s) << std::endl;
  std::cout << pid.calc(10, 12, 1s) << std::endl;
  std::cout << pid.calc(10, 13, 1s) << std::endl;
}

VelPid.hpp

#ifndef VEL_PID_HPP
#define VEL_PID_HPP
/// @file
/// @brief Provides the VelPid class for velocity PID control.
/// @copyright Copyright (c) 2024 Yoshikawa Teru
/// @license This project is released under the MIT License, see [LICENSE](https://github.com/teruyamato0731/Chassis/blob/main/LICENSE).

#include <algorithm>
#include <chrono>
#include <cmath>

/// @brief Gains for a PID controller.
struct PidGain {
  float kp;
  float ki;
  float kd;
};

/// @brief Gains and limits for a PID controller.
struct PidParam {
  PidGain gain;
  float min = NAN;
  float max = NAN;
};

/// @brief Velocity Form PID controller.
template<class T = float>
class VelPid {
 public:
  /// @brief Constructor with the specified gains and limits.
  /// @param param The parameters for the PID controller.
  VelPid(const PidParam& param) : param_{param} {}

  /// @brief Calculates the output of the PID controller.
  T calc(const T target, const T actual, const std::chrono::duration<float>& dt) {
    return calc(target - actual, dt);
  }

  /// @brief Calculates the output of the PID controller.
  T calc(const T error, const std::chrono::duration<float>& dt) {
    using namespace std;
    const auto sec = dt.count();
    const auto prop = (error - pre_error_) / sec;
    const auto deriv = isnan(pre_prop_) ? 0 : (prop - pre_prop_) / sec;
    pre_error_ = error;
    pre_prop_ = prop;
    low_pass_deriv_ += (deriv - low_pass_deriv_) / 8;
    const auto du = param_.gain.kp * prop + param_.gain.ki * error + param_.gain.kd * low_pass_deriv_;
    output_ = clamp(output_ + du * sec, param_.min, param_.max);
    return output_;
  }

  /// @brief Resets the PID controller.
  void reset() {
    pre_error_ = 0.0;
    pre_prop_ = NAN;
    low_pass_deriv_ = 0.0;
    output_ = 0.0;
  }

  /// @brief Sets the parameters for the PID controller.
  void set_param(const PidParam& param) {
    param_ = param;
    reset();
  }

  /// @brief Sets the gains for the PID controller.
  void set_gain(const PidGain& gain) {
    param_.gain = gain;
    reset();
  }

  /// @brief Sets the limits for the PID controller.
  void set_limit(const float min, const float max) {
    param_.min = min;
    param_.max = max;
  }

 private:
  PidParam param_;
  T pre_error_ = 0.0;
  T pre_prop_ = NAN;
  T low_pass_deriv_ = 0.0;
  T output_ = 0.0;
};

#endif  // VEL_PID_HPP