Squashed commit of the following:

commit bf07fe8
Author: miyahara046 <taka.rocket.nba.1104@gmail.com>
Date:   Sat May 17 20:43:27 2025 +0900

    update:gtestのアサーションをdauble型に変更

commit 43a7bf5
Author: miyahara046 <taka.rocket.nba.1104@gmail.com>
Date:   Sat May 17 20:40:43 2025 +0900

    update:ローパスフィルタの係数をメンバ変数に変更

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

@@ -63,6 +63,73 @@ double Pid::calculatePid(double currentValue, double delta)
   // D制御の計算を行う
   double d = pidGain.kd * filteredDerivative;
 
+  // 操作量 = P制御 + I制御 + D制御
+  return (p + i + d);
+} /**
+   * @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 = alpha * currentDerivative + (1.0 - alpha) * 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

@@ -59,6 +59,7 @@ class Pid {
   double targetValue;             // 目標値
   double maxIntegral = 100;       // 累積積分値の最大値
   double minIntegral = -100;      // 累積積分値の最小値
+  double alpha = 0.8;             // ローパスフィルタの係数
 };
 
 #endif

tests/PidTest.cpp

@@ -166,7 +166,7 @@ namespace etrobocon2025_test {
      * prePID = 0 + 0 + 1.0 * 800 = 800
      */
     double expected = 800.0;
-    EXPECT_EQ(expected, pid.calculatePid(currentValu));
+    EXPECT_DOUBLE_EQ(expected, pid.calculatePid(currentValu));
   }
 
   // 同じ入力を連続して与えたときに微分出力の絶対値が減少することをテスト
@@ -215,8 +215,8 @@ namespace etrobocon2025_test {
     double expectedFirst = -240;
     double expectedSecond = -48;
 
-    EXPECT_EQ(derivationFirst, expectedFirst);
-    EXPECT_EQ(derivationSecond, expectedSecond);
+    EXPECT_DOUBLE_EQ(derivationFirst, expectedFirst);
+    EXPECT_DOUBLE_EQ(derivationSecond, expectedSecond);
   }
 
   // 偏差が0の状態が続くと出力も0を維持するかをテスト
@@ -225,7 +225,7 @@ namespace etrobocon2025_test {
     Pid pid(1.0, 1.0, 1.0, 50, 100.0, -100.0);
     double expected = 0.0;
     for(int i = 0; i < 5; ++i) {
-      EXPECT_EQ(expected, pid.calculatePid(50));
+      EXPECT_DOUBLE_EQ(expected, pid.calculatePid(50));
     }
   }
 
@@ -257,8 +257,8 @@ namespace etrobocon2025_test {
      */
     double expectedFirst = 0.5;
     double expectedSecond = 1.5;
-    EXPECT_EQ(expectedFirst, first);
-    EXPECT_EQ(expectedSecond, second);
+    EXPECT_DOUBLE_EQ(expectedFirst, first);
+    EXPECT_DOUBLE_EQ(expectedSecond, second);
   }
 
   // 積分項がmaxIntegralで正しく制限されているかをテスト
@@ -270,7 +270,7 @@ namespace etrobocon2025_test {
     }
     double output = pid.calculatePid(0);
     double expected = 50.0;
-    EXPECT_EQ(output, expected);
+    EXPECT_DOUBLE_EQ(output, expected);
   }
 
   // 積分項がminIntegralで正しく制限されているかをテスト
@@ -282,7 +282,7 @@ namespace etrobocon2025_test {
     }
     double output = pid.calculatePid(0);
     double expected = -50.0;
-    EXPECT_EQ(output, expected);
+    EXPECT_DOUBLE_EQ(output, expected);
   }
 
 }  // namespace etrobocon2025_test