""" 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("测试结束")