feat: 速度制御対応 #165 (#236)

* add: 速度制御用のファイルを追加 #165

* update: 辞書ファイルを更新 #165

* fix: Arduino環境のMbedボードでビルドに失敗する問題を修正した #165

* add:テスト追加とバグの修正 #165

* fix: 指摘事項の修正 #165

* fix: include修正

* fix: Errorクラスの変更対応 #165

* fix: フォーマットエラーの修正 #165

* fix: ヘッダーファイルの記述漏れを修正 #165

---------

Co-authored-by: yutotnh <57719497+yutotnh@users.noreply.github.com>

.cspell.json

@@ -34,6 +34,7 @@
         "mattnotmitt",
         "Mbed",
         "mdled",
+        "INTERRUPTIN",
         "noninteractive",
         "Nucleo",
         "pico",

CMakeLists.txt

@@ -40,6 +40,7 @@ target_sources(spirit
     ./src/spirit/src/mutex.cpp
     ./src/spirit/src/PwmDataConverter.cpp
     ./src/spirit/src/SpeedDataConverter.cpp
+    ./src/spirit/src/SpeedController.cpp
 )
 
 if(CMAKE_PROJECT_NAME STREQUAL PROJECT_NAME)
@@ -67,6 +68,7 @@ if(DEFINED MBED_PATH)
     target_sources(spirit-platform-mbed
         PUBLIC
         ./src/spirit/platform/mbed/source/DigitalOut.cpp
+        ./src/spirit/platform/mbed/source/InterruptIn.cpp
         ./src/spirit/platform/mbed/source/PwmOut.cpp
     )
 

src/spirit.h

@@ -7,11 +7,13 @@
 #include "spirit/include/FakeUdpConverter.h"
 #include "spirit/include/Id.h"
 #include "spirit/include/InterfaceDigitalOut.h"
+#include "spirit/include/InterfaceInterruptIn.h"
 #include "spirit/include/InterfacePwmOut.h"
 #include "spirit/include/MdLed.h"
 #include "spirit/include/Motor.h"
 #include "spirit/include/MotorDataConverter.h"
 #include "spirit/include/PwmDataConverter.h"
+#include "spirit/include/SpeedController.h"
 #include "spirit/include/SpeedDataConverter.h"
 #include "spirit/include/adjust_duty_cycle.h"
 #include "spirit/include/bfloat16.h"
@@ -21,6 +23,7 @@
 
 #ifdef __MBED__
 #include "spirit/platform/mbed/include/DigitalOut.h"
+#include "spirit/platform/mbed/include/InterruptIn.h"
 #include "spirit/platform/mbed/include/PwmOut.h"
 #endif  // MBED_H
 

src/spirit/include/InterfaceInterruptIn.h

@@ -0,0 +1,68 @@
+#ifndef SPIRIT_INTERFACE_INTERRUPTIN_H
+#define SPIRIT_INTERFACE_INTERRUPTIN_H
+
+#include <cstdint>
+
+#if defined(__MBED__) && !defined(ARDUINO)
+#include "mbed.h"
+#elif defined(ARDUINO)
+// TODO: Arduinoの場合に必要なヘッダファイルを追加する
+#else
+#include <functional>
+#endif
+
+namespace spirit {
+
+/**
+ * @brief mbedのInterruptInクラスを模した基底クラス
+ */
+class InterfaceInterruptIn {
+public:
+    /**
+     * @brief デストラクタ
+     */
+    virtual ~InterfaceInterruptIn() = default;
+
+#if defined(__MBED__) && !defined(ARDUINO)
+    /**
+     * @brief 立ち上がり時に呼び出される関数を設定する
+     * @param func 立ち上がり時に呼び出される関数
+     */
+    virtual void rise(Callback<void()> func) = 0;
+#elif defined(ARDUINO)
+    // TODO: Arduinoの場合の実装を追加する
+#else
+    /**
+     * @brief 立ち上がり時に呼び出される関数を設定する
+     * @param func_rise 立ち上がり時に呼び出される関数
+     */
+    virtual void rise(std::function<void(void)>& func) = 0;
+#endif
+
+#if defined(__MBED__) && !defined(ARDUINO)
+    /**
+     * @brief 立ち下がり時に呼び出される関数を設定する
+     * @param func 立ち下がり時に呼び出される関数
+     */
+    virtual void fall(Callback<void()> func) = 0;
+#elif defined(ARDUINO)
+    // TODO: Arduinoの場合の実装を追加する
+#else
+    /**
+     * @brief 立ち下がり時に呼び出される関数を設定する
+     * @param func 立ち下がり時に呼び出される関数
+     */
+    virtual void fall(std::function<void(void)>& func) = 0;
+#endif
+
+    /**
+     * @brief 入力ピンの状態を返す
+     * @retval 0 Low
+     * @retval 1 High
+     */
+    virtual uint32_t read() = 0;
+};
+
+}  // namespace spirit
+
+#endif  //SPIRIT_INTERFACE_INTERRUPTIN_H

src/spirit/include/SpeedController.h

