（1）转盘步进电机控制-pymodbus
硬件设备：
MKS SERVO42D/57D_RS485 闭环步进电机
接线：
模式：串口开环控制。因负载转动惯量过大，闭环不稳定。
功能：限位归零
绝对位置控制
补充：主动轮15齿，从动轮：40齿
import time import ctypes import math from doctest import register_optionflag from pymodbus.client.sync import ModbusSerialClient from threading import Thread, Lock # 导入自己的类 import log logger = log.logger_init() # 代码 class StepMotorRTU: """MKS SERVO42D/57D_RS485 闭环步进电机 设置 逆时针为电机正方向。 01 06 00 86 00 01 A9 E3 4. 复位重启电机指令 01 06 00 41 00 01 18 1E 6. 设置工作模式 （对应屏幕上的“Mode”选项）mode = 03 SR_OPEN (串行接口开环模式) 01 06 00 82 00 03 69 E3 8.3.1 紧急停止指令 01 06 00 F7 00 01 F9 F8 8.3.2 执行限位归零 01 06 00 91 00 01 19 E7 2. 解除堵转保护 01 06 00 3D 00 01 D9 C6 21. 设置限位回零参数 01 10 00 90 00 03 06 00 00 00 01 00 0A 31 3A 8.3.7 位置控制模式 4 按坐标值绝对运动 01 10 00 F5 00 04 08 00 37 00 0C 00 00 AA AA 51 6A 3. 读取所有状态参数指令 01 04 12 48 00 0E F4 A0 """ # 创建一个类级别的静态锁 port_lock = Lock() # with MotorRTU.port_lock #类级别的静态锁 def __init__(self, x_port='COM3', x_baud=115200, x_parity='N', x_stop_bits=2, x_byte_size=8): # 配置串行通信参数 self.client = ModbusSerialClient( method='rtu', # 使用RTU模式 port=x_port, # 串行端口，根据你的设备连接情况修改 baudrate=x_baud, # 波特率，与你的设备设置相匹配 timeout=0.05, # 超时时间（秒） parity=x_parity, # 校验位：无（N），奇（O），偶（E） stopbits=x_stop_bits, # 停止位：1 bytesize=x_byte_size # 数据位：8 ) self.reg_list = list() self.status_dict = { 0: "查询失败", 1: "电机停止运行", 2: "电机加速运行", 3: "电机减速运行", 4: "电机全速运行", 5: "电机归零运行", 6: "电机校准运行" } # 在Modbus协议中，功能码0x04（十六进制表示）用于读取输入寄存器（Input Registers） def read_04(self, reg_address=0x0046, reg_count=3, slave_address=1): # 清空，准备放新的结果 self.reg_list.clear() ret = False # 连接logger.info到Modbus RTU服务器 if self.client.connect(): # logger.info("成功连接通信端口") # 读取输入寄存器 address = reg_address # 寄存器起始地址（从0开始） count = reg_count # 要读取的寄存器数量 with StepMotorRTU.port_lock: # 类级别的静态锁 result = self.client.read_input_registers(address=address, count=count, unit=slave_address) # 功能码号 04 # 检查读取是否成功 if not result.isError(): # logger.info("读取成功") self.reg_list.clear() for register in result.registers: self.reg_list.append(register) # logger.info(f"寄存器值: {register}") ret = True else: logger.info("Modbus-RTU fun 04读取失败") # 关闭连接 self.client.close() else: logger.info("无法连接到Modbus RTU服务器") # messagebox.showerror("错误提示", "没有通信连接，检查通信线路.") return ret # 在Modbus协议中，功能码0x06（十六进制表示）用于预置单个寄存器（Preset Single Register） def write_06(self, reg_address=0x000E, reg_value=0x0001, slave_address=1): # 要写入的值（注意：Modbus寄存器通常存储16位值） value = reg_value # 示例值，确保它在0到65535的范围内 # 将值转换为两个字节（Modbus寄存器是16位的） value_bytes = value.to_bytes(2, byteorder='big') # 大端序，根据设备文档可能需要调整 # 地址（注意：地址可能因设备而异） address = reg_address # 示例地址 ret = False try: # 连接到Modbus服务器（从设备） if self.client.connect(): with StepMotorRTU.port_lock: # 类级别的静态锁 # 预置单个寄存器（功能码0x06） # 注意：pymodbus的write_register方法内部处理了功能码0x06的调用 result = self.client.write_register(address, int.from_bytes(value_bytes, byteorder='big'), unit=slave_address) # 检查写入是否成功 if result.isError(): logger.error("写入失败，错误代码：", result) else: # logger.info("写入成功") ret = True else: logger.info("无法连接到Modbus服务器") # messagebox.showerror("错误提示", "没有通信连接，检查通信线路.") except Exception as e: logger.error("发生异常：", e) finally: # 关闭连接 self.client.close() # logger.info("已关闭连接") return ret # 在Modbus协议中，功能码0x10（十六进制表示）用于写多个个寄存器 def write_10(self, reg_address=0x00F0, reg_list=None, slave_address=1): # 要写入的数据（这里以两个16位寄存器为例，每个寄存器存储一个无符号整数） # 要写入的数据（这里以两个16位寄存器为例，每个寄存器存储一个无符号整数） if reg_list is None: reg_list = [0x0000, 0x0000] values = reg_list # 示例值 ret = False # 尝试连接到Modbus服务器（从设备）并写入数据 try: if self.client.connect(): with StepMotorRTU.port_lock: # 类级别的静态锁 # 写入多个保持寄存器（功能码10H） # 注意：unit参数对应于从设备的地址或ID，根据设备配置设置 # 直接将值列表传递给write_registers方法 result = self.client.write_registers(address=reg_address, values=values, unit=slave_address) # 检查写入是否成功 if result.isError(): logger.error(f"写入失败，错误代码：{result}") else: # logger.info("写入成功") ret = True else: logger.error("无法连接到Modbus服务器") # messagebox.showerror("错误提示", "没有通信连接，检查通信线路.") except Exception as e: logger.error("发生异常：", e) finally: # 关闭连接 self.client.close() # logger.info("已关闭连接") return ret def read_driver_status(self, slave_address=1): """4. 复位重启电机指令 01 06 00 41 00 01 18 1E 查询失败， 返回 FB 01 F1 00 ED； 电机停止运行，返回 FB 01 F1 01 EE； 电机加速运行，返回 FB 01 F1 02 EF； 电机减速运行，返回 FB 01 F1 03 F0； 电机全速运行，返回 FB 01 F1 04 F1； 电机归零运行，返回 FB 01 F1 05 F2； 电机校准运行，返回 FB 01 F1 06 F3； """ self.reg_list.clear() # 删除一个列表的全部元素 self.read_04(reg_address=0x1248, reg_count=14, slave_address=slave_address) status = dict() try: status["status"] = self.reg_list[0] >> 8 status["io"] = self.reg_list[0] & 0x00FF status["pulse_encoder"] = self.reg_list[1] * 0x10000 + self.reg_list[2] * 0x100 + self.reg_list[3] # 编码器值 status["rpm"] = self.reg_list[4] status["pulse_input"] = self.reg_list[5] * 0x100 + self.reg_list[6] # 脉冲数 status["pulse_encoder_original"] = self.reg_list[7] * 0x100000000 + self.reg_list[8] * 0x10000 + \ self.reg_list[9] # 原始编码器值 status["pulse_err"] = (self.reg_list[10] * 0x10000 + self.reg_list[11]) / 51200 * 360 # 角度误差 ° status["enable"] = self.reg_list[12] >> 8 # 使能状态 status["rtz_power_up"] = self.reg_list[12] & 0xFF # 上电回零 status["stall"] = self.reg_list[13] >> 8 # 使能状态 print(status) print(hex(status["pulse_encoder"])) except IndexError as err: logger.error(err) def driver_is_enable(self, slave_address=1): self.reg_list.clear() # 删除一个列表的全部元素 self.read_04(reg_address=0x003a, reg_count=14, slave_address=slave_address) try: if self.reg_list[0] == 1: return True except IndexError as err: logger.error(err) return False def driver_is_stall(self, slave_address=1): self.reg_list.clear() # 删除一个列表的全部元素 self.read_04(reg_address=0x003E, reg_count=14, slave_address=slave_address) try: if self.reg_list[0] == 1: return True except IndexError as err: logger.error(err) return False def motor_status(self, slave_address=1, fake_status=0): self.reg_list.clear() # 删除一个列表的全部元素 self.read_04(reg_address=0x00F1, reg_count=14, slave_address=slave_address) try: status = self.reg_list[0] self.reg_list.clear() except IndexError as err: status = fake_status return status # 软件复位电机驱动器 def restart_driver(self, slave_address=1): """4. 复位重启电机指令 01 06 00 41 00 01 18 1E""" self.write_06(reg_address=0x0041, reg_value=0x0001, slave_address=slave_address) def configure_driver(self, slave_address=1): """ 11. 设置电机正方向 （对应屏幕上的“Dir”选项） dir = 00 对应顺时针旋转 dir = 01 对应逆时针旋转 01 06 00 86 00 01 A9 E3 6. 设置工作模式 （对应屏幕上的“Mode”选项）mode = 03 SR_OPEN (串行接口开环模式) 01 06 00 82 00 03 69 E3 9. 设置细分 256 21. 设置限位回零参数 01 10 00 90 00 03 06 00 00 00 01 00 0A 31 3A """ _rtu_id = slave_address _servo_modbus_rtu = self _servo_modbus_rtu.write_06(reg_address=0x0086, reg_value=0x0001, slave_address=_rtu_id) # 逆时针为电机正方向 _servo_modbus_rtu.write_06(reg_address=0x0082, reg_value=0x0003, slave_address=_rtu_id) # 工作模式 _servo_modbus_rtu.write_06(reg_address=0x0084, reg_value=64, slave_address=_rtu_id) # 电流细分 reg_list = list() reg_list.append(0x0001) # 高8位：限位开关 有效输入 0 低电平 低8位：归零方向 1 顺时针转动 0 逆时针转动 reg_list.append(240) # 速度 rpm reg_list.append(0x0000) # 是否松轴使用 _servo_modbus_rtu.write_10(reg_address=0x0090, reg_list=reg_list, slave_address=_rtu_id) def return_zero(self, slave_address=1): """8.3.2 执行限位归零 01 06 00 91 00 01 19 E7""" _rtu_id = slave_address _servo_modbus_rtu = self _servo_modbus_rtu.write_06(reg_address=0x0091, reg_value=1, slave_address=_rtu_id) def enable_driver(self, slave_address=1, enable=1): """2. 20. 设置串行模式电机使能""" _rtu_id = slave_address _servo_modbus_rtu = self _servo_modbus_rtu.write_06(reg_address=0x00F3, reg_value=enable, slave_address=_rtu_id) def release_stall(self, slave_address=1): """2. 解除堵转保护 01 06 00 3D 00 01 D9 C6""" _rtu_id = slave_address _servo_modbus_rtu = self _servo_modbus_rtu.write_06(reg_address=0x003d, reg_value=1, slave_address=_rtu_id) def trapezoid_absolute_pulse(self, circles=0.0, rpm=12, slave_address=1, acc=55): """ 8.3.7 位置控制模式 4 按坐标值绝对运动 hex(16384) = '0x4000' 01 10 00 F5 00 04 08 00 37 00 0C 00 00 AA AA 51 6A""" _rtu_id = slave_address _servo_modbus_rtu = self reg_list = list() reg_list.append(acc) # 加速度 reg_list.append(rpm) # 转速 # absAxis(int32_t) 绝对坐标，取值范围（-2147483647，+2147483647） angle2pulses = int(circles *3200*4)# 0x4000) angle2pulses = StepMotorRTU.get_32bit_twos_complement_ctypes(angle2pulses) logger.info(hex(angle2pulses)) reg_list.append(angle2pulses >> 16) # 速度 高位 reg_list.append(angle2pulses & 0xFFFF) # 速度 低位 _servo_modbus_rtu.write_10(reg_address=0x00fE, reg_list=reg_list, slave_address=_rtu_id) @staticmethod def get_32bit_twos_complement_ctypes(n): """ 32位整数的补码""" # 转换为 32 位有符号整数 n_32bit = ctypes.c_int32(n).value # 计算补码（无符号形式） complement = n_32bit & 0xFFFFFFFF # print(hex(complement)) return complement @staticmethod def get_16bit_twos_complement_ctypes(n): """ 16位整数的补码""" # 转换为 16 位有符号整数 n_16bit = ctypes.c_int16(n).value # 计算补码（无符号形式） complement = n_16bit & 0xFFFF # print(hex(complement)) return complement if __name__ == "__main__": # 使用串口 类MotorRTU，通信 step_modbus_rtu = StepMotorRTU(x_port='COM6', x_baud=9600, x_parity='N', x_stop_bits=1, x_byte_size=8) step_modbus_rtu.configure_driver(slave_address=6) #step_modbus_rtu.restart_driver(slave_address=2) # 执行限位归零，并循环检查是否结束 step_modbus_rtu.return_zero(slave_address=6) while True: ret = step_modbus_rtu.motor_status(slave_address=6,fake_status=6) # logger.info(step_modbus_rtu.status_dict[ret]) if ret == 1: break if ret not in [5, 10]: # 驱动器内部发生错误，提示重新启动 print(ret) break #执行位置控制，并循环检查是否结束 step_modbus_rtu.trapezoid_absolute_pulse(circles= 8/3.0* 400.0 / 360.0*100, rpm=600, acc=200,slave_address=6)#执行绝对脉冲指令 #step_modbus_rtu.trapezoid_absolute_pulse(circles= 0, rpm=0, acc=200,slave_address=6)#立即停止当前绝对脉冲指令，实现切换 while True: ret = step_modbus_rtu.motor_status(slave_address=6,fake_status=10) # logger.info(step_modbus_rtu.status_dict[ret]) if ret == 1: break if ret not in [2, 3, 4, 10]: # 驱动器内部发生错误，提示重新启动 break step_modbus_rtu.trapezoid_absolute_pulse(circles= 8/3.0* 180.0 / 360.0*100, rpm=600, acc=200,slave_address=6)#执行绝对脉冲指令 step_modbus_rtu.read_driver_status(slave_address=6) logger.info(step_modbus_rtu.reg_list)
浙公网安备 33010602011771号