105 lines
2.9 KiB
Python
105 lines
2.9 KiB
Python
#!/usr/bin/env python
|
||
# -*- coding: utf-8 -*-
|
||
'''
|
||
# @Time : 2025/7/15 17:52
|
||
# @Author : hx
|
||
# @File : motor.py
|
||
'''
|
||
|
||
import time
|
||
|
||
start_speed = 8
|
||
stop_speed = 0
|
||
|
||
# 细分值映射表
|
||
MICROSTEP_MAP = {
|
||
2: 0x01,
|
||
4: 0x02,
|
||
8: 0x04,
|
||
16: 0x10,
|
||
32: 0x20
|
||
}
|
||
|
||
def generate_speed_command(direction=1, microstep=32, speed=10):
|
||
"""
|
||
生成速度控制模式的 CAN 数据帧（7字节）
|
||
:param direction: 0=逆时针，1=顺时针
|
||
:param microstep: 细分值（2, 4, 8, 16, 32）
|
||
:param speed: 速度值（Rad/s）
|
||
:return: 7字节 CAN 数据帧
|
||
"""
|
||
control_mode = 0x01 # 速度控制模式
|
||
direction_byte = 0x01 if direction == 1 else 0x00
|
||
microstep_byte = MICROSTEP_MAP.get(microstep, 0x20)
|
||
|
||
raw_speed = int(speed * 10)
|
||
speed_high = (raw_speed >> 8) & 0xFF
|
||
speed_low = raw_speed & 0xFF
|
||
|
||
return [control_mode, direction_byte, microstep_byte, 0x00, 0x00, speed_high, speed_low]
|
||
|
||
|
||
def generate_position_command(angle, speed=10, direction=1, microstep=32):
|
||
"""
|
||
生成位置控制模式的 CAN 数据帧（7字节）
|
||
:param angle: 目标角度（度）
|
||
:param speed: 速度值（Rad/s）
|
||
:param direction: 0=逆时针，1=顺时针
|
||
:param microstep: 细分值（2, 4, 8, 16, 32）
|
||
:return: 7字节 CAN 数据帧
|
||
"""
|
||
control_mode = 0x02 # 位置控制模式
|
||
direction_byte = 0x01 if direction == 1 else 0x00
|
||
microstep_byte = MICROSTEP_MAP.get(microstep, 0x20)
|
||
|
||
raw_angle = int(angle * 10)
|
||
pos_high = (raw_angle >> 8) & 0xFF
|
||
pos_low = raw_angle & 0xFF
|
||
|
||
raw_speed = int(speed * 10)
|
||
speed_high = (raw_speed >> 8) & 0xFF
|
||
speed_low = raw_speed & 0xFF
|
||
|
||
return [control_mode, direction_byte, microstep_byte, pos_high, pos_low, speed_high, speed_low]
|
||
|
||
|
||
def send_can_frame(data, can_id=0x01):
|
||
"""
|
||
模拟发送CAN帧（只打印，不实际发送）
|
||
"""
|
||
print(f"[发送CAN帧] ID={can_id:02X} 数据=[{', '.join(f'0x{x:02X}' for x in data)}]")
|
||
|
||
|
||
def listen_and_respond():
|
||
"""
|
||
模拟监听并响应信号的逻辑
|
||
"""
|
||
try:
|
||
print("[监听中] 等待信号...")
|
||
|
||
# 模拟收到信号后执行动作
|
||
time.sleep(2)
|
||
|
||
# 示例：位置控制模式 - 顺时针转动 1000 度，速度 10Rad/s，32细分
|
||
pos_data = generate_position_command(angle=1000, speed=10, direction=1, microstep=32)
|
||
send_can_frame(pos_data, can_id=0x01)
|
||
|
||
time.sleep(2)
|
||
|
||
# 示例：速度控制模式 - 顺时针，速度 8Rad/s，32细分
|
||
speed_data = generate_speed_command(direction=1, microstep=32, speed=8)
|
||
send_can_frame(speed_data, can_id=0x01)
|
||
|
||
print("[已完成一次响应]")
|
||
|
||
except Exception as e:
|
||
print(f"监听时发生错误: {e}")
|
||
|
||
|
||
if __name__ == "__main__":
|
||
try:
|
||
# 示例：执行一次电机控制流程
|
||
listen_and_respond()
|
||
|
||
except Exception as e:
|
||
print(f"运行错误: {e}") |