213 lines
7.3 KiB
Python
213 lines
7.3 KiB
Python
#!/usr/bin/env python
|
||
# -*- coding: utf-8 -*-
|
||
'''
|
||
# @Time : 2025/7/18
|
||
# @Author : hx
|
||
# @File : motor_control_modes.py
|
||
'''
|
||
|
||
import serial
|
||
import time
|
||
import argparse
|
||
|
||
|
||
# ========================================
|
||
# 通用发送函数
|
||
# ========================================
|
||
def send_hex_command(port, baudrate, hex_data):
|
||
"""
|
||
发送十六进制指令
|
||
"""
|
||
try:
|
||
byte_data = bytes.fromhex(hex_data.replace(" ", ""))
|
||
with serial.Serial(port, baudrate, timeout=1) as ser:
|
||
print(f"已连接串口 {port}, 波特率 {baudrate}")
|
||
ser.write(byte_data)
|
||
print(f"已发送指令: {hex_data}")
|
||
time.sleep(0.1)
|
||
if ser.in_waiting > 0:
|
||
response = ser.read(ser.in_waiting)
|
||
print(f"收到响应: {response.hex().upper()}")
|
||
except Exception as e:
|
||
print(f"发送失败: {e}")
|
||
|
||
|
||
# ========================================
|
||
# 模式1：速度控制模式 (0x01)
|
||
# 控制字节3 = 0x01
|
||
# 第6~7字节为0
|
||
# 第8~9字节：速度值（单位 rad/s，放大10倍）
|
||
# ========================================
|
||
def generate_speed_command(direction, microstep, speed):
|
||
"""
|
||
生成速度控制指令
|
||
:param direction: 方向 (0:逆时针, 1:顺时针)
|
||
:param microstep: 细分值 (2, 4, 8, 16, 32)
|
||
:param speed: 速度值 (单位: rad/s)
|
||
"""
|
||
command = [0x7B, 0x01, 0x01, direction, microstep, 0x00, 0x00]
|
||
|
||
# 转速数据，单位为 rad/s，放大10倍传输
|
||
raw_speed = int(speed * 10)
|
||
high_byte = (raw_speed >> 8) & 0xFF
|
||
low_byte = raw_speed & 0xFF
|
||
command += [high_byte, low_byte]
|
||
|
||
# 计算 BCC 校验和（前9个字节异或）
|
||
checksum = 0
|
||
for b in command:
|
||
checksum ^= b
|
||
|
||
full_command = command + [checksum, 0x7D]
|
||
return ' '.join(f'{b:02X}' for b in full_command)
|
||
|
||
|
||
# ========================================
|
||
# 模式2：位置控制模式 (0x02)
|
||
# 控制字节3 = 0x02
|
||
# 第6~7字节：角度值（单位 度，放大10倍）
|
||
# 第8~9字节为0
|
||
# ========================================
|
||
def generate_position_command(direction, microstep, angle):
|
||
"""
|
||
生成位置控制指令
|
||
:param direction: 方向 (0:逆时针, 1:顺时针)
|
||
:param microstep: 细分值 (2, 4, 8, 16, 32)
|
||
:param angle: 角度值 (单位: 度)
|
||
"""
|
||
command = [0x7B, 0x01, 0x02, direction, microstep]
|
||
|
||
# 角度数据，单位为度，放大10倍传输
|
||
raw_angle = int(angle * 10)
|
||
high_byte = (raw_angle >> 8) & 0xFF
|
||
low_byte = raw_angle & 0xFF
|
||
command += [low_byte, high_byte, 0x00, 0x64]
|
||
|
||
# 计算 BCC 校验和（前9个字节异或）
|
||
checksum = 0
|
||
for b in command:
|
||
checksum ^= b
|
||
|
||
full_command = command + [checksum, 0x7D]
|
||
return ' '.join(f'{b:02X}' for b in full_command)
|
||
|
||
|
||
# ========================================
|
||
# 模式3：力矩控制模式 (0x03)
|
||
# 控制字节3 = 0x03
|
||
# 第6~7字节：电流值（单位 mA）
|
||
# 第8~9字节为0
|
||
# ========================================
|
||
def generate_torque_command(direction, microstep, current):
|
||
"""
|
||
生成力矩控制指令
|
||
:param direction: 方向 (0:逆时针, 1:顺时针)
|
||
:param microstep: 细分值 (2, 4, 8, 16, 32)
|
||
:param current: 电流值 (单位: mA)
|
||
"""
|
||
command = [0x7B, 0x01, 0x03, direction, microstep]
|
||
|
||
# 电流数据，单位为 mA
|
||
raw_current = int(current)
|
||
high_byte = (raw_current >> 8) & 0xFF
|
||
low_byte = raw_current & 0xFF
|
||
command += [low_byte, high_byte, 0x00, 0x64]
|
||
|
||
# 计算 BCC 校验和（前9个字节异或）
|
||
checksum = 0
|
||
for b in command:
|
||
checksum ^= b
|
||
|
||
full_command = command + [checksum, 0x7D]
|
||
return ' '.join(f'{b:02X}' for b in full_command)
|
||
|
||
|
||
# ========================================
|
||
# 模式4：单圈绝对角度控制模式 (0x04)
|
||
# 控制字节3 = 0x04
|
||
# 第6~7字节：目标角度（单位 度，放大10倍）
|
||
# 第8~9字节为0
|
||
# ========================================
|
||
def generate_absolute_angle_command(direction, microstep, target_angle, speed_rad_per_sec):
|
||
"""
|
||
生成单圈绝对角度控制指令
|
||
:param direction: 方向 (0:逆时针, 1:顺时针)
|
||
:param microstep: 细分值 (2, 4, 8, 16, 32)
|
||
:param target_angle: 目标角度 (单位: 度)
|
||
:param speed_rad_per_sec: 速度 (单位: rad/s)
|
||
:return: 十六进制指令字符串
|
||
"""
|
||
command = [0x7B, 0x01, 0x04, direction, microstep]
|
||
|
||
# 目标角度（放大10倍）
|
||
raw_angle = int(target_angle * 10)
|
||
pos_high = (raw_angle >> 8) & 0xFF
|
||
pos_low = raw_angle & 0xFF
|
||
command += [pos_high, pos_low]
|
||
|
||
# 速度（放大10倍）
|
||
raw_speed = int(speed_rad_per_sec * 10)
|
||
speed_high = (raw_speed >> 8) & 0xFF
|
||
speed_low = raw_speed & 0xFF
|
||
command += [speed_high, speed_low]
|
||
|
||
# 计算 BCC 校验和（前9个字节异或）
|
||
checksum = 0
|
||
for b in command:
|
||
checksum ^= b
|
||
|
||
full_command = command + [checksum, 0x7D]
|
||
return ' '.join(f'{b:02X}' for b in full_command)
|
||
|
||
|
||
# ======================================1
|
||
# 主函数（命令行调用）
|
||
# ========================================
|
||
def main():
|
||
parser = argparse.ArgumentParser(description="电机控制程序，支持4种模式")
|
||
parser.add_argument("--port", default="/dev/ttyACM0", help="串口号")
|
||
parser.add_argument("--baud", type=int, default=115200, help="波特率")
|
||
parser.add_argument("--mode", type=int, choices=[1, 2, 3, 4],
|
||
help="控制模式: 1=速度, 2=位置, 3=力矩, 4=绝对角度")
|
||
parser.add_argument("--direction", type=int, choices=[0, 1], default=1,
|
||
help="方向: 0=逆时针, 1=顺时针")
|
||
parser.add_argument("--microstep", type=int, choices=[2, 4, 8, 16, 32], default=32,
|
||
help="细分值: 2, 4, 8, 16, 32")
|
||
parser.add_argument("--value", type=float, required=True,
|
||
help="控制值（速度: rad/s, 位置/角度: 度, 力矩: mA）")
|
||
parser.add_argument("--speed_rad_per_sec", type=float, required=True,
|
||
help="控制值（速度）")
|
||
args = parser.parse_args()
|
||
|
||
try:
|
||
if args.mode == 1:
|
||
cmd = generate_speed_command(args.direction, args.microstep, args.value)
|
||
elif args.mode == 2:
|
||
cmd = generate_position_command(args.direction, args.microstep, args.value)
|
||
elif args.mode == 3:
|
||
cmd = generate_torque_command(args.direction, args.microstep, args.value)
|
||
elif args.mode == 4:
|
||
cmd = generate_absolute_angle_command(args.direction, args.microstep, args.value,args.speed_rad_per_sec)
|
||
|
||
print(f"生成指令: {cmd}")
|
||
send_hex_command(args.port, args.baud, cmd)
|
||
print("指令发送完成。")
|
||
|
||
except Exception as e:
|
||
print(f"错误: {e}")
|
||
|
||
|
||
# ========================================
|
||
# 程序入口
|
||
# ========================================
|
||
if __name__ == "__main__":
|
||
main()
|
||
|
||
#python motor_control_usb_all.py --mode 1 --direction 1 --microstep 32 --value 10 --speed_rad_per_sec=10
|
||
|
||
#python motor_control_usb_all.py --mode 2 --direction 1 --microstep 32 --value 360.0 --speed_rad_per_sec=10
|
||
|
||
#python motor_control_usb_all.py --mode 3 --direction 1 --microstep 32 --value 1000 --speed_rad_per_sec=10
|
||
|
||
#python motor_control_usb_all.py --mode 4 --direction 1 --microstep 32 --value 100.0 --speed_rad_per_sec=10
|