Merge branch 'main' into ticket-KL25-19

Author: takahashitom

CMakeLists.txt

@@ -36,6 +36,10 @@ set(PROJECT_TEST_DIR ${PROJECT_SOURCE_DIR}/tests)
 set(INCLUDE_DIRS
   ${PROJECT_MODULE_DIR}
   ${PROJECT_MODULE_DIR}/CameraCapture
+  ${PROJECT_MODULE_DIR}/calculators
+  ${PROJECT_MODULE_DIR}/API
+  ${PROJECT_TEST_DIR}
+  ${PROJECT_TEST_DIR}/dummy
   ${PROJECT_TEST_DIR}/helpers
   /usr/include/opencv4
 )
@@ -44,6 +48,7 @@ set(INCLUDE_DIRS
 file(GLOB_RECURSE PROJECT_SOURCES
   "${PROJECT_MODULE_DIR}/*.cpp"
   "${PROJECT_MODULE_DIR}/CameraCapture/*.cpp"
+  "${PROJECT_MODULE_DIR}/calculators/*.cpp"
   "${PROJECT_TEST_DIR}/helpers/*.cpp"
 )
 
@@ -61,6 +66,11 @@ file(GLOB TEST_SOURCES "${PROJECT_TEST_DIR}/*Test.cpp")
 
 # テスト実行可能ファイルの作成
 add_executable(${PROJECT_NAME}_test ${TEST_SOURCES})
+target_include_directories(${PROJECT_NAME}_test
+PRIVATE
+    ${PROJECT_TEST_DIR}/dummy  # spikeapi ダミー用
+)
+
 target_link_libraries(${PROJECT_NAME}_test
   PRIVATE
     ${PROJECT_NAME}_impl
@@ -70,4 +80,4 @@ target_link_libraries(${PROJECT_NAME}_test
 
 # テストの登録
 include(GoogleTest)
-gtest_discover_tests(${PROJECT_NAME}_test)
+gtest_discover_tests(${PROJECT_NAME}_test)

Makefile.inc

@@ -2,7 +2,6 @@ USE_RASPIKE_ART=1
 
 mkfile_path := $(dir $(lastword $(MAKEFILE_LIST)))
 
-
 # Shellスクリプトで、moduleディレクトリ中のソースコード名をオブジェクトファイル名に変換している
 SRCS := $(shell find ${mkfile_path}modules -name '*.cpp')
 OBJS := $(notdir $(SRCS:.cpp=.o))
@@ -13,15 +12,19 @@ SRCLANG := c++
 APPL_LIBS += -lm
 
 APPL_DIRS += \
-	$(mkfile_path)modules \
+	$(mkfile_path)modules\
+	$(mkfile_path)modules/API\
+	$(mkfile_path)modules/calculators\
 	$(mkfile_path)modules/CameraCapture
 
 INCLUDES += \
-	-I$(mkfile_path)modules \
+	-I$(mkfile_path)modules\
+	-I$(mkfile_path)modules/API\
+	-I$(mkfile_path)modules/calculators\
 	-I$(mkfile_path)modules/CameraCapture \
 	-I/usr/include/opencv4
 
 APPL_LIBS += $(shell pkg-config --libs opencv4)
 
 CXXFLAGS += -std=c++11 -pthread
-LDFLAGS += -pthread
+LDFLAGS += -pthread

modules/API/Motor.h

@@ -0,0 +1,137 @@
+/**
+ * @file Motor.h
+ * @brief spikeapiのモータラッパークラス
+ * @author nishijima515
+ */
+
+#ifndef MOTOR_H
+#define MOTOR_H
+
+#include "spike/pup/motor.h"
+#include "Port.h"
+
+/**
+ * SPIKE モータラッパークラス
+ * モータの回転方向（時計回り、反時計回り）を指定するための列挙型
+ * CLOCKWISEは時計回り、COUNTERCLOCKWISEは反時計回りを表す
+ */
+class Motor {
+ public:
+  enum class EDirection {
+    CLOCKWISE = PUP_DIRECTION_CLOCKWISE,
+    COUNTERCLOCKWISE = PUP_DIRECTION_COUNTERCLOCKWISE,
+  };
+
+  /**
+   * コンストラクタ
+   * @param port PUPポートID
+   * @param direction モータの回転方向
+   * @param reset_count カウントをリセットするか
+   * pup_motor_setup()を複数回呼ぶとハングするため、コンストラクタで一回だけ呼ぶことでエラーを回避する
+   */
+  Motor(Port port, EDirection direction = EDirection::CLOCKWISE, bool reset_count = true)
+    : mHasError(false)
+  {
+    pupDevicePointer = pup_motor_get_device(static_cast<pbio_port_id_t>(port));
+    if(!pupDevicePointer) {
+      mHasError = true;
+      return;
+    }
+    pbio_error_t ret
+        = pup_motor_setup(pupDevicePointer, static_cast<pup_direction_t>(direction), reset_count);
+    if(ret != PBIO_SUCCESS) {
+      mHasError = true;
+    }
+  }
+
+  /**
+   * エンコーダをリセットする
+   */
+  void resetCount() const { pup_motor_reset_count(pupDevicePointer); }
+
+  /**
+   * エンコーダの値を取得する
+   * @return エンコーダの値 [°]
+   */
+  int32_t getCount() const { return pup_motor_get_count(pupDevicePointer); }
+
+  /**
+   * モータの回転速度を取得する
+   * @return 回転速度 [°/秒]
+   */
+  int32_t getSpeed() const { return pup_motor_get_speed(pupDevicePointer); }
+
+  /**
+   * モータの回転速度を設定する
+   * @param speed モータの回転速度 [°/秒]
+   */
+  void setSpeed(int speed) const { pup_motor_set_speed(pupDevicePointer, speed); }
+
+  /**
+   * モータのパワー値を取得する
+   * @return パワー値（-100 ～ +100）
+   */
+  int32_t getPower() const { return pup_motor_get_power(pupDevicePointer); }
+
+  /**
+   * モータのパワー値を設定する
+   * @param power モータのパワー値（-100 ～ +100）
+   */
+  void setPower(int power) const { pup_motor_set_power(pupDevicePointer, power); }
+
+  /**
+   * モータを止める
+   */
+  void stop() const { pup_motor_stop(pupDevicePointer); }
+
+  /**
+   * ブレーキをかけてモータを止める
+   */
+  void brake() const { pup_motor_brake(pupDevicePointer); }
+
+  /**
+   * モータを止めて角度を維持する
+   */
+  void hold() const { pup_motor_hold(pupDevicePointer); }
+
+  /**
+   * モータがストールしているか調べる
+   * @return true ストールしている
+   * @return false ストールしていない
+   */
+  bool isStalled() const { return pup_motor_is_stalled(pupDevicePointer); }
+
+  /**
+   * モータのデューティ値を下げる
+   * @param duty_limit 新しいデューティ値（0-100）
+   * @return 元の状態に戻すための最大電圧
+   */
+  int32_t setDutyLimit(int duty_limit) const
+  {
+    return pup_motor_set_duty_limit(pupDevicePointer, duty_limit);
+  }
+
+  /**
+   * モータのデューティ値を元に戻す
+   * @param old_value pup_motor_set_duty_limitの戻り値
+   */
+  void restoreDutyLimit(int old_value) const
+  {
+    pup_motor_restore_duty_limit(pupDevicePointer, old_value);
+  }
+
+  /**
+   * インスタンス生成が正常にできたかどうかを確認するための共通メソッド
+   * @return true インスタンス生成に失敗した
+   * @return false インスタンス生成に成功した
+   */
+  bool hasError() { return mHasError; }
+
+ private:
+  pup_motor_t* pupDevicePointer;  // SPIKE API のモーター構造体へのポインタ
+  // フォーマットエラー解消のための文章です
+  bool mHasError;  // インスタンス生成に失敗したかどうかを示すフラグ
+
+};  // class Motor
+
+#endif

modules/API/MotorController.cpp

@@ -0,0 +1,153 @@
+/**
+ * @file MotorController.cpp
+ * @brief モータ制御に用いる関数をまとめたラッパークラス
+ * @author nishijima515
+ */
+#include "MotorController.h"
+
+MotorController::MotorController()
+  : rightWheel(Port::PORT_A),
+    leftWheel(Port::PORT_B, Motor::EDirection::COUNTERCLOCKWISE),
+    armMotor(Port::PORT_C, Motor::EDirection::COUNTERCLOCKWISE)
+{
+}
+
+// モータに設定するpower値の制限
+int MotorController::limitPowerValue(int inputPower)
+{
+  if(inputPower > MOTOR_POWER_MAX) {
+    return MOTOR_POWER_MAX;
+  } else if(inputPower < MOTOR_POWER_MIN) {
+    return MOTOR_POWER_MIN;
+  }
+  return inputPower;
+}
+
+// 右モータにpower値をセット
+void MotorController::setRightMotorPower(int power)
+{
+  rightWheel.setPower(limitPowerValue(power));
+}
+
+// 左モータにpower値をセット
+void MotorController::setLeftMotorPower(int power)
+{
+  leftWheel.setPower(limitPowerValue(power));
+}
+
+// 右モータのpower値をリセット
+void MotorController::resetRightMotorPower()
+{
+  rightWheel.setPower(0);
+}
+
+// 左モータのpower値をリセット
+void MotorController::resetLeftMotorPower()
+{
+  leftWheel.setPower(0);
+}
+
+// 右左両モータの状態をリセット
+void MotorController::resetWheelsMotorPower()
+{
+  rightWheel.setPower(0);
+  leftWheel.setPower(0);
+}
+
+// 右タイヤのモータに回転速度をセット
+void MotorController::setRightMotorSpeed(int speed)
+{
+  rightWheel.setSpeed(speed);
+}
+
+// 左タイヤのモータに回転速度をセット
+void MotorController::setLeftMotorSpeed(int speed)
+{
+  leftWheel.setSpeed(speed);
+}
+
+// 両タイヤのモータを停止する
+void MotorController::stopWheelsMotor()
+{
+  rightWheel.stop();
+  leftWheel.stop();
+}
+
+// ブレーキをかけてタイヤのモータを停止する
+void MotorController::brakeWheelsMotor()
+{
+  rightWheel.brake();
+  leftWheel.brake();
+}
+
+// アームのモータにpower値をセット
+void MotorController::setArmMotorPower(int power)
+{
+  armMotor.setPower(limitPowerValue(power));
+}
+
+// アームのモータのpower値をリセット
+void MotorController::resetArmMotorPower()
+{
+  armMotor.setPower(0);
+}
+
+// アームのモータを停止する
+void MotorController::stopArmMotor()
+{
+  armMotor.stop();
+}
+
+// アームモータを止めて角度を維持する
+void MotorController::holdArmMotor()
+{
+  armMotor.hold();
+}
+
+// 右タイヤのpower値を取得する
+int MotorController::getRightMotorPower()
+{
+  return rightWheel.getPower();
+}
+
+// 左タイヤのpower値を取得する
+int MotorController::getLeftMotorPower()
+{
+  return leftWheel.getPower();
+}
+
+// アームモータのpower値を取得する
+int MotorController::getArmMotorPower()
+{
+  return armMotor.getPower();
+}
+
+// 右モータの角位置を取得する
+int32_t MotorController::getRightMotorCount()
+{
+  return rightWheel.getCount();
+}
+
+// 左モータの角位置を取得する
+int32_t MotorController::getLeftMotorCount()
+{
+  return leftWheel.getCount();
+}
+
+// アームモータの角位置を取得する
+int32_t MotorController::getArmMotorCount()
+{
+  return armMotor.getCount();
+}
+
+// 右タイヤモータの回転速度を取得する
+int32_t MotorController::getRightMotorSpeed()
+{
+  return rightWheel.getSpeed();
+}
+
+// 左タイヤモータの回転速度を取得する
+int32_t MotorController::getLeftMotorSpeed()
+{
+  return leftWheel.getSpeed();
+}