添加现有代码V1.6
This commit is contained in:
@@ -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("测试结束")
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user