Initial commit

unilabos/devices/motor/commands/PositionControl.py

@@ -0,0 +1,92 @@
+
+from ..base_types.PrPath import PR_PATH
+from ..Utils import create_command, get_result_byte_int, get_result_byte_str, send_command
+
+
+def create_position_commands(slave_id: int, which: int, how: PR_PATH, 
+                           position: float, velocity: int = 300, 
+                           acc_time: int = 50, dec_time: int = 50, 
+                           special: int = 0) -> list[list[bytes]]:
+    """
+    创建位置相关的Modbus命令列表
+    
+    Args:
+        slave_id: 从站地址
+        which: PR路径号(0-7)
+        how: PR_PATH枚举，定义运动方式
+        position: 目标位置(Pulse)
+        velocity: 运行速度(rpm)
+        acc_time: 加速时间(ms/Krpm)，默认50
+        dec_time: 减速时间(ms/Krpm)，默认50
+        special: 特殊参数，默认0
+    
+    Returns:
+        包含所有设置命令的列表
+    """
+    if not 0 <= which <= 7:
+        raise ValueError("which必须在0到7之间")
+    
+    base_addr = 0x6200 + which * 8
+    
+    # 位置值保持原样（Pulse）
+    position = int(position)
+    print(f"路径方式 {' '.join(bin(how.value)[2:])} 位置 {position} 速度 {velocity}")
+    position_high = (position >> 16) & 0xFFFF  # 获取高16位
+    position_low = position & 0xFFFF           # 获取低16位
+    
+    # 速度值（rpm）转换为0x0000格式
+    velocity_value = 0x0000 + velocity
+    
+    # 加减速时间（ms/Krpm）转换为0x0000格式
+    acc_time_value = 0x0000 + int(acc_time)
+    dec_time_value = 0x0000 + int(dec_time)
+    
+    # 特殊参数转换为0x0000格式
+    special_value = 0x0000 + special
+    return [
+        create_command(slave_id, 0x06, base_addr + 0, how.value),      # 路径方式
+        create_command(slave_id, 0x06, base_addr + 1, position_high),  # 位置高16位
+        create_command(slave_id, 0x06, base_addr + 2, position_low),   # 位置低16位
+        create_command(slave_id, 0x06, base_addr + 3, velocity_value), # 运行速度
+        create_command(slave_id, 0x06, base_addr + 4, acc_time_value), # 加速时间
+        create_command(slave_id, 0x06, base_addr + 5, dec_time_value), # 减速时间
+        create_command(slave_id, 0x06, base_addr + 6, special_value),  # 特殊参数
+    ]
+
+def create_position_run_command(slave_id: int, which: int) -> list[list[bytes]]:
+    print(f"运行路径 PR{which}")
+    return [create_command(slave_id, 0x06, 0x6002, 0x0010 + which)]
+
+def run_set_position_zero(ser, DEVICE_ADDRESS) -> list[list[bytes]]:
+    print(f"手动回零")
+    send_command(ser, create_command(DEVICE_ADDRESS, 0x06, 0x6002, 0x0021))
+
+def run_stop(ser, DEVICE_ADDRESS) -> list[list[bytes]]:
+    print(f"急停")
+    send_command(ser, create_command(DEVICE_ADDRESS, 0x06, 0x6002, 0x0040))
+
+def run_set_forward_run(ser, DEVICE_ADDRESS) -> list[list[bytes]]:
+    print(f"设定正方向运动")
+    send_command(ser, create_command(DEVICE_ADDRESS, 0x06, 0x0007, 0x0000))
+
+def run_set_backward_run(ser, DEVICE_ADDRESS) -> list[list[bytes]]:
+    print(f"设定反方向运动")
+    send_command(ser, create_command(DEVICE_ADDRESS, 0x06, 0x0007, 0x0001))
+
+def run_get_command_position(ser, DEVICE_ADDRESS, print_pos=True) -> int: 
+    retH = send_command(ser, create_command(DEVICE_ADDRESS, 0x03, 0x602A, 0x0001))  # 命令位置H
+    retL = send_command(ser, create_command(DEVICE_ADDRESS, 0x03, 0x602B, 0x0001))  # 命令位置L
+    value = get_result_byte_str(retH) + get_result_byte_str(retL)
+    value = int(value, 16)
+    if print_pos:
+        print(f"命令位置: {value}")
+    return value
+
+def run_get_motor_position(ser, DEVICE_ADDRESS, print_pos=True) -> int:
+    retH = send_command(ser, create_command(DEVICE_ADDRESS, 0x03, 0x602C, 0x0001))  # 电机位置H
+    retL = send_command(ser, create_command(DEVICE_ADDRESS, 0x03, 0x602D, 0x0001))  # 电机位置L
+    value = get_result_byte_str(retH) + get_result_byte_str(retL)
+    value = int(value, 16)
+    if print_pos:
+        print(f"电机位置: {value}")
+    return value

unilabos/devices/motor/commands/Status.py

@@ -0,0 +1,44 @@
+import time
+from ..Utils import create_command, send_command
+from .PositionControl import run_get_motor_position
+
+
+def run_get_status(ser, DEVICE_ADDRESS, print_status=True) -> list[list[bytes]]:
+    # Bit0 故障 
+    # Bit1 使能 
+    # Bit2 运行 
+    # Bit4 指令完成 
+    # Bit5 路径完成 
+    # Bit6 回零完成
+    ret = send_command(ser, create_command(DEVICE_ADDRESS, 0x03, 0x1003, 0x0001))
+    status = bin(int(ret.hex()[6:10], 16))[2:]
+    # 用0左位补齐
+    status = status.zfill(8)
+    status_info_list = []
+    if status[-1] == "1":
+        status_info_list.append("故障")
+    if status[-2] == "1":
+        status_info_list.append("使能")
+    if status[-3] == "1":
+        status_info_list.append("运行")
+    if status[-5] == "1":
+        status_info_list.append("指令完成")
+    if status[-6] == "1":
+        status_info_list.append("路径完成")
+    if status[-7] == "1":
+        status_info_list.append("回零完成")
+    if print_status:
+        print(f"获取状态: {' '.join(status_info_list)}")
+    return status_info_list
+
+def run_until_status(ser, DEVICE_ADDRESS, status_info: str, max_time=10):
+    start_time = time.time()
+    while True:
+        ret = run_get_status(ser, DEVICE_ADDRESS)
+        if status_info in ret:
+            break
+        if time.time() - start_time > max_time:
+            print(f"状态未达到 {status_info} 超时")
+            return False
+        time.sleep(0.05)
+    return True

unilabos/devices/motor/commands/Warning.py

@@ -0,0 +1,12 @@
+from serial import Serial
+from ..Consts import Config
+from ..Utils import create_command, send_command
+
+
+def create_elimate_warning_command(DEVICE_ADDRESS):
+    return create_command(DEVICE_ADDRESS, 0x06, 0x0145, 0x0087)
+
+
+def run_elimate_warning(ser: Serial, DEVICE_ADDRESS):
+    send_command(ser, create_elimate_warning_command(DEVICE_ADDRESS))
+    print("清除警报")