diff --git a/.cspell.json b/.cspell.json index d809c12..4b80d27 100644 --- a/.cspell.json +++ b/.cspell.json @@ -34,6 +34,7 @@ "mattnotmitt", "Mbed", "mdled", + "INTERRUPTIN", "noninteractive", "Nucleo", "pico", diff --git a/CMakeLists.txt b/CMakeLists.txt index ed372d4..1c8c3df 100644 --- a/CMakeLists.txt +++ b/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 ) diff --git a/src/spirit.h b/src/spirit.h index 3bbb6dc..e21a72b 100644 --- a/src/spirit.h +++ b/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 diff --git a/src/spirit/include/InterfaceInterruptIn.h b/src/spirit/include/InterfaceInterruptIn.h new file mode 100644 index 0000000..ff1f3c3 --- /dev/null +++ b/src/spirit/include/InterfaceInterruptIn.h @@ -0,0 +1,68 @@ +#ifndef SPIRIT_INTERFACE_INTERRUPTIN_H +#define SPIRIT_INTERFACE_INTERRUPTIN_H + +#include + +#if defined(__MBED__) && !defined(ARDUINO) +#include "mbed.h" +#elif defined(ARDUINO) +// TODO: Arduinoの場合に必要なヘッダファイルを追加する +#else +#include +#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 func) = 0; +#elif defined(ARDUINO) + // TODO: Arduinoの場合の実装を追加する +#else + /** + * @brief 立ち上がり時に呼び出される関数を設定する + * @param func_rise 立ち上がり時に呼び出される関数 + */ + virtual void rise(std::function& func) = 0; +#endif + +#if defined(__MBED__) && !defined(ARDUINO) + /** + * @brief 立ち下がり時に呼び出される関数を設定する + * @param func 立ち下がり時に呼び出される関数 + */ + virtual void fall(Callback func) = 0; +#elif defined(ARDUINO) + // TODO: Arduinoの場合の実装を追加する +#else + /** + * @brief 立ち下がり時に呼び出される関数を設定する + * @param func 立ち下がり時に呼び出される関数 + */ + virtual void fall(std::function& func) = 0; +#endif + + /** + * @brief 入力ピンの状態を返す + * @retval 0 Low + * @retval 1 High + */ + virtual uint32_t read() = 0; +}; + +} // namespace spirit + +#endif //SPIRIT_INTERFACE_INTERRUPTIN_H diff --git a/src/spirit/include/SpeedController.h b/src/spirit/include/SpeedController.h new file mode 100644 index 0000000..966ba05 --- /dev/null +++ b/src/spirit/include/SpeedController.h @@ -0,0 +1,107 @@ +#ifndef SPIRIT_SPEED_CONTROLLER_H +#define SPIRIT_SPEED_CONTROLLER_H + +#include + +#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 diff --git a/src/spirit/platform/mbed/include/InterruptIn.h b/src/spirit/platform/mbed/include/InterruptIn.h new file mode 100644 index 0000000..82402d5 --- /dev/null +++ b/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 func) override; + + /** + * @brief 立ち下がり時に呼び出される関数を設定する + * @param func 立ち下がり時に呼び出される関数 + */ + void fall(Callback 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 diff --git a/src/spirit/platform/mbed/source/InterruptIn.cpp b/src/spirit/platform/mbed/source/InterruptIn.cpp new file mode 100644 index 0000000..0bbd19c --- /dev/null +++ b/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 func) +{ + _interrupt_in.rise(func); +} + +void InterruptIn::fall(Callback func) +{ + _interrupt_in.fall(func); +} + +uint32_t InterruptIn::read() +{ + return _interrupt_in.read(); +} + +} // namespace mbed + +} // namespace spirit + +#endif // defined(__MBED__) && !defined(ARDUINO) diff --git a/src/spirit/src/SpeedController.cpp b/src/spirit/src/SpeedController.cpp new file mode 100644 index 0000000..acbfbbd --- /dev/null +++ b/src/spirit/src/SpeedController.cpp @@ -0,0 +1,160 @@ +#include "spirit/include/SpeedController.h" + +#include + +#include "spirit/include/Error.h" + +namespace spirit { + +SpeedController::SpeedController(InterfaceInterruptIn& a_phase, InterfaceInterruptIn& b_phase) + : _a_phase(a_phase), _b_phase(b_phase) +{ +#if defined(__MBED__) && !defined(ARDUINO) + _a_phase.rise(mbed::callback(this, &SpeedController::a_phase_interrupt)); + _a_phase.fall(mbed::callback(this, &SpeedController::a_phase_interrupt)); + _b_phase.rise(mbed::callback(this, &SpeedController::b_phase_interrupt)); + _b_phase.fall(mbed::callback(this, &SpeedController::b_phase_interrupt)); +#elif defined(ARDUINO) + // TODO Arduinoに対応 +#else + std::function func_a = std::bind(&SpeedController::a_phase_interrupt, this); + std::function func_b = std::bind(&SpeedController::b_phase_interrupt, this); + _a_phase.rise(func_a); + _a_phase.fall(func_a); + _b_phase.rise(func_b); + _b_phase.fall(func_b); +#endif + reset(); +} + +float SpeedController::calculation(float target_rps, float dt) +{ + if (target_rps < 0.0f) { + Error::get_instance().error(Error::Type::InvalidValue, 0, __FILE__, __func__, __LINE__, + "[target_rps = %g] value cannot be negative", target_rps); + return 0.0f; + } + + if (dt <= 0.0f) { + Error::get_instance().error(Error::Type::InvalidValue, 0, __FILE__, __func__, __LINE__, + "[dt = %g] value cannot be less than 0", dt); + return 0.0f; + } + + float error = target_rps - rps(dt); + _sum_error += error; + _delta_error = error - _last_error; + + float p = _kp * error; + float i = limiter(_ki * _sum_error * dt / 2.0f); + float d = _kd * _delta_error / dt; + + _last_error = error; + + return limiter(p + i + d); +} + +float SpeedController::rps(float dt) +{ + if (dt <= 0.0f) { + Error::get_instance().error(Error::Type::InvalidValue, 0, __FILE__, __func__, __LINE__, + "[dt = %g] value cannot be less than 0", dt); + return 0.0f; + } + + float rps; + _angle_buff_index++; + + if (_angle_buff_index == _angle_buff_max) { + _angle_buff_index = 0; + first_loop = false; + } else if (first_loop) { + rps = angle() / (dt * _angle_buff_index); + if (rps < 0.0f) { + rps *= -1; + } + return rps; + } + + float angle_diff = _angle_counter - _angle_buff[_angle_buff_index]; + _angle_buff[_angle_buff_index] = _angle_counter; + + rps = (angle_diff * _deg_unit) / (dt * _angle_buff_max); + if (rps < 0.0f) { + rps *= -1; + } + return rps; +} + +void SpeedController::limit(float high_limit, float low_limit) +{ + if (high_limit > low_limit) { + _high_limit = high_limit; + _low_limit = low_limit; + } else { + Error::get_instance().error(Error::Type::InvalidValue, 0, __FILE__, __func__, __LINE__, + "[high_limit = %g, low_limit = %g]The relationship between the upper limit value " + "and the lower limit value is strange", + high_limit, low_limit); + } +} + +bool SpeedController::pid_gain(float kp, float ki, float kd) +{ + if ((_kp == kp) && (_ki == ki) && (_kd == kd)) { + return false; + } + + _kp = kp; + _ki = ki; + _kd = kd; + + return true; +} + +void SpeedController::reset() +{ + _angle_counter = 0; + _angle_buff_index = 0; + _sum_error = 0.0f; + _last_error = 0.0f; + for (int i = 0; i < _angle_buff_max; i++) { + _angle_buff[i] = 0; + } + first_loop = true; +} + +float SpeedController::limiter(float val) +{ + if (val > _high_limit) { + return _high_limit; + } else if (val < _low_limit) { + return _low_limit; + } + return val; +} + +void SpeedController::a_phase_interrupt() +{ + if (_a_phase.read() != _b_phase.read()) { + _angle_counter++; + } else { + _angle_counter--; + } +} + +void SpeedController::b_phase_interrupt() +{ + if (_a_phase.read() == _b_phase.read()) { + _angle_counter++; + } else { + _angle_counter--; + } +} + +float SpeedController::angle() +{ + return _angle_counter * _deg_unit; +} + +} // namespace spirit diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 56b103c..15f6dbc 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -51,3 +51,4 @@ add_gtest(test_Motor.cpp) add_gtest(test_MotorDataConverter.cpp) add_gtest(test_PwmDataConverter.cpp) add_gtest(test_SpeedDataConverter.cpp) +add_gtest(test_SpeedController.cpp) diff --git a/tests/stubs.h b/tests/stubs.h index a927221..0e8d5da 100644 --- a/tests/stubs.h +++ b/tests/stubs.h @@ -2,6 +2,7 @@ #define SPIRIT_STUB_H #include "InterfaceDigitalOut.h" +#include "InterfaceInterruptIn.h" #include "InterfacePwmOut.h" namespace spirit { @@ -34,6 +35,65 @@ class StubDigitalOut : public InterfaceDigitalOut { uint32_t _value{0}; }; +class StubInterruptIn : public InterfaceInterruptIn { +public: + /** + * @brief 立ち上がり時に呼び出される関数を設定する + * @param func_rise 立ち上がり時に呼び出される関数 + */ + void rise(std::function& func_rise) override + { + _func_rise = func_rise; + } + + /** + * @brief 立ち下がり時に呼び出される関数を設定する + * @param func_fall 立ち下がり時に呼び出される関数 + */ + void fall(std::function& func_fall) override + { + _func_fall = func_fall; + } + + /** + * @brief 立ち上がり時に行われる関数呼び出しを再現 + */ + void interrupt_rise() + { + _value = 1; + _func_rise(); + } + + /** + * @brief 立ち下がり時に行われる関数呼び出しを再現 + */ + void interrupt_fall() + { + _value = 0; + _func_fall(); + } + + /** + * @brief 入力ピンの状態を返す + * @retval 0 Low + * @retval 1 High + */ + uint32_t read() override + { + return _value; + } + +private: + /// 立ち上がり時に呼び出す関数 + std::function _func_rise; + + /// 立ち下がり時に呼び出す関数 + std::function _func_fall; + + /// 入力ピンの状態 + uint32_t _value{0}; +}; + class StubPwmOut : public InterfacePwmOut { public: /** diff --git a/tests/test_SpeedController.cpp b/tests/test_SpeedController.cpp new file mode 100644 index 0000000..0104634 --- /dev/null +++ b/tests/test_SpeedController.cpp @@ -0,0 +1,96 @@ +#include + +#include "Error.h" +#include "SpeedController.h" +#include "tests/stubs.h" + +namespace { + +using namespace spirit; + +TEST(SpeedController, SpeedControllerTest) +{ + StubInterruptIn a_phase; + StubInterruptIn b_phase; + SpeedController speed_controller(a_phase, b_phase); + + // 計算しやすいようにゲインを設定 + speed_controller.pid_gain(1.00f, 1.00f, 1.00f); + speed_controller.limit(3.00f, 0.00f); + + /* エンコーダの仮想パルス生成 (1.8°回転) */ + a_phase.interrupt_rise(); + b_phase.interrupt_rise(); + a_phase.interrupt_fall(); + b_phase.interrupt_fall(); + /* ここまで */ + + // 角度 + EXPECT_FLOAT_EQ(speed_controller.angle(), 1.80f); + // RPS + EXPECT_FLOAT_EQ(speed_controller.rps(1.00f), 1.80f); + // PID計算(リミットなし) + EXPECT_FLOAT_EQ(speed_controller.calculation(1.80f, 1.00f), 2.25f); + + speed_controller.limit(1.00f, 0.00f); + + // PID計算(上限リミット) + EXPECT_FLOAT_EQ(speed_controller.calculation(1.80f, 1.00f), 1.00f); + + speed_controller.reset(); + + speed_controller.limit(1.00f, -3.00f); + + /* エンコーダの仮想パルス生成 (-1.8°回転) */ + b_phase.interrupt_rise(); + a_phase.interrupt_rise(); + b_phase.interrupt_fall(); + a_phase.interrupt_fall(); + /* ここまで */ + + // 角度(マイナス) + EXPECT_FLOAT_EQ(speed_controller.angle(), -1.80f); + // 角度(マイナスなし) + EXPECT_FLOAT_EQ(speed_controller.rps(1.00f), 1.80f); + // PID計算(リミットなし) + EXPECT_FLOAT_EQ(speed_controller.calculation(0.00f, 1.00f), -2.25); + + speed_controller.limit(1.00f, 0.00f); + // PID計算(下限リミット) + EXPECT_FLOAT_EQ(speed_controller.calculation(0.00f, 1.00f), 0.00f); +} + +TEST(SpeedController, SpeedControllerErrorTest) +{ + StubInterruptIn a_phase; + StubInterruptIn b_phase; + SpeedController speed_controller(a_phase, b_phase); + + // Error時に標準エラー出力に文字列が出力される + // 本当のエラー時にエラー出力させたいので、異常系のテスト中は標準エラー出力をキャプチャする + testing::internal::CaptureStderr(); + + Error& error = Error::get_instance(); + EXPECT_EQ(error.get_status(), Error::Status::Normal); + speed_controller.limit(0.00f, 1.00f); + EXPECT_EQ(error.get_status(), Error::Status::Error); + + error.reset(); + EXPECT_EQ(error.get_status(), Error::Status::Normal); + speed_controller.calculation(-1.00f, 1.00f); + EXPECT_EQ(error.get_status(), Error::Status::Error); + + error.reset(); + EXPECT_EQ(error.get_status(), Error::Status::Normal); + speed_controller.calculation(1.00f, 0.00f); + EXPECT_EQ(error.get_status(), Error::Status::Error); + + error.reset(); + EXPECT_EQ(error.get_status(), Error::Status::Normal); + speed_controller.rps(0.00f); + EXPECT_EQ(error.get_status(), Error::Status::Error); + + testing::internal::GetCapturedStderr(); +} + +} // namespace \ No newline at end of file