#KL25-27 モータコントローラクラスの作成

modules/API/MotorController.cpp

@@ -4,11 +4,12 @@
  * @author nishijima515
  */
 #include "MotorController.h"
+using namespace spikeapi;
 
 MotorController::MotorController()
-  : rightWheel(Port::PORT_A),
-    leftWheel(Port::PORT_B, Motor::EDirection::COUNTERCLOCKWISE),
-    armMotor(Port::PORT_C, Motor::EDirection::COUNTERCLOCKWISE)
+  : rightWheel(EPort::PORT_A),
+    leftWheel(EPort::PORT_B, Motor::EDirection::COUNTERCLOCKWISE),
+    armMotor(EPort::PORT_C, Motor::EDirection::COUNTERCLOCKWISE)
 {
 }
 
@@ -54,16 +55,18 @@ void MotorController::resetWheelsMotorPower()
   leftWheel.setPower(0);
 }
 
-// 右タイヤのモータに回転速度をセット
-void MotorController::setRightMotorSpeed(int speed)
+// 右タイヤのモータに,線速度を回転速度に変換しセットする
+void MotorController::setRightMotorSpeed(double speed)
 {
-  rightWheel.setSpeed(speed);
+  int rightAngleSpeed = static_cast<int>(speed / WHEEL_RADIUS * (RAD_TO_DEG));
+  rightWheel.setSpeed(rightAngleSpeed);
 }
 
-// 左タイヤのモータに回転速度をセット
-void MotorController::setLeftMotorSpeed(int speed)
+// 左タイヤのモータに,線速度を回転速度に変換しセットする
+void MotorController::setLeftMotorSpeed(double speed)
 {
-  leftWheel.setSpeed(speed);
+  int leftAngleSpeed = static_cast<int>(speed / WHEEL_RADIUS * (RAD_TO_DEG));
+  leftWheel.setSpeed(leftAngleSpeed);
 }
 
 // 両タイヤのモータを停止する
@@ -140,14 +143,16 @@ int32_t MotorController::getArmMotorCount()
   return armMotor.getCount();
 }
 
-// 右タイヤモータの回転速度を取得する
-int32_t MotorController::getRightMotorSpeed()
+// 右タイヤモータの線速度を取得する
+double MotorController::getRightMotorSpeed()
 {
-  return rightWheel.getSpeed();
+  double rightSpeed = rightWheel.getSpeed() * (DEG_TO_RAD)*WHEEL_RADIUS;
+  return rightSpeed;
 }
 
-// 左タイヤモータの回転速度を取得する
-int32_t MotorController::getLeftMotorSpeed()
+// 左タイヤモータの線速度を取得する
+double MotorController::getLeftMotorSpeed()
 {
-  return leftWheel.getSpeed();
+  double leftSpeed = leftWheel.getSpeed() * (DEG_TO_RAD)*WHEEL_RADIUS;
+  return leftSpeed;
 }

modules/API/MotorController.h

@@ -6,7 +6,10 @@
 #ifndef MOTOR_MOTORCONTROLLER_H
 #define MOTOR_MOTORCONTROLLER_H
 
+#define WHEEL_RADIUS RADIUS  // 車輪の半径[mm]
+
 #include "Motor.h"
