MG4010I36をPythonで動かす

探してみても、以下のような情報くらいしか出ておらず……肝心のパケットはどこに行ったんだっていう

Open CAN Protocol
1. CAN bus parameters and single motor command send and receive message format
Bus interface: CAN
Baud rate: 1Mbps
The format of message used to send commands and motor replies to a single motor is as follows:
ID: 0x140 + ID(1~32)
Frame format: DATA
Frame type: Standard
DLC: 8 Byte

実際のところ

import can
import struct
import math

class MG4010I36:
    def __init__(self):
        self.MAX_VEL = 1000  # 仮の値、実際の最大速度に応じて調整してください
        self.max_vel = int(self.MAX_VEL * 36.0)  # 減速比36
        self.bus = can.interface.Bus(channel='can0', bustype='socketcan', bitrate=1000000)

    def encode_position_command(self, motor_id, position):
        """位置制御コマンドを生成"""
        position = position / math.pi * 180.0  # ラジアンから度に変換
        int32_position = int(position * 100.0 * 36.0)  # 360度は36000LSBに対応、減速比36を考慮
        
        data = [0xA4, 0x00,
                self.max_vel & 0xFF, (self.max_vel >> 8) & 0xFF,
                int32_position & 0xFF, (int32_position >> 8) & 0xFF,
                (int32_position >> 16) & 0xFF, (int32_position >> 24) & 0xFF]
        
        return can.Message(arbitration_id=0x140 + motor_id, data=data, is_extended_id=False)

    def encode_calibrate_command(self, motor_id):
        """キャリブレーションコマンドを生成"""
        data = [0x19, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]
        return can.Message(arbitration_id=0x140 + motor_id, data=data, is_extended_id=False)

    def encode_disable_command(self, motor_id):
        """無効化コマンドを生成"""
        data = [0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]
        return can.Message(arbitration_id=0x140 + motor_id, data=data, is_extended_id=False)

    def encode_enable_command(self, motor_id):
        """有効化コマンドを生成"""
        data = [0x88, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]
        return can.Message(arbitration_id=0x140 + motor_id, data=data, is_extended_id=False)

    def decode_position_frame(self, frame):
        """位置フレームをデコード"""
        position_raw = (frame.data[7] << 8) | frame.data[6]
        position = float(position_raw) / 16384.0 / 2.0 * math.pi
        return position

    def send_command(self, message):
        """コマンドを送信"""
        self.bus.send(message)

    def receive_message(self, timeout=1.0):
        """メッセージを受信"""
        return self.bus.recv(timeout)

def main():
    motor = MG4010I36()
    motor_id = 1
    target_position = math.pi / 2  # 90度（π/2ラジアン）

    # モーターを有効化
    enable_msg = motor.encode_enable_command(motor_id)
    motor.send_command(enable_msg)

    # 位置制御コマンドを送信
    position_msg = motor.encode_position_command(motor_id, target_position)
    motor.send_command(position_msg)

    print(f"モーターID {motor_id} を位置 {target_position} ラジアンに移動します")

    # 現在位置をモニタリング
    while True:
        msg = motor.receive_message()
        if msg is not None and msg.arbitration_id == 0x240 + motor_id:  # 応答IDは0x240 + motor_idと仮定
            current_position = motor.decode_position_frame(msg)
            print(f"現在位置: {current_position} ラジアン")
            if abs(current_position - target_position) < 0.01:  # 許容誤差
                print("目標位置に到達しました")
                break

    # モーターを無効化
    disable_msg = motor.encode_disable_command(motor_id)
    motor.send_command(disable_msg)

if __name__ == "__main__":
    main()

参考もと

github.com