Squashed commit of the following:

commit 7a3a536
Author: miyahara046 <taka.rocket.nba.1104@gmail.com>
Date:   Sat May 17 15:14:16 2025 +0900

    add:Pid.cppのテストをするファイルの追加

commit 84558db
Author: miyahara046 <taka.rocket.nba.1104@gmail.com>
Date:   Sat May 17 15:12:15 2025 +0900

    add:Pidを計算するファイルの追加

Author: miyahara046

modules/Pid.cpp

@@ -0,0 +1,68 @@
+/**
+ * @file Pid.cpp
+ * @brief PIDを計算するクラス
+ * @author miyahara046
+ */
+
+#include "Pid.h"
+PidGain::PidGain(double _kp, double _ki, double _kd)
+  // pidゲインが負の値にならないようにする
+  : kp(_kp < 0 ? 0 : _kp), ki(_ki < 0 ? 0 : _ki), kd(_kd < 0 ? 0 : _kd)
+{
+}
+
+Pid::Pid(double _kp, double _ki, double _kd, double _targetValue, double _maxIntegral,
+         double _minIntegral)
+  : pidGain(_kp, _ki, _kd),
+    prevDeviation(0.0),
+    integral(0.0),
+    targetValue(_targetValue),
+    maxIntegral(_maxIntegral),
+    minIntegral(_minIntegral)
+{
+}
+
+void Pid::setPidGain(double _kp, double _ki, double _kd)
+{
+  // pidゲインが負の値にならないようにする
+  pidGain.kp = _kp < 0 ? 0 : _kp;
+  pidGain.ki = _ki < 0 ? 0 : _ki;
+  pidGain.kd = _kd < 0 ? 0 : _kd;
+}
+
+double Pid::calculatePid(double currentValue, double delta)
+{
+  // 0除算を避けるために0の場合はデフォルト周期0.01とする
+  if(delta == 0) delta = 0.01;
+
+  // 現在の目標値との偏差を求める
+  double currentDeviation = targetValue - currentValue;
+  // 積分の処理を行う
+  integral += (currentDeviation + prevDeviation) * delta / 2;
+  // 累積する積分値の大きさ制限
+  if(integral > maxIntegral)
+    integral = maxIntegral;
+  else if(integral < minIntegral)
+    integral = minIntegral;
+  // 微分の処理を行う
+  double currentDerivative = (currentDeviation - prevDeviation) / delta;
+  /**
+   * 微分項にローパスフィルタを適用
+   * 偏差が大きい際に過大な変化量を一気に与えず
+   * 滑らかな変化にし、機体の暴走を防ぐため
+   */
+  filteredDerivative = 0.8 * currentDerivative + (0.2) * filteredDerivative;
+
+  // 前回の偏差を更新する
+  prevDeviation = currentDeviation;
+
+  // P制御の計算を行う
+  double p = pidGain.kp * currentDeviation;
+  // I制御の計算を行う
+  double i = pidGain.ki * integral;
+  // D制御の計算を行う
+  double d = pidGain.kd * filteredDerivative;
+
+  // 操作量 = P制御 + I制御 + D制御
+  return (p + i + d);
+}

modules/Pid.h

@@ -0,0 +1,64 @@
+/**
+ * @file Pid.h
+ * @brief PIDを計算するクラス
+ * @author miyahara046
+ */
+
+#ifndef PID_H
+#define PID_H
+
+//   PIDゲインを保持する構造体
+struct PidGain {
+ public:
+  double kp;  // Pゲイン
+  double ki;  // Iゲイン
+  double kd;  // Dゲイン
+
+  /** コンストラクタ
+   * @param _kp Pゲイン
+   * @param _ki Iゲイン
+   * @param _kd Dゲイン
+   */
+  PidGain(double _kp, double _ki, double _kd);
+};
+
+class Pid {
+ public:
+  /** コンストラクタ
+   * @param _kp Pゲイン
+   * @param _ki Iゲイン
+   * @param _kd Dゲイン
+   * @param _targetValue 目標値
+   * @param _maxIntegral 累積積分値の最大値
+   * @param _minIntegral 累積積分値の最小値
+   */
+  Pid(double _kp, double _ki, double _kd, double _targetValue, double _maxIntegral,
+      double _minIntegral);
+
+  /**
+   * @brief PIDゲインを設定する
+   * @param _kp Pゲイン
+   * @param _ki Iゲイン
+   * @param _kd Dゲイン
+   */
+  void setPidGain(double _kp, double _ki, double _kd);
+
+  /**
+   * @brief PIDを計算する
+   * @param currentValue 現在値
+   * @param delta 周期[ms](デフォルト値0.01[10ms]、省略可)
+   * @return PIDの計算結果(操作量)
+   */
+  double calculatePid(double currentValue, double delta = 0.01);
+
+ private:
+  PidGain pidGain;
+  double prevDeviation;           // 前回の偏差
+  double integral;                // 偏差の累積
+  double filteredDerivative = 0;  // フィルタされた微分項を保持する変数
+  double targetValue;             // 目標値
+  double maxIntegral = 100;       // 累積積分値の最大値
+  double minIntegral = -100;      // 累積積分値の最小値
+};
+
+#endif