fix: rpsが正しく算出されていない問題の修正とrpsを参照可能に変更 #281 (#282)

* fix: rpsが正しく算出されていない問題の修正とrpsを参照可能に変更 #281 * fix: バグ修正に伴ったプログラムの変更漏れを修正 #281 * fix: フォーマットエラーを修正 #281 * fix: フォーマットエラーを修正 #281
yutotnh · Sep 11, 2023 · 5a67e49 · 5a67e49
1 parent 0af14c3
commit 5a67e49
Show file tree

Hide file tree

Showing 3 changed files with 35 additions and 24 deletions.
diff --git 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,13 +76,22 @@ 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;
 
     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
@@ -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
@@ -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();
 }