@@ -0,0 +1,107 @@
+#ifndef SPIRIT_SPEED_CONTROLLER_H
+#define SPIRIT_SPEED_CONTROLLER_H
+
+#include <stdint.h>
+
+#include "InterfaceInterruptIn.h"
+
+namespace spirit {
+
+class SpeedController {
+public:
+    /**
+     * @brief コンストラクタ
+     * @param a_phase RotaryEncoder A phase pin
+     * @param b_phase RotaryEncoder B phase pin
+     */
+    SpeedController(InterfaceInterruptIn& a_phase, InterfaceInterruptIn& b_phase);
+
+    /**
+     * @brief 目標rpsを与えると速度制御の計算結果を返す
+     * @param target_rps 速度制御の目標rps
+     * @param dt         制御周期(s)
+     */
+    float calculation(float target_rps, float dt);
+
+    /**
+     * @brief rpsを算出して返す
+     * @param dt rps
+     * @return 算出されたrps
+     */
+    float rps(float dt);
+
+    /**
+     * @brief 速度制御の計算結果にかける上限値と下限値を設定
+     * @param high_limit 速度制御の計算結果の最大値
+     * @param low_limit  速度制御の計算結果の最小値
+     */
+    void limit(float high_limit, float low_limit);
+
+    /**
+     * @brief PID制御のゲイン係数を設定
+     * @param kp 比例ゲイン
+     * @param ki 積分ゲイン
+     * @param kd 微分ゲイン
+     * @retval false ゲイン変更なし
+     * @retval true  ゲイン変更あり
+     */
+    bool pid_gain(float kp, float ki, float kd);
+
+    /**
+     * @brief ロータリーエンコーダの計算値をリセット
+     */
+    void reset();
+
+    /**
+     * @brief ロータリーエンコーダの角度を返す
+     */
+    float angle();
+
+private:
+    InterfaceInterruptIn& _a_phase;
+    InterfaceInterruptIn& _b_phase;
+
+    float _kp{0.0f};
+    float _ki{0.0f};
+    float _kd{0.0f};
+
+    static constexpr int   _ppr{200};
+    static constexpr float _deg_unit{360.0f / _ppr / 4.0f};
+
+    int _angle_counter;
+
+    bool first_loop;
+
+    /// @note モータの最低回転速度によっては変更の余地あり(ppr=200で0.15[rps]以下が目安??)
+    static constexpr int _angle_buff_max{10};
+    int                  _angle_buff_index;
+    int                  _angle_buff[_angle_buff_max];
+
+    float _sum_error;
+    float _delta_error;
+    float _last_error;
+
+    float _high_limit{0.00f};
+    float _low_limit{0.00f};
+
+    /**
+     * @brief 元の数値にリミッターをかける
+     * @param val 元の数値
+     * @return リミッターをかけた数値
+     */
+    float limiter(float val);
+
+    /**
+     * @brief ロータリーエンコーダのA相が変化した際の処理を行う
+     */
+    void a_phase_interrupt();
+
+    /**
+     * @brief ロータリーエンコーダのB相が変化した際の処理を行う
+     */
+    void b_phase_interrupt();
+};
+
+}  // namespace spirit
+
+#endif  // SPIRIT_SPEED_CONTROLLER_H

src/spirit/platform/mbed/include/InterruptIn.h

@@ -0,0 +1,55 @@
+#ifndef SPIRIT_MBED_INTERRUPTIN_H
+#define SPIRIT_MBED_INTERRUPTIN_H
+
+#if !defined(ARDUINO)
+
+#include "mbed.h"
+#include "spirit/include/InterfaceInterruptIn.h"
+
+namespace spirit {
+
+namespace mbed {
+
+class InterruptIn : public InterfaceInterruptIn {
+public:
+    /**
+     * @brief Constructor
+     * @param pin InterruptIn pin to connect to
+     */
+    InterruptIn(PinName pin);
+
+    /**
+     * @brief Destructor
+     */
+    ~InterruptIn();
+
+    /**
+     * @brief 立ち上がり時に呼び出される関数を設定する
+     * @param func_rise 立ち上がり時に呼び出される関数
+     */
+    void rise(Callback<void()> func) override;
+
+    /**
+     * @brief 立ち下がり時に呼び出される関数を設定する
+     * @param func 立ち下がり時に呼び出される関数
+     */
+    void fall(Callback<void()> func) override;
+
+    /**
+     * @brief 入力ピンの状態を返す
+     * @retval 0 Low
+     * @retval 1 High
+     */
+    uint32_t read() override;
+
+private:
+    ::mbed::InterruptIn _interrupt_in;
+};
+
+}  // namespace mbed
+
+}  // namespace spirit
+
+#endif  // !defined(ARDUINO)
+
+#endif  // SPIRIT_MBED_INTERRUPTIN_H

src/spirit/platform/mbed/source/InterruptIn.cpp

@@ -0,0 +1,39 @@
+#if defined(__MBED__) && !defined(ARDUINO)
+// ArduinoのMbedではCallbackが使えないので、定義しない
+
+#include "spirit/platform/mbed/include/InterruptIn.h"
+
+#include "mbed.h"
+
+namespace spirit {
+
+namespace mbed {
+
+InterruptIn::InterruptIn(PinName pin) : _interrupt_in(pin)
+{
+}
+
+InterruptIn::~InterruptIn()
+{
+}
+
+void InterruptIn::rise(Callback<void()> func)
+{
+    _interrupt_in.rise(func);
+}
+
+void InterruptIn::fall(Callback<void()> func)
+{
+    _interrupt_in.fall(func);
+}
+
+uint32_t InterruptIn::read()
+{
+    return _interrupt_in.read();
+}
+
+}  // namespace mbed
+
+}  // namespace spirit
+
+#endif  // defined(__MBED__) && !defined(ARDUINO)