添加现有代码V1.6

This commit is contained in:
2026-06-20 15:44:09 +08:00
commit 3c92f6c285
107 changed files with 15211 additions and 0 deletions
+155
View File
@@ -0,0 +1,155 @@
"""
MWMotor轮询模式核心功能测试
测试使用轮询模式实现基本的电机控制功能
"""
import pyb
import time
import math as Math
from mwmotor import MWMotor, POLL_MODE, VELOCITY_CONTROL, RAMP_RATE_INPUT, AXIS_STATE_CLOSED_LOOP_CONTROL, POSITION_CONTROL, TRAPEZOIDAL_CURVE_INPUT
mw_pwr_pin = pyb.Pin(pyb.Pin.cpu.C12, pyb.Pin.OUT_PP)
origin_pos = 0
# 1. 初始化CAN总线 - 按照FDCAN HAL参数配置
print("步骤1: 初始化CAN总线...")
# FDCAN参数: NominalPrescaler=12, NominalTimeSeg1=3, NominalTimeSeg2=3, SyncJumpWidth=1
# 对应波特率计算: 48MHz / (12 * (1 + 3 + 3)) = 500kHz
__can = pyb.CAN(2, pyb.CAN.NORMAL, baudrate=500000, sample_point = 80) # 精确匹配FDCAN HAL参数
# 设置CAN滤波器:接收ID范围0x00000000到0x000007FF
__can.setfilter(0, pyb.CAN.RANGE, 0, (0x0000, 0x07FF)) # 标准帧滤波器
# 2. 创建电机对象
__motor = MWMotor(__can, 1) # bus_id=1, node_id=0
print("✓ 电机对象创建成功")
def motor_poweron():
mw_pwr_pin.low()
def motor_poweroff():
mw_pwr_pin.high()
def motor_init():
motor_poweron()
time.sleep_ms(1000)
# 3. 设置为轮询模式
__motor.set_poll_mode(POLL_MODE)
print(f"✓ 设置为轮询模式: {__motor.get_poll_mode() == 1}")
# 4. 设置模式为斜坡速度控制模式
print("设置控制器模式为斜坡速度控制...")
__motor.set_controller_mode(VELOCITY_CONTROL, RAMP_RATE_INPUT)
count = __motor.poll()
print(f"✓ 处理了 {count} 条CAN消息")
# 5. 进入闭环控制状态
print("设置电机状态为闭环控制...")
__motor.set_axis_state(AXIS_STATE_CLOSED_LOOP_CONTROL)
count = __motor.poll()
print(f"✓ 处理了 {count} 条CAN消息")
def motor_stop():
global speed
speed = 0
# 8. 输入控制速度
print(f"设置电机速度为{speed} rad/s...")
__motor.vel_control(speed, 0.0) # 速度10 rad/s, 转矩前馈0
#用户单位mm/s
def __speed_convert_user2device(spd):
return spd *9.67/60 * 3 / Math.pi
def __speed_convert_device2user(spd):
return spd *60 /9.67 * Math.pi / 3
#用户单位mm
def __pos_convert_user2device(pos):
return pos * 9.67 / 20 / Math.pi
def __pos_convert_device2user(pos):
return pos / 9.67 * 20 * Math.pi
def motor_speed_set(spd):
print(f"设置电机速度为{__speed_convert_user2device(spd)} rad/s...")
__motor.vel_control(__speed_convert_user2device(-spd), 1.0) # 速度10 rad/s, 转矩前馈0
def motor_pos_set(pos):
mpos = __pos_convert_user2device(-pos) + origin_pos
__motor.set_controller_mode(POSITION_CONTROL, TRAPEZOIDAL_CURVE_INPUT)
__motor.set_traj_vel_limit(__speed_convert_user2device(20))
__motor.set_traj_accel_limits(10, 10)
__motor.pos_control(mpos, 0, 0)
def motor_set_mode_speed():
__motor.set_traj_vel_limit(__speed_convert_user2device(40))
__motor.set_traj_accel_limits(100, 100)
__motor.set_controller_mode(VELOCITY_CONTROL, RAMP_RATE_INPUT)
motor_speed_set(0)
def motor_poll():
count = __motor.poll()
if count > 0:
try:
pos, vel = __motor.get_encoder_estimates()
motor_speed = __speed_convert_device2user(vel)
motor_pos = __pos_convert_device2user(pos - origin_pos)
return (-motor_pos, -motor_speed)
except:
print("读取电机状态失败")
return (None, None)
def is_at_origin():
pos_estimate, vel = __motor.get_encoder_estimates()
motor_poll()
for i in range(3):
__motor.estop()
time.sleep_ms(50)
__motor.clear_errors()
time.sleep_ms(5)
__motor.set_axis_state(AXIS_STATE_CLOSED_LOOP_CONTROL)
time.sleep_ms(5)
__motor.set_axis_state(AXIS_STATE_CLOSED_LOOP_CONTROL)
time.sleep_ms(5)
__motor.set_axis_state(AXIS_STATE_CLOSED_LOOP_CONTROL)
time.sleep_ms(10)
new_pos_estimate, _ = __motor.get_encoder_estimates()
motor_poll()
if abs(new_pos_estimate - pos_estimate) < 0.05:
# print(new_pos_estimate, pos_estimate, new_pos_estimate - pos_estimate)
return True
else:
# print(False, new_pos_estimate, pos_estimate, new_pos_estimate - pos_estimate)
return False
def move_to_origin():
global origin_pos
tim = time.time()
while not is_at_origin():
if time.time() - tim > 10:
print("Move to bottom timeout")
False
origin_pos, _ = __motor.get_encoder_estimates()
print("origin_pos", origin_pos)
return True
# 运行测试
if __name__ == "__main__":
motor_init()
try:
while True:
motor_poll()
time.sleep_ms(10)
# lw_check()
except KeyboardInterrupt:
motor_speed_set(0)
print("测试结束")