Merge pull request #3 from KatLab-MiyazakiUniv/ticket-KL25-20

#KL25-20 PID計算

Author: takuchi17

CMakeLists.txt

@@ -32,6 +32,7 @@ set(PROJECT_TEST_DIR ${PROJECT_SOURCE_DIR}/tests)
 # 必要なインクルードディレクトリの設定 (適宜登録)
 set(INCLUDE_DIRS
   ${PROJECT_MODULE_DIR}
+  ${PROJECT_MODULE_DIR}/calculators
   ${PROJECT_MODULE_DIR}/API
   ${PROJECT_TEST_DIR}
   ${PROJECT_TEST_DIR}/dummy
@@ -41,6 +42,7 @@ set(INCLUDE_DIRS
 # ソースファイルの登録 (適宜登録)
 file(GLOB_RECURSE PROJECT_SOURCES
   "${PROJECT_MODULE_DIR}/*.cpp"
+  "${PROJECT_MODULE_DIR}/calculators/*.cpp"
   "${PROJECT_TEST_DIR}/helpers/*.cpp"
 )
 

Makefile.inc

@@ -2,7 +2,6 @@ USE_RASPIKE_ART=1
 
 mkfile_path := $(dir $(lastword $(MAKEFILE_LIST)))
 
-
 # Shellスクリプトで、moduleディレクトリ中のソースコード名をオブジェクトファイル名に変換している
 SRCS := $(shell find ${mkfile_path}modules -name '*.cpp')
 OBJS := $(notdir $(SRCS:.cpp=.o))
@@ -14,10 +13,10 @@ APPL_LIBS += -lm
 
 APPL_DIRS += \
 	$(mkfile_path)modules\
-	$(mkfile_path)modules/API
-
-
+	$(mkfile_path)modules/API\
+	$(mkfile_path)modules/calculators
 
 INCLUDES += \
 	-I$(mkfile_path)modules\
-	-I$(mkfile_path)modules/API
+	-I$(mkfile_path)modules/API\
+	-I$(mkfile_path)modules/calculators

modules/calculators/Pid.cpp

@@ -0,0 +1,73 @@
+/**
+ * @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),
+    targetValue(_targetValue),
+    maxIntegral(_maxIntegral),
+    minIntegral(_minIntegral)
+{
+}
+
+Pid::Pid(double _kp, double _ki, double _kd, double _targetValue)
+  : Pid(_kp, _ki, _kd, _targetValue, 100.0, -100.0)
+{
+}
+
+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除算を避けるために delta=0 の場合はデフォルト周期0.01とする
+  if(delta == 0) delta = 0.01;
+
+  // 現在の目標値との偏差を求める
+  double currentDeviation = targetValue - currentValue;
+  // 積分の処理を行う
+  integral += (currentDeviation + prevDeviation) * delta / 2.0;
+  // 累積する積分値の大きさ制限
+  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/calculators/Pid.h

@@ -0,0 +1,73 @@
+/**
+ * @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);
+
+  /** 積分値制限を設定しない場合のコンストラクタ
+   * @param _kp Pゲイン
+   * @param _ki Iゲイン
+   * @param _kd Dゲイン
+   * @param _targetValue 目標値
+   */
+  Pid(double _kp, double _ki, double _kd, double _targetValue);
+
+  /**
+   * @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 = 0.0;           // 前回の偏差
+  double integral = 0.0;                // 偏差の累積
+  double filteredDerivative = 0.0;      // フィルタされた微分項を保持する変数
+  double targetValue;                   // 目標値
+  double maxIntegral = 100.0;           // 累積積分値の最大値
+  double minIntegral = -100.0;          // 累積積分値の最小値
+  static constexpr double alpha = 0.8;  // ローパスフィルタの係数
+};
+
+#endif  // PID_H