diff --git a/src/spirit/include/SpeedController.h b/src/spirit/include/SpeedController.h index 966ba05..9e498a2 100644 --- a/src/spirit/include/SpeedController.h +++ b/src/spirit/include/SpeedController.h @@ -23,13 +23,6 @@ class SpeedController { */ float calculation(float target_rps, float dt); - /** - * @brief rpsを算出して返す - * @param dt rps - * @return 算出されたrps - */ - float rps(float dt); - /** * @brief 速度制御の計算結果にかける上限値と下限値を設定 * @param high_limit 速度制御の計算結果の最大値 @@ -57,6 +50,12 @@ class SpeedController { */ float angle(); + /** + * @brief rpsを返す + * @return rps + */ + float rps(); + private: InterfaceInterruptIn& _a_phase; InterfaceInterruptIn& _b_phase; @@ -77,6 +76,8 @@ class SpeedController { int _angle_buff_index; int _angle_buff[_angle_buff_max]; + float _rps{0.00f}; + float _sum_error; float _delta_error; float _last_error; @@ -84,6 +85,13 @@ class SpeedController { float _high_limit{0.00f}; float _low_limit{0.00f}; + /** + * @brief rpsを算出して返す + * @param dt rps + * @return 算出されたrps + */ + float rps_calculation(float dt); + /** * @brief 元の数値にリミッターをかける * @param val 元の数値 diff --git a/src/spirit/src/SpeedController.cpp b/src/spirit/src/SpeedController.cpp index acbfbbd..55600d9 100644 --- a/src/spirit/src/SpeedController.cpp +++ b/src/spirit/src/SpeedController.cpp @@ -41,7 +41,9 @@ float SpeedController::calculation(float target_rps, float dt) return 0.0f; } - float error = target_rps - rps(dt); + _rps = rps_calculation(dt); + + float error = target_rps - _rps; _sum_error += error; _delta_error = error - _last_error; @@ -54,7 +56,7 @@ float SpeedController::calculation(float target_rps, float dt) return limiter(p + i + d); } -float SpeedController::rps(float dt) +float SpeedController::rps_calculation(float dt) { if (dt <= 0.0f) { Error::get_instance().error(Error::Type::InvalidValue, 0, __FILE__, __func__, __LINE__, @@ -69,7 +71,7 @@ float SpeedController::rps(float dt) _angle_buff_index = 0; first_loop = false; } else if (first_loop) { - rps = angle() / (dt * _angle_buff_index); + rps = angle() / 360.0f / (dt * _angle_buff_index); if (rps < 0.0f) { rps *= -1; } @@ -79,7 +81,7 @@ float SpeedController::rps(float dt) 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); + rps = (angle_diff * _deg_unit) / 360.0f / (dt * _angle_buff_max); if (rps < 0.0f) { rps *= -1; } @@ -118,6 +120,7 @@ void SpeedController::reset() _angle_buff_index = 0; _sum_error = 0.0f; _last_error = 0.0f; + _rps = 0.0f; for (int i = 0; i < _angle_buff_max; i++) { _angle_buff[i] = 0; } @@ -157,4 +160,9 @@ float SpeedController::angle() return _angle_counter * _deg_unit; } +float SpeedController::rps() +{ + return _rps; +} + } // namespace spirit diff --git a/tests/test_SpeedController.cpp b/tests/test_SpeedController.cpp index 0104634..c2b454b 100644 --- a/tests/test_SpeedController.cpp +++ b/tests/test_SpeedController.cpp @@ -27,19 +27,19 @@ TEST(SpeedController, SpeedControllerTest) // 角度 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); + EXPECT_FLOAT_EQ(speed_controller.calculation(1.005f, 1.00f), 2.5f); + // RPS + EXPECT_FLOAT_EQ(speed_controller.rps(), 0.005f); speed_controller.limit(1.00f, 0.00f); // PID計算(上限リミット) - EXPECT_FLOAT_EQ(speed_controller.calculation(1.80f, 1.00f), 1.00f); + EXPECT_FLOAT_EQ(speed_controller.calculation(1.00f, 1.00f), 1.00f); speed_controller.reset(); - speed_controller.limit(1.00f, -3.00f); + speed_controller.limit(1.00f, -1.00f); /* エンコーダの仮想パルス生成 (-1.8°回転) */ b_phase.interrupt_rise(); @@ -50,10 +50,10 @@ TEST(SpeedController, SpeedControllerTest) // 角度(マイナス) 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); + EXPECT_FLOAT_EQ(speed_controller.calculation(0.00f, 1.00f), -0.0125); + // RPS(マイナスなし) + EXPECT_FLOAT_EQ(speed_controller.rps(), 0.005f); speed_controller.limit(1.00f, 0.00f); // PID計算(下限リミット) @@ -85,11 +85,6 @@ TEST(SpeedController, SpeedControllerErrorTest) 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(); }