Feat/driver/add can driver (#134)

* Add example code for motor driver

* create ddt_motor_driver pkg directory

Signed-off-by: Autumn60 <[email protected]>

* Add ddt motor driver

* Add drivers

* Update file structure

* Update package depends

* Update pre-commint fix

* Delete unnecessary files

* Update pre-commit fix

* Update to fix spell

* Update src/driver/ddt_motor_driver/package.xml

Co-authored-by: Autumn60 <[email protected]>

* Update CODEOWNERS

---------

Signed-off-by: Autumn60 <[email protected]>
Co-authored-by: Autumn60 <[email protected]>
Co-authored-by: Autumn60 <[email protected]>

.github/CODEOWNERS

@@ -1,6 +1,7 @@
 src/common/wheel_stuck_common_utils/** @Autumn60 @Bey9434
 src/control/joy2twist/** @Bey9434
 src/description/wheel_stuck_go/** @Autumn60
+src/driver/ddt_motor_driver/** @RyodoTanaka
 src/launch/wheel_stuck_launcher/** @Autumn60
 src/launch/wheel_stuck_robot_launcher/** @Autumn60
 src/launch/wheel_stuck_sensing_launcher/** @Autumn60

src/driver/ddt_motor_driver/CMakeLists.txt

@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 3.5)
+project(ddt_motor_driver)
+
+find_package(ament_cmake_python REQUIRED)
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_python_install_package(${PROJECT_NAME})
+
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+  config
+)

src/driver/ddt_motor_driver/ddt_motor_driver/ddt_motor_driver.py

@@ -0,0 +1,154 @@
+import math
+import time
+
+import can
+
+
+class DDTMotorDriver:
+    def __init__(self, max_speed_mps, wheel_diameter, max_acceleration_mps2):
+        """
+        max_speed_mps: モーターの最大速度 [m/s]
+        wheel_diameter: ホイールの直径 [m]（モーターが車輪に接続されている場合）
+        max_acceleration_mps2: 最大加速度 [m/s^2]
+        """
+        self.max_speed_mps = max_speed_mps
+        self.wheel_circumference = math.pi * wheel_diameter  # 円周を計算
+        self.max_acceleration_mps2 = max_acceleration_mps2  # 最大加速度
+        self.current_speeds = [0.0] * 8  # 現在の各モーターの速度[m/s]
+        self.bus = can.interface.Bus(channel="can0", interface="socketcan")  # CANインターフェース
+
+        # 初期化: モーターを初期化し、速度制御モードに切り替える
+        self.initialize_motor()
+
+    def initialize_motor(self):
+        self.set_mode_enable()
+        self.set_mode_velocity_control()
+
+    def mps_to_rpm(self, speed_mps):
+        """
+        m/s から RPM (回転数) に変換する
+        """
+        rpm = (speed_mps / self.wheel_circumference) * 60
+        return int(rpm)
+
+    def set_mode_enable(self):
+        # ID 1 to 8 をenableに
+        init_command = [0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A]
+        msg = can.Message(arbitration_id=0x105, data=init_command, is_extended_id=False)
+        try:
+            self.bus.send(msg)
+            time.sleep(1)
+            print("モーターを初期化しました。")
+        except can.CanError as e:
+            print(f"モーター初期化に失敗しました: {e}")
+
+    def set_mode_velocity_control(self):
+        """
+        モーターを速度制御モードに設定する
+        """
+        velocity_control_command = [0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02]
+        msg = can.Message(arbitration_id=0x105, data=velocity_control_command, is_extended_id=False)
+        try:
+            self.bus.send(msg)
+            print("モーターを速度制御モードに設定しました。")
+        except can.CanError as e:
+            print(f"速度制御モード設定に失敗しました: {e}")
+
+    def set_mode_disable(self):
+        """
+        モーターをDisableにする
+        """
+        torque_control_command = [0x09, 0x09, 0x09, 0x09, 0x09, 0x09, 0x09, 0x09]
+        msg = can.Message(arbitration_id=0x105, data=torque_control_command, is_extended_id=False)
+        try:
+            self.bus.send(msg)
+            print("モーターをトルク制御モードに設定しました。")
+        except can.CanError as e:
+            print(f"トルク制御モード設定に失敗しました: {e}")
+
+    def set_speed(self, target_speeds_mps):
+        """
+        速度指令を与える際に加速度制限を加える
+        target_speeds_mps: 各モーターの目標速度 [m/s] の配列
+        """
+        if len(target_speeds_mps) != 8:
+            raise ValueError("速度指令はID 1〜8までの設定を配列で与える必要があります。")
+
+        rpm_list = [0.0] * 8
+
+        for i, target_speed_mps in enumerate(target_speeds_mps):
+            """
+            指定された速度[m/s]をモーターに送信する前に、加速度制限を考慮して速度を調整
+            target_speed_mps: 目標速度 [m/s]
+            """
+            if abs(target_speed_mps) > self.max_speed_mps:
+                raise ValueError("指定された速度が最大速度を超えています！")
+
+            # 加速度制限を考慮して新しい速度を決定
+            speed_diff = target_speed_mps - self.current_speeds[i]
+            max_speed_change = (
+                self.max_acceleration_mps2 * 0.1
+            )  # 加速度制限に基づいた最大速度変化量（0.1秒ごとに制御）
+
+            if abs(speed_diff) > max_speed_change:
+                # 加速度制限を超えないように調整
+                speed_diff = max_speed_change if speed_diff > 0 else -max_speed_change
+
+            # 更新された速度
+            self.current_speeds[i] += speed_diff
+            rpm_list[i] = self.mps_to_rpm(self.current_speeds[i])
+
+        # CAN メッセージを構築して送信
+        speed_command0 = [
+            (rpm_list[0] >> 8) & 0xFF,
+            rpm_list[0] & 0xFF,
+            (rpm_list[1] >> 8) & 0xFF,
+            rpm_list[1] & 0xFF,
+            (rpm_list[2] >> 8) & 0xFF,
+            rpm_list[2] & 0xFF,
+            (rpm_list[3] >> 8) & 0xFF,
+            rpm_list[3] & 0xFF,
+        ]
+        speed_command1 = [
+            (rpm_list[4] >> 8) & 0xFF,
+            rpm_list[4] & 0xFF,
+            (rpm_list[5] >> 8) & 0xFF,
+            rpm_list[5] & 0xFF,
+            (rpm_list[6] >> 8) & 0xFF,
+            rpm_list[6] & 0xFF,
+            (rpm_list[7] >> 8) & 0xFF,
+            rpm_list[7] & 0xFF,
+        ]
+
+        msg0 = can.Message(arbitration_id=0x32, data=speed_command0, is_extended_id=False)
+        msg1 = can.Message(arbitration_id=0x33, data=speed_command1, is_extended_id=False)
+
+        try:
+            self.bus.send(msg0)
+            self.bus.send(msg1)
+        except can.CanError as e:
+            print(f"速度設定に失敗しました: {e}")
+
+    def stop_motor(self):
+        """
+        モーターを停止する
+        """
+        self.set_speed([0, 0, 0, 0, 0, 0, 0, 0])  # 速度を0に設定
+        self.set_mode_disable()  # モーターを無効にする
+        print("モーターを停止しました。")
+
+    def shutdown(self):
+        """
+        CANバスを閉じる
+        """
+        self.bus.shutdown()
+        print("CANバスをシャットダウンしました。")
+
+
+# 使用例:
+# if __name__ == "__main__":
+#     motor_controller = DDTMotorDriver(max_speed_mps=1.6, wheel_diameter=0.185, max_acceleration_mps2=0.5)
+#     motor_controller.set_speed([1.1, -1.1, 0, 0, 0, 0, 0, 0])
+#     time.sleep(3)
+#     motor_controller.stop_motor()    # モーターを停止
+#     motor_controller.shutdown()      # CANバスを閉じる

src/driver/ddt_motor_driver/ddt_motor_driver/diff_drive_controller.py

@@ -0,0 +1,56 @@
+import math
+
+
+class DiffDrive:
+    def __init__(self, wheel_radius, wheel_base):
+        """
+        コンストラクタ
+        wheel_radius: 車輪の半径（m）
+        wheel_base: 左右の車輪間の距離（m）
+        """
+        self.wheel_radius = wheel_radius  # 車輪の半径
+        self.wheel_base = wheel_base  # 車輪間の距離
+
+    def calculate_velocity(self, left_wheel_speed, right_wheel_speed):
+        """
+        左右の車輪速度（m/s）から直線速度と角速度を計算する
+        left_wheel_speed: 左車輪の速度（m/s）
+        right_wheel_speed: 右車輪の速度（m/s）
+        """
+        # 直線速度 (linear velocity)
+        linear_velocity = (left_wheel_speed + right_wheel_speed) / 2
+
+        # 角速度 (angular velocity)
+        angular_velocity = (right_wheel_speed - left_wheel_speed) / self.wheel_base
+
+        return linear_velocity, angular_velocity
+
+    def calculate_wheel_speeds(self, linear_velocity, angular_velocity):
+        """
+        直線速度と角速度から左右の車輪の速度を計算する
+        linear_velocity: 直線速度（m/s）
+        angular_velocity: 角速度（rad/s）
+        """
+        # 左右の車輪の速度 (m/s) を計算
+        left_wheel_speed = linear_velocity - (angular_velocity * self.wheel_base) / 2
+        right_wheel_speed = linear_velocity + (angular_velocity * self.wheel_base) / 2
+
+        return left_wheel_speed, right_wheel_speed
+
+
+# 使用例
+if __name__ == "__main__":
+    # 車輪の半径が0.1m、車輪間の距離が0.5mの場合
+    robot = DiffDrive(wheel_radius=0.1, wheel_base=0.5)
+
+    # 左右の車輪の直線速度 (m/s)
+    left_speed = 1.0  # 左車輪
+    right_speed = 1.5  # 右車輪
+
+    # 直線速度と角速度を計算
+    linear_vel, angular_vel = robot.calculate_velocity(left_speed, right_speed)
+    print(f"直線速度: {linear_vel} m/s, 角速度: {angular_vel} rad/s")
+
+    # 直線速度1.0 m/s, 角速度0.5 rad/sに対して左右の車輪速度を計算
+    left_wheel, right_wheel = robot.calculate_wheel_speeds(linear_vel, angular_vel)
+    print(f"左車輪の速度: {left_wheel} m/s, 右車輪の速度: {right_wheel} m/s")