+#include "SystemInfo.h"  // RADIUS, PI, RAD_TO_DEGの定義を含む
 
 class MotorController {
  public:
@@ -49,16 +52,16 @@ class MotorController {
   void resetWheelsMotorPower();
 
   /**
-   * @brief 右タイヤのモータに回転速度をセット
-   * @param speed 回転速度（°/秒）
+   * @brief 右タイヤのモータに, 線速度から算出した回転速度をセット
+   * @param speed 線速度（mm/秒）
    */
-  void setRightMotorSpeed(int speed);
+  void setRightMotorSpeed(double speed);
 
   /**
-   * @brief 左タイヤのモータに回転速度をセット
-   * @param speed 回転速度（°/秒）
+   * @brief 左タイヤのモータに, 線速度から算出した回転速度をセット
+   * @param speed 線速度（mm/秒）
    */
-  void setLeftMotorSpeed(int speed);
+  void setLeftMotorSpeed(double speed);
 
   /**
    * @brief 両タイヤのモータを停止する
@@ -129,21 +132,21 @@ class MotorController {
   int getArmMotorPower();
 
   /**
-   * @brief 右タイヤの回転速度を取得する
-   * @return 右タイヤの回転速度（°/秒）
+   * @brief 右タイヤの線速度を取得する
+   * @return 右タイヤの線速度（mm/秒）
    */
-  int32_t getRightMotorSpeed();
+  double getRightMotorSpeed();
 
   /**
-   * @brief 左タイヤの回転速度を取得する
-   * @return 左タイヤの回転速度（°/秒）
+   * @brief 左タイヤの線速度を取得する
+   * @return 左タイヤの線速度（mm/秒）
    */
-  int32_t getLeftMotorSpeed();
+  double getLeftMotorSpeed();
 
  private:
-  Motor rightWheel;  // 右タイヤモータのインスタンス
-  Motor leftWheel;   // 左タイヤモータのインスタンス
-  Motor armMotor;    // アームモータのインスタンス
+  spikeapi::Motor rightWheel;  // 右タイヤモータのインスタンス
+  spikeapi::Motor leftWheel;   // 左タイヤモータのインスタンス
+  spikeapi::Motor armMotor;    // アームモータのインスタンス
 
   /**
    * @brief モータに設定するpower値の制限

modules/common/SystemInfo.h

@@ -7,6 +7,10 @@
 #ifndef SYSTEM_INFO_H
 #define SYSTEM_INFO_H
 
+#define PI 3.1415926           // 円周率
+#define RAD_TO_DEG 180.0 / PI  // ラジアンを度数に変換するための係数
+#define DEG_TO_RAD PI / 180.0  // 度数をラジアンに変換するための係数
+
 static constexpr double RADIUS = 28.0;  // 車輪の半径[mm]
 static constexpr double TREAD = 112.0;  // 走行体のトレッド幅（両輪の間の距離）[mm]
 

tests/MotorControllerTest.cpp

@@ -7,6 +7,8 @@
 #include "MotorController.h"
 #include <gtest/gtest.h>
 
+using namespace spikeapi;
+
 namespace etrobocon2025_test {
 
   // 右車輪のモータにpower値をセットできるかのテスト
@@ -219,39 +221,46 @@ namespace etrobocon2025_test {
   TEST(MotorControllerTest, GetRightMotorSpeed)
   {
     MotorController MotorController;
-    int expected = 0;
-    int actual = MotorController.getRightMotorSpeed();
+    double expected = 0.0;
+    double actual = MotorController.getRightMotorSpeed();
     EXPECT_EQ(expected, actual);
-    int speed = 1000;
+    double speed = 1000.0;
     MotorController.setRightMotorSpeed(speed);
-    expected = 1000;
+    double expectedLower = speed - 1;  // getやsetの計算過程でintに丸められた際の許容誤差
     actual = MotorController.getRightMotorSpeed();
-    EXPECT_EQ(expected, actual);
-    speed = -1000;
+    // 　誤差が1以内であれば取得できているとする
+    EXPECT_LE(expectedLower, actual);
+    EXPECT_GE(speed, actual);
+    speed = -1000.0;
     MotorController.setRightMotorSpeed(speed);
-    expected = -1000;
+    double expectedUpper = speed + 1;  // getやsetの計算過程でintに丸められた際の許容誤差
     actual = MotorController.getRightMotorSpeed();
-    EXPECT_EQ(expected, actual);
+    // 　誤差が1以内であれば取得できているとする
+    EXPECT_GE(actual, speed);
+    EXPECT_LE(actual, expectedUpper);
     MotorController.resetRightMotorPower();
   }
 
   // 左車輪のモータの回転速度を取得できるかのテスト
   TEST(MotorControllerTest, GetLeftMotorSpeed)
   {
     MotorController MotorController;
-    int expected = 0;
-    int actual = MotorController.getLeftMotorSpeed();
+    double expected = 0.0;
+    double actual = MotorController.getLeftMotorSpeed();
     EXPECT_EQ(expected, actual);
-    int speed = 1000;
+    double speed = 1000.0;
     MotorController.setLeftMotorSpeed(speed);
-    expected = 1000;
+    double expectedLower = speed - 1;  // getやsetの計算過程でintに丸められた際の許容誤差
     actual = MotorController.getLeftMotorSpeed();
-    EXPECT_EQ(expected, actual);
-    speed = -1000;
+    // 　誤差が1以内であれば取得できているとする
+    EXPECT_LE(expectedLower, actual);
+    EXPECT_GE(speed, actual);
+    speed = -1000.0;
     MotorController.setLeftMotorSpeed(speed);
-    expected = -1000;
+    double expectedUpper = speed + 1;  // getやsetの計算過程でintに丸められた際の許容誤差
     actual = MotorController.getLeftMotorSpeed();
-    EXPECT_EQ(expected, actual);
+    EXPECT_GE(actual, speed);
+    EXPECT_LE(actual, expectedUpper);
     MotorController.resetLeftMotorPower();
   }
 }  // namespace etrobocon2025_test