（4）交流伺服电机 初始化 单圈电机 绝对式位置控制 点动， CWL限位，CCWL限位
import time import ctypes import math from pymodbus.client.sync import ModbusSerialClient from threading import Thread ,Lock #导入自己的类 import log logger = log.logger_init() #代码 class ServoMotorRTU: # 创建一个类级别的静态锁 port_lock = Lock()# with MotorRTU.port_lock #类级别的静态锁 def __init__(self, x_port='COM3', x_baud=9600,x_parity='N',x_stop_bits=2,x_byte_size=8): # 配置串行通信参数 self.client = ModbusSerialClient( method='rtu', # 使用RTU模式 port=x_port, # 串行端口，根据你的设备连接情况修改 baudrate=x_baud, # 波特率，与你的设备设置相匹配 timeout=3, # 超时时间（秒） parity=x_parity, # 校验位：无（N），奇（O），偶（E） stopbits=x_stop_bits, # 停止位：1 bytesize=x_byte_size # 数据位：8 ) self.cycle_pulse_num = 7200 #设定相当于电机每旋转1圈的指令脉冲数。按照一个脉冲0.01mm计算：导程72mm，一个脉冲0.01mm，72/0.01 =7200 self.reg_list = list() # 在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 ServoMotorRTU.port_lock: # 类级别的静态锁 result = self.client.read_input_registers(address=address,count=count,unit=slave_address)#功能码号 04 # 检查读取是否成功 if not result.isError(): logger.info("读取成功") 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 ServoMotorRTU.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.info("写入失败，错误代码：", result) else: logger.info("写入成功") ret = True else: logger.info("无法连接到Modbus服务器") # messagebox.showerror("错误提示", "没有通信连接，检查通信线路.") except Exception as e: logger.info("发生异常：", 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 ServoMotorRTU.port_lock: # 类级别的静态锁 # 写入多个保持寄存器（功能码10H） # 注意：unit参数对应于从设备的地址或ID，根据设备配置设置 # 直接将值列表传递给write_registers方法 result = self.client.write_registers(address=reg_address, values=values, unit=slave_address ) # 检查写入是否成功 if result.isError(): logger.info(f"写入失败，错误代码：{ result}") else: logger.info("写入成功") ret = True else: print("无法连接到Modbus服务器") #messagebox.showerror("错误提示", "没有通信连接，检查通信线路.") except Exception as e: print("发生异常：", e) finally: # 关闭连接 self.client.close() print("已关闭连接") return ret # 软件复位伺服电机驱动器 # PA-60 def restart_driver(self, slave_address=1): """应加入延时 time.sleep(20) 等待伺服驱动器重启结束""" self.write_06(reg_address=0x003c, reg_value=0x0001, slave_address=slave_address) #11、单圈/增量电机回零+绝对定位使用方法： # 使用内部位置模式，多圈绝对值无需回零，直接可以绝对定位。 # 单圈/增量电机开机做回零,配置接近开关。 # #两侧限位,加回零操作 def configure_driver(self,slave_address=1): """ 初始化 单圈电机 绝对式位置控制 点动， CWL限位，CCWL限位 必须重新启动，使得设置生效。 可调用 restart_driver（）方法 输入端子： P3-31_bit0 ~bit7 虚拟端子DI1 虚拟端子DI2 虚拟端子DI3 虚拟端子DI4 虚拟端子DI5 虚拟端子DI6 虚拟端子DI7 虚拟端子DI8 P3-38 P3-39 P3-40 P3-41 P3-42 P3-43 P3-44 P3-45 2 22 23 27 28 29 0 清除报警 正点动 负点动 位置暂停 位置0触发 位置1触发 NULL 输出端子 输出端子（5个端子对应 P3 组参数分别为 P3-20、 P3-21、 P3-22、 P3-23、 P3-24）的定义值 bit0 bit1 bit2 bit3 bit4 逻辑1： 供电正常 无报警 定位类命令完成 电磁制动释放 回原点已做， :param slave_address: :return: """ _rtu_id = slave_address _servo_modbus_rtu = self # PA组参数 PA-4=0 驱动器的控制方式 0：位置控制方式 # PA-4=0 PA-14=1 PA-22=5 PA-53=1 _servo_modbus_rtu.write_06(reg_address=4, reg_value=0, slave_address=_rtu_id) # PA-4=0 位置 _servo_modbus_rtu.write_06(reg_address=11, reg_value=self.cycle_pulse_num, slave_address=_rtu_id) # PA-11=7200 导程72mm，一个脉冲0.01mm 1.设定相当于电机每旋转1圈的指令脉冲数。 _servo_modbus_rtu.write_06(reg_address=14, reg_value=3, slave_address=_rtu_id) # PA-14=1 内部位置输入 _servo_modbus_rtu.write_06(reg_address=22, reg_value=3, slave_address=_rtu_id) # PA-22=3 点动控制命令输入 5：IO端子控制点动操作 _servo_modbus_rtu.write_06(reg_address=53, reg_value=1, slave_address=_rtu_id) # PA-53=1 _servo_modbus_rtu.write_06(reg_address=17, reg_value=6000, slave_address=_rtu_id) # PA-17=1 # P3组参数 # P3-30 =2 设置虚拟DI和物理DI的全部使用的方式 _servo_modbus_rtu.write_06(reg_address=0x0100 + 30, reg_value=2, slave_address=_rtu_id) # P3-39=22 点动正转 P3-40=23 点动反转 _servo_modbus_rtu.write_06(reg_address=0x0100 + 39, reg_value=22, slave_address=_rtu_id) _servo_modbus_rtu.write_06(reg_address=0x0100 + 40, reg_value=23, slave_address=_rtu_id) # PA-55 写3 因为接的是NPN集电极开路的接近开关，常开，逻辑取反为常闭 _servo_modbus_rtu.write_06(reg_address=55, reg_value=0x0003, slave_address=_rtu_id) # 限位动作 P3-0=3，P3-1=4，P3-2，P3-3都是0， PA20=0，PA83=0 _servo_modbus_rtu.write_06(reg_address=0x0100 + 0, reg_value=4, slave_address=_rtu_id) _servo_modbus_rtu.write_06(reg_address=0x0100 + 1, reg_value=3, slave_address=_rtu_id) _servo_modbus_rtu.write_06(reg_address=0x0100 + 2, reg_value=0, slave_address=_rtu_id) _servo_modbus_rtu.write_06(reg_address=0x0100 + 3, reg_value=0, slave_address=_rtu_id) _servo_modbus_rtu.write_06(reg_address=20, reg_value=0, slave_address=_rtu_id) _servo_modbus_rtu.write_06(reg_address=83, reg_value=0, slave_address=_rtu_id) ####归零动作####################################################################### # 归零动作 P4-32=1， P4-33=2， # P4组参数：P4-32=1 反转方向原点回归，CWL 做为回归原点 # P4-33=0 # P4-36=60 回零速度 # P4-37=30 爬行速度 # P4-0=2 2：单圈电机绝对模式 _servo_modbus_rtu.write_06(reg_address=0x0200 + 32, reg_value=1, slave_address=_rtu_id) # P4组参数：P4-32=1 反转方向原点回归，CWL 做为回归原点 _servo_modbus_rtu.write_06(reg_address=0x0200 + 33, reg_value=0, slave_address=_rtu_id) # P4-33=0 0：找到参考原点后返回寻找单圈绝对位置零点做为机械原点； # 2：找到参考原点（上升沿或单圈绝对位置零点）作为机械原点后，减速停止。 _servo_modbus_rtu.write_06(reg_address=0x0200 + 34, reg_value=1, slave_address=_rtu_id) # P4-34=1 1：电源开启时，自动执行原点回归功能 ； 2：由原点搜寻功能（SHOM）输入接点触发原点回归功能。 _servo_modbus_rtu.write_06(reg_address=0x0200 + 35, reg_value=0, slave_address=_rtu_id) # P4-35=0 原点停止模式设定 _servo_modbus_rtu.write_06(reg_address=0x0200 + 0, reg_value=2, slave_address=_rtu_id) # P4-0=2 2：单圈电机绝对模式 _servo_modbus_rtu.write_06(reg_address=0x0200 + 36, reg_value=200, slave_address=_rtu_id) # P4-36=200 RPM回零速度 _servo_modbus_rtu.write_06(reg_address=0x0200 + 37, reg_value=50, slave_address=_rtu_id) # P4-37=50 RPM爬行速度 # P3-38=2 报警清除 _servo_modbus_rtu.write_06(reg_address=0x0100 + 38, reg_value=2, slave_address=_rtu_id) # P3-38=0 ####位置控制####################################################################### # P3-41=27 P3-42=28 P3-43=29 _servo_modbus_rtu.write_06(reg_address=0x0100 + 41, reg_value=27, slave_address=_rtu_id) # 位置暂停 _servo_modbus_rtu.write_06(reg_address=0x0100 + 42, reg_value=28, slave_address=_rtu_id) # 位置触发 _servo_modbus_rtu.write_06(reg_address=0x0100 + 43, reg_value=29, slave_address=_rtu_id) # 位置选择1 ####输出端子####################################################################### # 输出端子（5个端子对应 P3 组参数分别为 P3-20、P3-21、P3-22、P3-23、P3-24）的定义值 # bit4 bit3 bit2 bit1 bit0 # 读取 P3-33 虚拟输出端子状态值 0000-1111 0000 0x0121 _servo_modbus_rtu.write_06(reg_address=0x0100 + 20, reg_value=2, slave_address=_rtu_id) # P3-20 =2 1 伺服器正常 _servo_modbus_rtu.write_06(reg_address=0x0100 + 21, reg_value=3, slave_address=_rtu_id) # P3-21 =3 1 无报警 _servo_modbus_rtu.write_06(reg_address=0x0100 + 22, reg_value=16, slave_address=_rtu_id) # P3-22 =16 1 当内部位置命令完成或内部位置命令停止时，经过 P4-1 设置时间后输出信号。 _servo_modbus_rtu.write_06(reg_address=0x0100 + 23, reg_value=8, slave_address=_rtu_id) # P3-23 =8 1 电磁制动器释放 _servo_modbus_rtu.write_06(reg_address=0x0100 + 24, reg_value=15, slave_address=_rtu_id) # P3-24 =8 1 原点回归动作完成 # PA-60 写1 重启伺服驱动器 _servo_modbus_rtu.write_06(reg_address=60, reg_value=1, slave_address=_rtu_id) # PA-60 写1 重启 time.sleep(5) # 设点动 PA21= 60 rpm PA40= 200ms PA41=200ms _servo_modbus_rtu.write_06(reg_address=21, reg_value=100, slave_address=_rtu_id) _servo_modbus_rtu.write_06(reg_address=40, reg_value=200, slave_address=_rtu_id) _servo_modbus_rtu.write_06(reg_address=41, reg_value=200, slave_address=_rtu_id) # 设位置 # 位置0设置：P4-2:0 P4-3=0 P4-4=300 定义位置1在原点位置,速度300 # P4-2 内部位置指令 1 的位置圈数设定 # P4-3 内部位置指令 1 的位置圈内脉冲数设定 # P4-4 内部位置指令控制 1 的移动速度设定 rpm _servo_modbus_rtu.write_06(reg_address=0x0200 + 2, reg_value=0, slave_address=_rtu_id) _servo_modbus_rtu.write_06(reg_address=0x0200 + 3, reg_value=0, slave_address=_rtu_id) _servo_modbus_rtu.write_06(reg_address=0x0200 + 4, reg_value=100, slave_address=_rtu_id) # 位置1设置：P4-5：20 P4-6=0 P4-7=100rpm 位置1在绝对坐标20圈位置，定位速度300 _servo_modbus_rtu.write_06(reg_address=0x0200 + 5, reg_value=4, slave_address=_rtu_id) _servo_modbus_rtu.write_06(reg_address=0x0200 + 6, reg_value=0, slave_address=_rtu_id) _servo_modbus_rtu.write_06(reg_address=0x0200 + 7, reg_value=100, slave_address=_rtu_id) def reset_encoder_zero(self,slave_address =1): """重新设置编码器的零点""" _rtu_id = slave_address _servo_modbus_rtu = self _servo_modbus_rtu.write_06(reg_address=0x0100 + 36, reg_value=1, slave_address=_rtu_id) _servo_modbus_rtu.write_06(reg_address=0x0100 + 34, reg_value=1, slave_address=_rtu_id) def forward_jog(self,slave_address=1): _rtu_id = slave_address _servo_modbus_rtu = self # P3-31（0X011F） # 写 0 _servo_modbus_rtu.write_06(reg_address=0x0100 + 31, reg_value=0, slave_address=_rtu_id) # 正向点动：0x011F写2 _servo_modbus_rtu.write_06(reg_address=0x0100 + 31, reg_value=2, slave_address=_rtu_id) def backward_jog(self, slave_address=1): _rtu_id = slave_address _servo_modbus_rtu = self # P3-31（0X011F） # 写 0 _servo_modbus_rtu.write_06(reg_address=0x0100 + 31, reg_value=0, slave_address=_rtu_id) # 反向点动：0x011F写4 _servo_modbus_rtu.write_06(reg_address=0x0100 + 31, reg_value=4, slave_address=_rtu_id) def clear_err(self, slave_address=1): """清除伺服器报错，自动重启可接受新的位置命令""" _rtu_id = slave_address _servo_modbus_rtu = self # P3-31（0X011F） # 回原点： 0x011F先写0，然后在写1 _servo_modbus_rtu.write_06(reg_address=0x0100 + 31, reg_value=0, slave_address=_rtu_id) _servo_modbus_rtu.write_06(reg_address=0x0100 + 31, reg_value=1, slave_address=_rtu_id) def goto_pos_1(self, slave_address=1): """固定设置为 0 位置，绝对位置 坐标0""" _rtu_id = slave_address _servo_modbus_rtu = self # P3-31（0X011F） # 定位到位置0（零点位置）：0x011F先写0在写16 _servo_modbus_rtu.write_06(reg_address=0x0100 + 31, reg_value=0, slave_address=_rtu_id) _servo_modbus_rtu.write_06(reg_address=0x0100 + 31, reg_value=16, slave_address=_rtu_id) def set_pos_2(self,slave_address=1,distance_pulse_counter=0,rpm = 60): """ 按照圈速控制电机， 正负数 # 圈数限制：-30000- 30000 # 脉冲限制：+/-max .cnt/rev""" rpm = math.fabs(rpm) #正整数 rpm = int(rpm) offset_address = 0x0080 # 暂存地址偏移量 _rtu_id = slave_address _servo_modbus_rtu = self if math.isnan(distance_pulse_counter) or math.isinf(distance_pulse_counter): return False cycle_pulse = self.cycle_pulse_num sign = 1 if distance_pulse_counter<0: sign = -1 cycles = (sign* distance_pulse_counter)//cycle_pulse # 去符号，得正数 if cycles > 30000 or cycles < -30000: return False else: # 求圈数 circles = ServoMotorRTU.get_16bit_twos_complement_ctypes(cycles*sign) # 求不足一圈的部分的脉冲数 circles_fractional =(sign* distance_pulse_counter)%cycle_pulse # 去符号，得正数 pulses = ServoMotorRTU.get_16bit_twos_complement_ctypes(circles_fractional*sign) # 恢复符号 # 通信设置 _rtu_id = slave_address _servo_modbus_rtu = self # 位置1设置：P4-5：20 P4-6=0 P4-7=60 位置1在绝对坐标20圈位置，定位速度300 _servo_modbus_rtu.write_06(reg_address=0x0200 + 5+offset_address , reg_value= circles, slave_address=_rtu_id) _servo_modbus_rtu.write_06(reg_address=0x0200 + 6+offset_address, reg_value= pulses, slave_address=_rtu_id) _servo_modbus_rtu.write_06(reg_address=0x0200 + 7+offset_address, reg_value= rpm, slave_address=_rtu_id) return True def goto_pos_2(self,slave_address=1): """去预设的位置2，执行位置指令""" _rtu_id = slave_address _servo_modbus_rtu = self # P3-31（0X011F） # 定位到位置1：0x011F先写0在写48（32+16） _servo_modbus_rtu.write_06(reg_address=0x0100 + 31, reg_value=0, slave_address=_rtu_id) _servo_modbus_rtu.write_06(reg_address=0x0100 + 31, reg_value=48, slave_address=_rtu_id) def pause_motion(self, slave_address=1): """暂停伺服电机转动""" _rtu_id = slave_address _servo_modbus_rtu = self # P3-31（0X011F） # 写 0 _servo_modbus_rtu.write_06(reg_address=0x0100 + 31, reg_value=0, slave_address=_rtu_id) ## 位置暂停：0x011F写8 继续位置触发会把刚才没走完的位置走完 _servo_modbus_rtu.write_06(reg_address=0x0100 + 31, reg_value=8, slave_address=_rtu_id) def get_input_port(self,slave_address=1): """读取输入端子状态值""" _rtu_id = slave_address _servo_modbus_rtu = self # P3-31（0X011F） # 写 0 _servo_modbus_rtu.read_04(reg_address =0x100f,reg_count=1,slave_address=_rtu_id) try: code = _servo_modbus_rtu.reg_list[0] except IndexError: return 0 return code def get_output_port(self, slave_address=1): """ 0b00010111 归零已做，NC，定位命令完成，无报警，有电， 读取输出端子状态值 输出端子 输出端子（5个端子对应 P3 组参数分别为 P3-20、 P3-21、 P3-22、 P3-23、 P3-24）的定义值 bit0 bit1 bit2 bit3 bit4 逻辑1： 供电正常 无报警 定位类命令完成 电磁制动释放 回原点已做， """ _rtu_id = slave_address _servo_modbus_rtu = self # P3-31（0X011F） # 写 0 _servo_modbus_rtu.read_04(reg_address=0x1010, reg_count=1, slave_address=_rtu_id) try: code = _servo_modbus_rtu.reg_list[0] except IndexError: return 0 return code def get_err_code(self, slave_address=1): """读取错代号状态值,通过输入DI1清除，方法 clear_err""" _rtu_id = slave_address _servo_modbus_rtu = self # P3-31（0X011F） # 写 0 _servo_modbus_rtu.read_04(reg_address=0x1013, reg_count=1, slave_address=_rtu_id) try: code = _servo_modbus_rtu.reg_list[0] except IndexError: return 0 return code def is_command_ok(self,slave_address =1, time_out =100): """阻塞，间隔不断地读取DO端口，判断是否完成CMD。 time_out 如果在此时间time_out ms内没有读取状态值，报错""" _rtu_id = slave_address _servo_modbus_rtu = self _time_out = time_out//10 _clock_10ms =0 while True: time.sleep(0.01) ret = _servo_modbus_rtu.get_output_port(slave_address=_rtu_id) if ret & 0b00010111 == 0b00010111: return True if time_out != 0: _clock_10ms+=1 if _clock_10ms>=_time_out: return False @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，通信 servo_modbus_rtu = ServoMotorRTU(x_port='COM6', x_baud=9600,x_parity='N',x_stop_bits=1,x_byte_size=8) servo_modbus_rtu.configure_driver(slave_address=2) #servo_modbus_rtu.restart_driver(slave_address=2) # servo_modbus_rtu.reset_encoder_zero(slave_address=1) # recycle_times =0 # while True: # time.sleep(0.5) # recycle_times+=1 # servo_modbus_rtu.set_pos_2(slave_address=1, distance_pulse_counter=7200 * 9, rpm=600) # servo_modbus_rtu.goto_pos_2(slave_address=1) # # 等待动作CMD完成 # servo_modbus_rtu.is_command_ok(slave_address=1, time_out=5000) # # servo_modbus_rtu.set_pos_2(slave_address=1, distance_pulse_counter=7200 * 0, rpm=100) # servo_modbus_rtu.goto_pos_2(slave_address=1) # # 等待动作CMD完成 # servo_modbus_rtu.is_command_ok(slave_address=1, time_out=5000) # if recycle_times>=5: # break # # # # #servo_modbus_rtu.configure_jog_template(slave_address=1) # #servo_modbus_rtu.restart_driver(slave_address=1) # # #servo_modbus_rtu.goto_pos_2(slave_address=1) # # servo_modbus_rtu.clear_err(slave_address=1) # # time.sleep(2) # # servo_modbus_rtu.goto_pos_2(slave_address=1) # # # time.sleep(0.5) # # # servo_modbus_rtu.backward_jog(slave_address=1) # # # time.sleep(0.5) # # # servo_modbus_rtu.forward_jog(slave_address=1) # # time.sleep(2) # # servo_modbus_rtu.goto_pos_1(slave_address=1) # recycle_times =0 # while True: # time.sleep(0.5) # recycle_times+=1 # #logger.info(hex(servo_modbus_rtu.get_input_port(slave_address=1))) # logger.info(hex(servo_modbus_rtu.get_output_port(slave_address=1))) # if recycle_times>=2: # break # logger.info(hex(servo_modbus_rtu.get_err_code(slave_address=1))) # break # # PA组参数 PA-4=0 驱动器的控制方式 0：位置控制方式 # # PA-4=0 PA-14=1 PA-22=5 PA-53=1 # servo_modbus_rtu.write_06(reg_address=4, reg_value=0, slave_address=1) #PA-4=0 位置 # servo_modbus_rtu.write_06(reg_address=14, reg_value=3, slave_address=1) #PA-14=1 内部位置输入 # servo_modbus_rtu.write_06(reg_address=22, reg_value=0, slave_address=1) #PA-22=5 速度命令输入 5：IO端子控制点动操作 # servo_modbus_rtu.write_06(reg_address=53, reg_value=1, slave_address=1) #PA-53=1 # # servo_modbus_rtu.write_06(reg_address=17, reg_value=6000, slave_address=1) #PA-17=1 # # P3组参数 # # P3-30 =2 设置虚拟DI和物理DI的全部使用的方式 # servo_modbus_rtu.write_06(reg_address=0x0100 + 30, reg_value=2, slave_address=1) # # P3-39=22 点动正转 P3-40=23 点动反转 # servo_modbus_rtu.write_06(reg_address=0x0100 + 39, reg_value=22, slave_address=1) # servo_modbus_rtu.write_06(reg_address=0x0100 + 40, reg_value=23, slave_address=1) # # # PA-55 写3 因为接的是NPN集电极开路的接近开关，常开，逻辑取反为常闭 # servo_modbus_rtu.write_06(reg_address=55, reg_value=0x0003, slave_address=1) # # # 限位动作 P3-0=3，P3-1=4，P3-2，P3-3都是0， PA20=0，PA83=0 # servo_modbus_rtu.write_06(reg_address=0x0100 + 0, reg_value=4, slave_address=1) # servo_modbus_rtu.write_06(reg_address=0x0100 + 1, reg_value=3, slave_address=1) # servo_modbus_rtu.write_06(reg_address=0x0100 + 2, reg_value=0, slave_address=1) # servo_modbus_rtu.write_06(reg_address=0x0100 + 3, reg_value=0, slave_address=1) # servo_modbus_rtu.write_06(reg_address= 20, reg_value=0, slave_address=1) # servo_modbus_rtu.write_06(reg_address= 83, reg_value=0, slave_address=1) # ####归零动作####################################################################### # # 归零动作 P4-32=1， P4-33=2， # # P4组参数：P4-32=1 反转方向原点回归，CWL 做为回归原点 # # P4-33=0 # # P4-36=60 回零速度 # # P4-37=30 爬行速度 # # P4-0=2 2：单圈电机绝对模式 # servo_modbus_rtu.write_06(reg_address=0x0200 + 32, reg_value=1, slave_address=1) #P4组参数：P4-32=1 反转方向原点回归，CWL 做为回归原点 # servo_modbus_rtu.write_06(reg_address=0x0200 + 33, reg_value=0, slave_address=1) #P4-33=0 0：找到参考原点后返回寻找单圈绝对位置零点做为机械原点； # # 2：找到参考原点（上升沿或单圈绝对位置零点）作为机械原点后，减速停止。 # servo_modbus_rtu.write_06(reg_address=0x0200 + 34, reg_value=1, slave_address=1) # P4-34=1 1：电源开启时，自动执行原点回归功能 ； 2：由原点搜寻功能（SHOM）输入接点触发原点回归功能。 # servo_modbus_rtu.write_06(reg_address=0x0200 + 35, reg_value=0, slave_address=1) # P4-35=0 原点停止模式设定 # # servo_modbus_rtu.write_06(reg_address=0x0200 + 0, reg_value=2, slave_address=1) # P4-0=2 2：单圈电机绝对模式 # # servo_modbus_rtu.write_06(reg_address=0x0200 + 36, reg_value=200, slave_address=1) # P4-36=200 RPM回零速度 # servo_modbus_rtu.write_06(reg_address=0x0200 + 37, reg_value=50, slave_address=1) # P4-37=50 RPM爬行速度 # # P3-38=2 报警清除 # servo_modbus_rtu.write_06(reg_address=0x0100 + 38, reg_value=2, slave_address=1)# P3-38=0 # # ####位置控制####################################################################### # # P3-41=27 P3-42=28 P3-43=29 # servo_modbus_rtu.write_06(reg_address=0x0100 + 41, reg_value=27, slave_address=1) # 位置暂停 # servo_modbus_rtu.write_06(reg_address=0x0100 + 42, reg_value=28, slave_address=1) # 位置触发 # servo_modbus_rtu.write_06(reg_address=0x0100 + 43, reg_value=29, slave_address=1) # 位置选择1 # # ####输出端子####################################################################### # #输出端子（4 个端子对应 P3 组参数分别为 P3-20、P3-21、P3-22、P3-23）的定义值 # # 读取 P3-33 虚拟输出端子状态值 0000-1111 0000 0x0121 # servo_modbus_rtu.write_06(reg_address=0x0100 + 20, reg_value=2, slave_address=1)#P3-20 =2 # servo_modbus_rtu.write_06(reg_address=0x0100 + 21, reg_value=3, slave_address=1)#P3-21 =3 报警 # servo_modbus_rtu.write_06(reg_address=0x0100 + 22, reg_value=5, slave_address=1)#P3-22 =5 定位完成 # servo_modbus_rtu.write_06(reg_address=0x0100 + 23, reg_value=8, slave_address=1)#P3-23 =8 电磁制动器 # # # PA-60 写1 重启伺服驱动器 # servo_modbus_rtu.write_06(reg_address=60, reg_value=1, slave_address=1) # PA-60 写1 重启 # time.sleep(5) # # 设位置 # # 位置0设置：P4-2:0 P4-3=0 P4-4=300 定义位置1在原点位置,速度300 # # P4-2 内部位置指令 1 的位置圈数设定 # # P4-3 内部位置指令 1 的位置圈内脉冲数设定 # # P4-4 内部位置指令控制 1 的移动速度设定 rpm # servo_modbus_rtu.write_06(reg_address=0x0200 + 2, reg_value=0, slave_address=1) # servo_modbus_rtu.write_06(reg_address=0x0200 + 3, reg_value=0, slave_address=1) # servo_modbus_rtu.write_06(reg_address=0x0200 + 4, reg_value=100, slave_address=1) # # 位置1设置：P4-5：20 P4-6=0 P4-7=100rpm 位置1在绝对坐标20圈位置，定位速度300 # servo_modbus_rtu.write_06(reg_address=0x0200 + 5, reg_value=4, slave_address=1) # servo_modbus_rtu.write_06(reg_address=0x0200 + 6, reg_value=0, slave_address=1) # servo_modbus_rtu.write_06(reg_address=0x0200 + 7, reg_value=100, slave_address=1)
伺服电机资料
单圈编码器，带记忆，绝对坐标控制，带断电锁紧抱闸，分软件SON和外部信号SON 代码。
#!/usr/bin/env python3 #************************** # Z轴伺服电机 软件强制使能 # X、Y轴伺服电机 外部信号使能 # X、Y轴电机受Z轴控制，即Z轴处于最高时，X、Y轴可以运动，其他情况禁止。 # 按照如下命令行方式打开VSCODE，方便调试 # source install/setup.bash && code #************************** import time import math import ctypes # 用于数据类型转换 import numpy as np #import inspect import threading import rclpy from rclpy.node import Node from rclpy.task import Future from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup from rclpy.qos import QoSProfile, QoSReliabilityPolicy from std_msgs.msg import Header from pkg_interfaces.msg import Partdetect # 确保正确导入自定义消息 from pkg_interfaces.msg import Partweight # 确保正确导入自定义消息 from pkg_interfaces.msg import Turntable # 确保正确导入自定义消息 from pkg_interfaces.srv import ModbusRTU # 替换为你的包名 from pkg_joints.actuators.actuator_base import Actuator from pkg_joints.actuators.transform_pts import Cls_pts_transform # 如何使用其他包中的类 import logging #设置调试信息打印 logging.basicConfig( level=logging.INFO, format='%(asctime)s - %(levelname)s - %(filename)s:%(lineno)d - %(message)s', datefmt='%Y-%m-%d %H:%M:%S' ) class ServoStatus: """伺服系统状态码定义 (使用8位二进制表示，兼容标准状态和扩展标志)""" # 基础状态（低4位） POWER_READY = 0b0000_0001 # 0001 主电源正常(就绪基础状态) ENABLED = 0b0000_0010 # 0010 伺服使能标志 INPOSITION = 0b0000_0100 # 0100 位置指令完成 HOME_COMPLETE = 0b0000_1000 # 1000 原点回归完成 # 复合状态（推荐使用组合） ## 完成，等待新命令 STATUS_READY = POWER_READY | ENABLED | INPOSITION # 0b0000_0111 (电源+使能+定位完成) ## 移动中 STATUS_MOVING = POWER_READY | ENABLED # 0b0000_0011 (电源+使能) STATUS_ERROR = 0b1000_0000 # 最高位表示错误 STATUS_TIMEOUT = 0b1000_0001 # 超时错误(含电源标志) # 状态检查方法 @staticmethod def is_ready(status: int) -> bool: return (status & 0b0000_0111) == 0b0000_0111 @staticmethod def is_moving(status: int) -> bool: return (status & 0b0000_0011) == 0b0000_0011 and not (status & 0b1000_0000) @staticmethod def is_error(status: int) -> bool: return bool(status & 0b1000_0000) class PFDE_SERVO_CTL: # 普菲德 PS100 伺服电机 绝对坐标 def __init__(self, **kwargs): """ 伺服电机类构造函数 使用 ROS2 service 实现 Modbus通信,作为客户端 具有刹车,用作Z轴,绝对定位方式。 CCW时,导轨滑块远离电机,ORGP接近开关设在 有 电机侧 CW时,电机回归零点,RGP 驱动器CN1_4PIN --- DI2 设置为ROGP 零点 驱动器CN1_20PIN --- DI1 没有使用 """ # 初始化 Actuator 基类 self.speed = kwargs.get('speed', 0) # RPM self.displace = kwargs.get('displace', 0.0)# 关节位移 米 self.acc = kwargs.get('acc', 0) # 电机加速度的设定数,可能不等于加速度 #print(type(self.acc ),self.acc ) # 声明参数并根据**kwargs设置,并给定默认值 self.slave_id = kwargs.get('slave_id', 1) #,多个逗号,变成了元组# 驱动器通信地址 self.ratio = kwargs.get('ratio', 1) #,直接驱动,无减速机 self.radius = kwargs.get('radius', 0.011459155902616465) #,滑动模组,导程72mm 即周长72mm,输出轮半径 self.pulses = kwargs.get('pulses', 7200) #,# 转1周的脉冲数量 72mm/7200 0.01mm #print(type(self.pulses ),self.pulses ) self.status_dict = { "POWER_ON" : False, #电源现正常 "" : False, #位置误差合格 "REACH_ON" : False, #已达到位置 "HOMED_OK" : False #已作回零点 } #self.reset =None # 复位伺服控制器或初始化伺服控制器的函数方法入口 self.modbus_srv_init =None self.modbus_srv_request =None self.debug_print = self.no_print def no_print(self, *args,**kwargs): """空函数""" pass def connect(self, **kwargs): """ROS2服务通信连接""" if self.modbus_srv_init != None: self.modbus_srv_init() def send_request(self, slave_id, address, count=0, value=0, function_code=3,registers=[]): """服务的回调函数""" if self.modbus_srv_request == None: # Create a future future = Future() result = future.result() result.success = False # 返回一个无效的result return result # 返回future.result() 实例 response =self.modbus_srv_request(slave_id=slave_id, address=address, count=count, value=value, function_code=function_code, registers=registers) return response def reset(self,**kwargs): """复位重启驱动器,新设置生效""" """应加入延时 time.sleep(20) 等待伺服驱动器重启结束""" ret = self.clear_err() self._write_06(reg_address=60, reg_value=1) # PA-60 写1 重启 time.sleep(5) # 等5s return ret def clear_err(self): """清除伺服器报错，自动重启可接受新的位置命令""" # P3-31（0X011F） # 回原点： 0x011F先写0，然后在写0x80 即对虚拟DI1写入上升沿信号 offset_address = 0x0080 # 暂存地址偏移量，直接操作，不写入EEROM ret = self._write_06(reg_address=0x0100 + 31+offset_address, reg_value=0x00) if ret: ret = self._write_06(reg_address=0x0100 + 31+offset_address, reg_value=0x80) return ret def init_Z2(self, **kwargs): """设置电机参数 注意！注意！ 更改已用过的电机驱动EEROM参数时,首先菜单设置为缺省值,否则因参数过多而发生错乱。 设置为： 软件SON(PA-53=1) 绝对编码单圈电机 绝对式位置控制 点动, 无限位 ,手动设置零点 物理DI1 为 ORGP 菜单设置: PA- 71 MODBUS 从机地址 X电机 2,Y电机 3,Z电机4 PA- 72 =96 波特率 9600 PA- 73 =43 通信方式:8,N,1(MODBUS, RTU)。 物理输入部分端子： 信号#引脚号#缺省功能 #配置功能 DI1# 20#伺服使能 #无功能 DI2# 4#报警清除 #ORGP DI3# 19#正转驱动禁止 #无功能 DI4# 3#反转驱动禁止 #无功能 必须重新启动,使得设置生效。 输入端子: P3-31_bit0 ~bit7 虚拟端子DI1 P3-38 SHOM 虚拟端子DI2 P3-39 点动正转 虚拟端子DI3 P3-40 点动反转 虚拟端子DI4 P3-41 位置暂停 虚拟端子DI5 P3-42 位置指令触发 虚拟端子DI6 P3-43 目标位置选择 虚拟端子DI7 P3-44 无功能 虚拟端子DI8 P3-45 报警清除 输出端子 虚拟输出端子(4个端子对应 P3 组参数分别为 P3-20、 P3-21、 P3-22、 P3-23)的定义值 P3-20 =2 1 伺服主电源正常，无报警。 P3-21 =9 1 SON有效。 P3-22 =16 1 当内部位置命令完成或内部位置命令停止时，经过 P4-1 设置时间后输出信号。 P3-24 =15 1 原点回归动作完成 HOME """ self.slave_id = kwargs.get('slave_id', self.slave_id ) #伺服驱动器作为RTU，通信地址 self.ratio = kwargs.get('ratio', self.ratio) #直接驱动，无减速机 减速比是1 self.radius = kwargs.get('radius', self.radius) #单位米 输出轮半径。滑动模组，导程72mm 即输出轮周长72mm，计算输出轮半径 self.pulses = kwargs.get('pulses', self.pulses) # 1圈脉冲数量 #print(type(self.pulses ),self.pulses ) ret = True # PA组参数 PA-4=0 驱动器的控制方式 0：位置控制方式 # PA-4=0 PA-14=1 PA-22=5 PA-53=1 self._write_06(reg_address=4, reg_value=0) # PA-4=0 0：位置控制方式 self._write_06(reg_address=11, reg_value=self.pulses) # PA-11=7200 设定相当于电机每旋转1圈的指令脉冲数。导程72mm，一个脉冲0.01mm 1.设定相当于电机每旋转1圈的指令脉冲数。 # P3-32=3 电机位置、指令位置、位置差、单圈绝对位置的显示方式(读取得1000H -101BH ) self._write_06(reg_address=0x0100 + 32, reg_value=3)# P3-32=3 3：上位机（PA-11）分辨率显示绝对位置。 self._write_06(reg_address=14, reg_value=3) # PA-14=3 位置指令输入方式：3:内部位置输入 #self._write_06(reg_address=22, reg_value=5) # PA-22=5 设置速度指令来源。 点动控制命令输入 5：IO端子控制点动操作 self._write_06(reg_address=53, reg_value=1) # PA-53=1 软件强制使能伺服驱动器的驱动功能 #self._write_06(reg_address=17, reg_value=6000) # PA-17=1 位置超差范围检测，超过报警 # P3组参数 # P3-30 =2 设置虚拟DI和物理DI的全部使用的方式 # P3-30=2 时，IO 输入由 DI1~DI4 和 P3-31 决定，输入 IO 数为 12 个,对应参数 # P3-0~P3-3 及 P3-38~P3-45。 self._write_06(reg_address=0x0100 + 30, reg_value=2) # P3-39=22 点动正转 P3-40=23 点动反转 self._write_06(reg_address=0x0100 + 39, reg_value=22)#虚拟 IO 输入 DI2 功能 self._write_06(reg_address=0x0100 + 40, reg_value=23)#虚拟 IO 输入 DI3 功能 # PA-55 写3 因为物理DI1、DI2接的是NPN集电极开路的接近开关 # 设置输入端子取反。按位设置物理DI1、DI2、DI3、DI4的逻辑信号。 # 不取反的端子，在开关闭合时有效，开关断开时无效；（有问题？？？，需实验确定） # 取反的端子，在开关闭合时无效，开关断开时有效。 self._write_06(reg_address=55, reg_value=0x0000) # 信号逻辑 # DI1 引脚接 NPN接近开关 # 限位动作 P3-0=34，P3-1=4，P3-2，P3-3都是0， PA20=0，PA83=0 self._write_06(reg_address=0x0100 + 0, reg_value=0) #数字输入 物理DI1 功能 无功能 self._write_06(reg_address=0x0100 + 1, reg_value=34) #数字输入 物理DI2 功能 ORGP 回归的原点 self._write_06(reg_address=0x0100 + 2, reg_value=0) #数字输入 物理DI3 功能 无功能 self._write_06(reg_address=0x0100 + 3, reg_value=0) #数字输入 物理DI4 功能 无功能 # self._write_06(reg_address=20, reg_value=0) #CWL,CCWL 驱动禁止，输入无效，或有效。这里无效。 # self._write_06(reg_address=83, reg_value=0) #CWL,CCWL 方向禁止的方式 ####归零动作####################################################################### # 归零动作 P4-32=1， P4-33=2， # P4组参数：P4-32=1 反转方向原点回归，CWL 做为回归原点 # P4-33=0 # P4-36=60 回零速度 # P4-37=30 爬行速度 # P4-0=2 2：单圈电机绝对模式 self._write_06(reg_address=0x0200 + 32, reg_value=3) # P4-32=3 3：反转方向原点回归，ORGP 做为回归原点 self._write_06(reg_address=0x0200 + 33, reg_value=2) # P4-33=0 0：找到参考原点后返回寻找单圈绝对位置零点做为机械原点； # 2：找到参考原点（上升沿或单圈绝对位置零点）作为机械原点后，减速停止。 self._write_06(reg_address=0x0200 + 34, reg_value=2) # P4-34=1 2：由原点搜寻功能（SHOM）输入接点触发原点回归功能。 # 1：电源开启时，自动执行原点回归功能 ； 2：由原点搜寻功能（SHOM）输入接点触发原点回归功能。 self._write_06(reg_address=0x0200 + 35, reg_value=0) # P4-35=0 原点停止模式设定，0：原点检测完成后，电机减速并拉回至原点。 self._write_06(reg_address=0x0200 + 36, reg_value=50) # P4-36=200 RPM回零速度 self._write_06(reg_address=0x0200 + 37, reg_value=50) # P4-37=50 RPM爬行速度 # 编码器类型和指令（相对或绝对）位置 # 自动参数 PA-62=4 单圈绝对值编码器 self._write_06(reg_address=0x0200 + 0, reg_value=2) # P4-0=0 2：绝对式位置指令，适用于 PA62 = 4 单圈编码器模式 # P3-38=33 SHOM self._write_06(reg_address=0x0100 + 38, reg_value=33) # P3-38=33 虚拟 IO 输入 DI1 功能 ####位置控制####################################################################### # P3-41=27 P3-42=28 P3-43=29 self._write_06(reg_address=0x0100 + 41, reg_value=27) # 位置暂停 虚拟 IO 输入 DI4 功能 self._write_06(reg_address=0x0100 + 42, reg_value=28) # 位置触发 虚拟 IO 输入 DI5 功能 self._write_06(reg_address=0x0100 + 43, reg_value=29) # 位置选择2 虚拟 IO 输入 DI6 功能 self._write_06(reg_address=0x0100 + 44, reg_value=0) # 没有使用 虚拟 IO 输入 DI7 功能 # P3-45=2 报警清除 self._write_06(reg_address=0x0100 + 45, reg_value=2) # 报警清除 虚拟 IO 输入 DI8 功能 ####输出端子####################################################################### # 虚拟DO输出端子（4个端子对应 P3 组参数分别为 P3-20、P3-21、P3-22、P3-23）的定义值 # P3-33的位 bit0 bit1 bit2 bit3 # 读取 P3-33 虚拟输出端子状态值 0000-1111 0000 0x0121 self._write_06(reg_address=0x0100 + 20, reg_value=2) # P3-20 =2 DO1 1 伺服主电源正常，无报警。 self._write_06(reg_address=0x0100 + 21, reg_value=9) # P3-21 =9 DO2 1 伺服器使能标志。 self._write_06(reg_address=0x0100 + 22, reg_value=16) # P3-22 =16 DO3 1 当内部位置命令完成或内部位置命令停止时，经过 P4-1 设置时间后输出信号。 self._write_06(reg_address=0x0100 + 23, reg_value=15) # P3-24 =15 DO4 1 原点回归动作完成 HOME # 设点动 PA21= 60 rpm PA40= 200ms PA41=200ms self._write_06(reg_address=21, reg_value=30)#JOG 运行速度 30 rpm self._write_06(reg_address=40, reg_value=200)#表示电机从0-1000r/min的加速时间ms self._write_06(reg_address=41, reg_value=200)#表示电机从1000-0r/min的减速时间 ms # 设位置 # 位置0设置：P4-2:0 P4-3=0 P4-4=300 定义位置1在原点位置,速度100 # P4-2 内部位置指令 1 的位置圈数设定 # P4-3 内部位置指令 1 的位置圈内脉冲数设定 # P4-4 内部位置指令控制 1 的移动速度设定 rpm self._write_06(reg_address=0x0200 + 2, reg_value=0) self._write_06(reg_address=0x0200 + 3, reg_value=0) self._write_06(reg_address=0x0200 + 4, reg_value=100) # 位置1设置：P4-5：20 P4-6=0 P4-7=100rpm 位置1在绝对坐标0圈位置，定位速度100 self._write_06(reg_address=0x0200 + 5, reg_value=0)#圈 self._write_06(reg_address=0x0200 + 6, reg_value=0) self._write_06(reg_address=0x0200 + 7, reg_value=100) time.sleep(2) # 等2s # PA-60 写1 重启伺服驱动器 self._write_06(reg_address=60, reg_value=1) # PA-60 写1 重启 time.sleep(5) # 等5s logging.info(f"Z motor init:{ret} ") # line =inspect.currentframe().f_lineno # self.debug_print(f"@{line}:Z motor init:{ret} ") return ret def init_XY(self, **kwargs): """设置XY轴电机参数 注意！注意！ 更改已用过的电机驱动EEROM参数时,首先菜单设置为缺省值,否则因参数过多而发生错乱。 设置为： 硬件SON(DI1引脚) 绝对编码单圈电机 绝对式位置控制 点动, 无限位 ,手动设置零点 OGPR(DI2引脚) 作为归零点 菜单设置: PA- 71 MODBUS 从机地址 X电机 2,Y电机 3,Z电机4 PA- 72 =96 波特率 9600 PA- 73 =43 通信方式:8,N,1(MODBUS, RTU)。 物理输入部分端子： 信号#引脚号#缺省功能 #配置功能 DI1# 20#伺服使能 #伺服使能 必须设置PA53=0 DI2# 4#报警清除 #ORGP DI3# 19#正转驱动禁止 #无功能 DI4# 3#反转驱动禁止 #无功能 必须重新启动,使得设置生效。 输入端子: P3-31_bit0 ~bit7 虚拟端子DI1 P3-38 SHOM 虚拟端子DI2 P3-39 点动正转 虚拟端子DI3 P3-40 点动反转 虚拟端子DI4 P3-41 位置暂停 虚拟端子DI5 P3-42 位置指令触发 虚拟端子DI6 P3-43 目标位置选择 虚拟端子DI7 P3-44 无功能 虚拟端子DI8 P3-45 报警清除 输出端子 虚拟输出端子(4个端子对应 P3 组参数分别为 P3-20、 P3-21、 P3-22、 P3-23)的定义值 P3-20 =2 1 伺服主电源正常，无报警。 P3-21 =9 1 SON有效 P3-22 =16 1 当内部位置命令完成或内部位置命令停止时，经过 P4-1 设置时间后输出信号。 P3-24 =15 1 原点回归动作完成 HOME """ self.slave_id = kwargs.get('slave_id', self.slave_id ) #伺服驱动器作为RTU，通信地址 self.ratio = kwargs.get('ratio', self.ratio) #直接驱动，无减速机 减速比是1 self.radius = kwargs.get('radius', self.radius) #单位米 输出轮半径。滑动模组，导程72mm 即输出轮周长72mm，计算输出轮半径 self.pulses = kwargs.get('pulses', self.pulses) # 1圈脉冲数量 #print(type(self.pulses ),self.pulses ) ret = True # PA组参数 PA-4=0 驱动器的控制方式 0：位置控制方式 # PA-4=0 PA-14=1 PA-22=5 PA-53=1 self._write_06(reg_address=4, reg_value=0) # PA-4=0 0：位置控制方式 self._write_06(reg_address=11, reg_value=self.pulses) # PA-11=7200 设定相当于电机每旋转1圈的指令脉冲数。导程72mm，一个脉冲0.01mm 1.设定相当于电机每旋转1圈的指令脉冲数。 # P3-32=3 电机位置、指令位置、位置差、单圈绝对位置的显示方式(读取得1000H -101BH ) self._write_06(reg_address=0x0100 + 32, reg_value=3)# P3-32=3 3：上位机（PA-11）分辨率显示绝对位置。 self._write_06(reg_address=14, reg_value=3) # PA-14=3 位置指令输入方式：3:内部位置输入 #self._write_06(reg_address=22, reg_value=5) # PA-22=5 设置速度指令来源。 点动控制命令输入 5：IO端子控制点动操作 # 硬件使能SON ，必须设置PA53=0 self._write_06(reg_address=53, reg_value=0) # PA-53= 0：使能信号由 DI 输入的 SON 控制； 1 软件强制使能伺服驱动器的驱动功能 #self._write_06(reg_address=17, reg_value=6000) # PA-17=1 位置超差范围检测，超过报警 # P3组参数 # P3-30 =2 设置虚拟DI和物理DI的全部使用的方式 # P3-30=2 时，IO 输入由 DI1~DI4 和 P3-31 决定，输入 IO 数为 12 个,对应参数 # P3-0~P3-3 及 P3-38~P3-45。 self._write_06(reg_address=0x0100 + 30, reg_value=2) # P3-39=22 点动正转 P3-40=23 点动反转 self._write_06(reg_address=0x0100 + 39, reg_value=22)#虚拟 IO 输入 DI2 功能 self._write_06(reg_address=0x0100 + 40, reg_value=23)#虚拟 IO 输入 DI3 功能 # PA-55 写3 因为物理DI1、DI2接的是NPN集电极开路的接近开关 # 设置输入端子取反。按位设置物理DI1、DI2、DI3、DI4的逻辑信号。 # 不取反的端子，在开关闭合时有效，开关断开时无效；（有问题？？？，需实验确定） # 取反的端子，在开关闭合时无效，开关断开时有效。 self._write_06(reg_address=55, reg_value=0x0000) # 信号逻辑 # DI1 引脚接 NPN接近开关 # 限位动作 P3-0=34，P3-1=4，P3-2，P3-3都是0， PA20=0，PA83=0 self._write_06(reg_address=0x0100 + 0, reg_value=1) #数字输入 物理DI1 功能 SON 伺服使能输入端子。同时设置PA53=0。 self._write_06(reg_address=0x0100 + 1, reg_value=34) #数字输入 物理DI2 功能 ORGP 回归的原点 self._write_06(reg_address=0x0100 + 2, reg_value=0) #数字输入 物理DI3 功能 无功能 self._write_06(reg_address=0x0100 + 3, reg_value=0) #数字输入 物理DI4 功能 无功能 # self._write_06(reg_address=20, reg_value=0) #CWL,CCWL 驱动禁止，输入无效，或有效。这里无效。 # self._write_06(reg_address=83, reg_value=0) #CWL,CCWL 方向禁止的方式 ####归零动作####################################################################### # 归零动作 P4-32=1， P4-33=2， # P4组参数：P4-32=1 反转方向原点回归，CWL 做为回归原点 # P4-33=0 # P4-36=60 回零速度 # P4-37=30 爬行速度 # P4-0=2 2：单圈电机绝对模式 self._write_06(reg_address=0x0200 + 32, reg_value=3) # P4-32=3 3：反转方向原点回归，ORGP 做为回归原点 self._write_06(reg_address=0x0200 + 33, reg_value=2) # P4-33=2 0：找到参考原点后返回寻找单圈绝对位置零点做为机械原点； # 2：找到参考原点（上升沿或单圈绝对位置零点）作为机械原点后，减速停止。 self._write_06(reg_address=0x0200 + 34, reg_value=2) # P4-34=2 2：由原点搜寻功能（SHOM）输入接点触发原点回归功能。 # 1：电源开启时，自动执行原点回归功能 ； 2：由原点搜寻功能（SHOM）输入接点触发原点回归功能。 self._write_06(reg_address=0x0200 + 35, reg_value=0) # P4-35=0 原点停止模式设定，0：原点检测完成后，电机减速并拉回至原点。 self._write_06(reg_address=0x0200 + 36, reg_value=50) # P4-36=200 RPM回零速度 self._write_06(reg_address=0x0200 + 37, reg_value=50) # P4-37=50 RPM爬行速度 # 编码器类型和指令（相对或绝对）位置 # 自动参数 PA-62=4 单圈绝对值编码器 self._write_06(reg_address=0x0200 + 0, reg_value=2) # P4-0=0 2：绝对式位置指令，适用于 PA62 = 4 单圈编码器模式 # P3-38=33 SHOM self._write_06(reg_address=0x0100 + 38, reg_value=33) # P3-38=33 虚拟 IO 输入 DI1 功能 ####位置控制####################################################################### # P3-41=27 P3-42=28 P3-43=29 self._write_06(reg_address=0x0100 + 41, reg_value=27) # 位置暂停 虚拟 IO 输入 DI4 功能 self._write_06(reg_address=0x0100 + 42, reg_value=28) # 位置触发 虚拟 IO 输入 DI5 功能 self._write_06(reg_address=0x0100 + 43, reg_value=29) # 位置选择2 虚拟 IO 输入 DI6 功能 self._write_06(reg_address=0x0100 + 44, reg_value=0) # 没有使用 虚拟 IO 输入 DI7 功能 # P3-45=2 报警清除 self._write_06(reg_address=0x0100 + 45, reg_value=2) # 报警清除 虚拟 IO 输入 DI8 功能 ####输出端子####################################################################### # 虚拟DO输出端子（4个端子对应 P3 组参数分别为 P3-20、P3-21、P3-22、P3-23）的定义值 # P3-33的位 bit0 bit1 bit2 bit3 # 读取 P3-33 虚拟输出端子状态值 0000-1111 0000 0x0121 self._write_06(reg_address=0x0100 + 20, reg_value=2) # P3-20 =2 DO1 1 伺服主电源正常，无报警。 self._write_06(reg_address=0x0100 + 21, reg_value=9) # P3-21 =9 DO2 1 伺服器使能标志。 self._write_06(reg_address=0x0100 + 22, reg_value=16) # P3-22 =16 DO3 1 当内部位置命令完成或内部位置命令停止时，经过 P4-1 设置时间后输出信号。 self._write_06(reg_address=0x0100 + 23, reg_value=15) # P3-24 =15 DO4 1 原点回归动作完成 HOME # 设点动 PA21= 60 rpm PA40= 200ms PA41=200ms self._write_06(reg_address=21, reg_value=30)#JOG 运行速度 30 rpm self._write_06(reg_address=40, reg_value=200)#表示电机从0-1000r/min的加速时间ms self._write_06(reg_address=41, reg_value=200)#表示电机从1000-0r/min的减速时间 ms # 设位置 # 位置0设置：P4-2:0 P4-3=0 P4-4=300 定义位置1在原点位置,速度100 # P4-2 内部位置指令 1 的位置圈数设定 # P4-3 内部位置指令 1 的位置圈内脉冲数设定 # P4-4 内部位置指令控制 1 的移动速度设定 rpm self._write_06(reg_address=0x0200 + 2, reg_value=0) self._write_06(reg_address=0x0200 + 3, reg_value=0) self._write_06(reg_address=0x0200 + 4, reg_value=100) # 位置1设置：P4-5：20 P4-6=0 P4-7=100rpm 位置1在绝对坐标0圈位置，定位速度100 self._write_06(reg_address=0x0200 + 5, reg_value=0)#圈 self._write_06(reg_address=0x0200 + 6, reg_value=0) self._write_06(reg_address=0x0200 + 7, reg_value=100) time.sleep(2) # 等2s # PA-60 写1 重启伺服驱动器 self._write_06(reg_address=60, reg_value=1) # PA-60 写1 重启 time.sleep(5) # 等5s logging.info("XY motor init:{ret} ") # line =inspect.currentframe().f_lineno # self.debug_print(f"@{line}:Z motor init:{ret} ") return ret def do_SHOME(self): offset_address = 0x0080 # 暂存地址偏移量,直接操作,不写入EEROM # 触发位置1动作,即定位到位置0:0x011F先写0后写1, 即虚拟DI1 ret = self._write_06(reg_address=0x0100 + 31+offset_address, reg_value=0) ret = self._write_06(reg_address=0x0100 + 31+offset_address, reg_value=1) return ret def z_servo_on(self,**kwargs): """软件方式打开伺服驱动功能，同时驱动器自动通电流松开抱匝""" offset_address = 0x0080 # 暂存地址偏移量，直接操作，不写入EEROM self._write_06(reg_address=53+offset_address, reg_value=1) # PA-53=1 软件伺服控制器使能 SON def z_servo_off(self,**kwargs): """软件方式关闭伺服驱动功能，同时驱动器自动断电流闭合抱匝""" offset_address = 0x0080 # 暂存地址偏移量，直接操作，不写入EEROM self._write_06(reg_address=53+offset_address, reg_value=0) # PA-53=1 软件伺服控制器使能 SON def reset_encoder_zero(self): """重新设置绝对编码器的零点""" self._write_06(reg_address=0x0100 + 34, reg_value=1)#复位归零编码器多圈数据 P3 -34 self._write_06(reg_address=0x0100 + 36, reg_value=1)#设置当前位置为单圈编码器的零点 P3 -36 def goto(self, **kwargs): """执行器达指定位 输入参数 speed: RPM 输入参数 displace: 绝对坐标,弧度 或 米 ,不小于0 输入参数 acc: ms,变化1000RPM的耗时 按照圈速控制电机, 正负数 # 圈数限制:-30000- 30000 # 脉冲限制:+/-max .cnt/rev """ """电机按照梯形速度曲线运动,位置1""" self.speed = kwargs.get('speed', 0) #RPM self.displace = kwargs.get('displace', 0.0) #joint 转角或位移 self.acc = kwargs.get('acc', 200) #变化1000RPM的耗时缺省值200ms,电机加速度的设定数,可能不等于加速度 circles = self.displace/self.radius/ math.pi/2 # 关节转动周数 angle2pulses = int(circles *self.pulses*self.ratio)# 转换为脉冲总数 #计算距离对应的脉冲数 distance_pulse_counter = angle2pulses # 处理转速,得正整数 rpm = math.fabs(self.speed) #正整数 rpm = int(rpm) _servo_modbus_rtu = self if math.isnan(distance_pulse_counter) or math.isinf(distance_pulse_counter): self.get_logger().warn(f"运行圈数量超过正常范围:{distance_pulse_counter} ") return False cycle_pulse = self.pulses # 一圈的脉冲数 sign = 1 if distance_pulse_counter<0: sign = -1 cycles = (sign* distance_pulse_counter)//cycle_pulse # 去符号,得正数 if cycles > 30000 or cycles < -30000: self.get_logger().warn(f"运行整圈数量超过正常范围:{distance_pulse_counter} ") return False else: # 求圈数 circles = self.get_16bit_twos_complement_ctypes(cycles*sign) # 求不足一圈的部分的脉冲数 circles_fractional =(sign* distance_pulse_counter)%cycle_pulse # 去符号,得正数 pulses = self.get_16bit_twos_complement_ctypes(circles_fractional*sign) # 恢复符号 offset_address = 0x0000 # 暂存地址偏移量,直接操作,不写入EEROM # 位置1设置:P4-5:20 P4-6=0 P4-7=60 位置1在绝对坐标circles=20圈位置,不足一圈的脉冲数pulses=0,定位速度rpm=60 self._write_06(reg_address=0x0200 + 5+offset_address, reg_value= circles) #圈数量 self._write_06(reg_address=0x0200 + 6+offset_address, reg_value= pulses) #不足一圈的脉冲数 self._write_06(reg_address=0x0200 + 7+offset_address, reg_value= rpm) #转速 RPM # 加减速度。适用于速度控制、内部位置控制。 self._write_06(reg_address=40+offset_address, reg_value=self.acc)#加速时间ms,适用于速度控制、内部位置控制。变化1000RPM的时间self.acc ms self._write_06(reg_address=41+offset_address, reg_value=self.acc)#减速时间ms,变化1000RPM的时间self.acc ms # 触发位置1动作,即定位到位置1:0x011F先写0后写48（32+16） offset_address =0x0080 self._write_06(reg_address=0x0100 + 31+offset_address, reg_value=0) self._write_06(reg_address=0x0100 + 31+offset_address, reg_value=48) return True def fdbk(self, **kwargs): """执行器反馈值""" """读取虚拟DO口,获得电机运动状态反馈值: 0b0000_0001 # P3-20 =2 1 伺服器正常 0b0000_0010 # P3-21 =3 1 无报警。 0b0000_0100 # P3-22 =16 1 当内部位置命令完成或内部位置命令停止时，经过 P4-1 设置时间后输出信号。 0b0000_1000 # P3-23 =15 1 原点回归动作完成 """ code = 0x0000 regisers = self._read_04(reg_address=0x1010, reg_count=1) try: code = regisers[0] except IndexError as e: logging.warn(f'{e}') return 0 self.status_dict["POWER_ON"] = (0b00000001&code ==0b00000001) #电源现正常 self.status_dict["SERVO_ON"] = (0b00000010&code ==0b00000010) #电机SON self.status_dict["REACH_ON"] = (0b00000100&code ==0b00000100) #已达到位置 self.status_dict["HOMED_OK"] = (0b00001000&code ==0b00001000) #已作回零点 return code def speed_rpm(self): """返回运动的RPM""" registers = self._read_04(reg_address=0x1000, reg_count=1) try: bits = registers[0] # 显示绝对位置值的 15bit~ 0bit rpm = self.twos_complement_fast(bits, 16) return rpm except IndexError: return None def postion(self): """返回运动的绝对位置""" registers = self._read_04(reg_address=0x1018, reg_count=4) try: pulses = registers[3] # 显示绝对位置值的 63bit~48bit pulses = (pulses<<16) + registers[2] # 显示绝对位置值的 47bit~32bit pulses = (pulses<<16) + registers[1] # 显示绝对位置值的 31bit~16bit bits = (pulses<<16) + registers[0] # 显示绝对位置值的 15bit~ 0bit pulses = self.twos_complement_fast(bits, 64) circles = float(pulses)/self.pulses #return circles displace = circles * math.pi * 2 *self.radius/self.ratio return displace except IndexError: return None def readRegisterValue(self,**kwargs): """读取制定寄存器组： """ reg_address = kwargs.get('reg_address', 4) #地址 reg_count = kwargs.get('reg_count', 1) # registers = self._read_04(reg_address =reg_address,reg_count=reg_count) try: value = 0 for i in range(reg_count): value = value<<16 value = registers[reg_count-1-i] num = reg_address&0x00FF group = reg_address&0x0F00 if group == 0x0000: print(f'PA-{num}@{reg_address} is {value}') elif group == 0x0100: print(f'P3-{num}@{reg_address} is {value}') elif group == 0x0200: print(f'P4-{num}@{reg_address} is {value}') else: print("input params is wrong") except IndexError: return def returnZero(self, **kwargs): """执行器归回零位""" """""" ret = True speed = kwargs.get('speed', 100) #RPM offset_address = 0x0080 # 暂存地址偏移量,直接操作,不写入EEROM ret = self._write_06(reg_address=0x0200 + 4+offset_address, reg_value=speed) # 触发位置1动作,即定位到位置0:0x011F先写0后写16, 即虚拟DI5 offset_address = 0x0000 ret = self._write_06(reg_address=0x0100 + 31+offset_address, reg_value=0) ret = self._write_06(reg_address=0x0100 + 31+offset_address, reg_value=16) return ret def _write_06(self, **kwargs): reg_address = kwargs.get('reg_address', None) reg_value = kwargs.get('reg_value', None) # 通信 response = self.send_request(address= reg_address,value= reg_value,function_code=0x06,slave_id=self.slave_id) while not response.success:# 阻塞 time.sleep(1) #self.debug_print(f"modbus fun=0x06 com failure: {response.message}") logging.error(f"modbus fun=0x06 com failure: {response.message}") return response.success def _read_04(self,reg_address =0x1000,reg_count=3): """ROS2服务,Modbus-RTU,功能码3,读取输入寄存器组""" #在Modbus协议中,功能码0x04(十六进制表示)用于读取输入寄存器(Input Registers) ret = True while ret: # 读取输入寄存器组 response = self.send_request( slave_id=self.slave_id, address=reg_address, count= reg_count, function_code=0x04 #(0x04): 读取输入寄存器组 ) ret = response.success if not ret:# 阻塞 logging.error(f"Z motor com failure: {response.message}") #self.debug_print(f"Z motor com failure: {response.message}") # 短暂休眠避免CPU占用过高 time.sleep(0.2) ret = True continue else: logging.info(f"Z motor com success: {response.message}") #self.debug_print(f"Z motor com success: {response.message}") break if ret: return response.registers else: return []# 失败,返回空表 @staticmethod def twos_complement_fast(bits, bit_length): """bits 补码数 bit_length位数""" sign_bit = 1 << (bit_length - 1) return (bits & (sign_bit - 1)) - (bits & sign_bit) @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 twos_complement_to_int32(value): """ 将 32 位补码数转换为有符号整数 :param value: 32 位无符号整数 (0 到 4,294,967,295) :return: 有符号整数 (-2,147,483,648 到 2,147,483,647) """ # 检查最高位(符号位) if value & 0x80000000: # 如果是负数:取反加一,然后添加负号 return -(~(value - 1) & 0xFFFFFFFF) else: # 如果是正数:直接返回 return value @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 class servo_joint_node(Node): def __init__(self, **kwargs): node_name = kwargs.get('node_name', "zjoint_nd") Node.__init__(self,node_name)#super().__init__('my_python_node') if not self.has_parameter('use_sim_time'): self.declare_parameter('use_sim_time', False) else: self.set_parameters([rclpy.parameter.Parameter('use_sim_time', rclpy.Parameter.Type.BOOL, False)]) # 创建两个不同的回调组 self.group1 = MutuallyExclusiveCallbackGroup() # 互斥组 #self.group2 = ReentrantCallbackGroup() # 可重入组 self.group2 = MutuallyExclusiveCallbackGroup() # 互斥组 ############################################### # Z 轴 驱动电机 # 声明参数并根据**kwargs设置,并给定默认值 self.slave_id = kwargs.get('slave_id', 1) #,多个逗号,变成了元组# 驱动器通信地址 self.ratio = kwargs.get('ratio', 1) #,直接驱动,无减速机 self.radius = kwargs.get('radius', 0.011459155902616465) #,滑动模组,导程72mm 即周长72mm,输出轮半径 self.pulses_per_rotate = kwargs.get('pulses_per_rotate', 7200) #,# 转1周的脉冲数量 72mm/7200 0.01mm # 驱动减速电机 self.motor =PFDE_SERVO_CTL(slave_id =self.slave_id ,ratio =self.ratio,radius=self.radius,pulese=self.pulses_per_rotate) # ROS2 service 实现Modbus通信,作为客户端 self.modbus_svr_cli = None # 连接 ros2 service client self.motor.modbus_srv_init = self.modbus_srv_init # 方法函数名 self.motor.modbus_srv_request = self.modbus_srv_request # 方法函数名 self.motor.debug_print = self.nd_print # 方法函数名 def modbus_srv_init(self, **kwargs): """ROS2服务通信连接""" # 作为客户端 向服务器提出请求 request,等待响应response self.modbus_svr_cli= self.create_client(ModbusRTU, '/modbus_rtu_service',callback_group=self.group2) while not self.modbus_svr_cli.wait_for_service(timeout_sec=1.0): logging.info(f'Start, Waiting the on_line modbus servor ...') time.sleep(0.2)# 等0.2s logging.info(f'conected!') def modbus_srv_request(self, slave_id, address, count=0, value=0, function_code=3,registers=[]): """服务的回调函数""" times = 0 while times <2:# 容错 可以通信2次 request = ModbusRTU.Request() request.slave_id = slave_id request.address = address request.count = count request.value = value request.function_code = function_code request.registers = registers # 同步调用service future = self.modbus_svr_cli.call_async(request) rclpy.spin_until_future_complete(self, future) # 异步调用service # future = self.client.call_async(request) # future.add_done_callback(self.service_response_callback) #通信是否成功 response = future.result() ret = response.success if ret: break times =times +1 time.sleep(0.1) return response def nd_print(self,msg): logging.info(msg) if __name__ == "__main__": # #初始化 Node 基类 if not rclpy.ok():#在初始化节点之前,先确保rclpy已经初始化。 rclpy.init()#在初始化节点之前,先确保rclpy已经初始化。 #############Z轴电机################################################## z_joint_nd =servo_joint_node(node_name="z_joint_nd",slave_id = 4) z_joint_nd.motor.debug_print('test print ...') z_joint_nd.motor.connect()# 通信ROS服务连接 #z_joint_nd.motor.init_Z2() # Z轴 伺服电机驱动器初始化 # 首先检查对象是否存在 print(z_joint_nd.motor) # 应该是 None 还是有实际对象？ # 设置零坐标 # z_joint_nd.motor.reset_encoder_zero()#编码器设零点 # z_joint_nd.motor.reset() # 重启伺服电机驱动器 #清除故障ERR54 使用 # z_joint_nd.motor.clear_err() # z_joint_nd.motor.reset() # 清除错误并重新启动伺服驱动器 # 读取指定地址的多个寄存器 # z_joint_nd.motor.readRegisterValue(reg_address =62,reg_count=1)#读取指定地址的多个寄存器 # z_joint_nd.motor.readRegisterValue(reg_address =0x0200 + 5,reg_count=1)#0x0200 + 5 #归零点 # z_joint_nd.motor.do_SHOME() # while not z_joint_nd.motor.status_dict["HOMED_OK"]: # code = z_joint_nd.motor.fdbk() # print(z_joint_nd.motor.speed_rpm()) # print(f'状态BIT值:0B{code:08b}') # time.sleep(0.5) # z_joint_nd.motor.goto(speed=100,displace=0.80,acc=200) # code = z_joint_nd.motor.fdbk() # print(f'状态BIT值:0B{code:08b}') # while not z_joint_nd.motor.status_dict["REACH_ON"]: # code = z_joint_nd.motor.fdbk() # print(z_joint_nd.motor.speed_rpm()) # print(f'状态BIT值:0B{code:08b}') # time.sleep(0.5) # print(z_joint_nd.motor.postion()) # 当前距离坐标 #############Y轴电机################################################## y_joint_nd =servo_joint_node(node_name="y_joint_nd",slave_id = 3) y_joint_nd.motor.debug_print('test print ...') y_joint_nd.motor.connect()# 通信ROS服务连接 y_joint_nd.motor.init_XY() # XY轴 伺服电机驱动器初始化 # 首先检查对象是否存在 print(y_joint_nd.motor) # 应该是 None 还是有实际对象？ # 设置零坐标 没有用 # y_joint_nd.motor.reset_encoder_zero()#编码器设零点 # y_joint_nd.motor.reset() # 重启伺服电机驱动器 #清除故障ERR54 使用 # y_joint_nd.motor.clear_err() # y_joint_nd.motor.reset() # 清除错误并重新启动伺服驱动器 # 读取指定地址的多个寄存器 # z_joint_nd.motor.readRegisterValue(reg_address =62,reg_count=1)#读取指定地址的多个寄存器 # z_joint_nd.motor.readRegisterValue(reg_address =0x0200 + 5,reg_count=1)#0x0200 + 5 #归零点 y_joint_nd.motor.do_SHOME() while not y_joint_nd.motor.status_dict["HOMED_OK"]: code = y_joint_nd.motor.fdbk() print(y_joint_nd.motor.speed_rpm()) print(f'状态BIT值:0B{code:08b}') time.sleep(0.5) y_joint_nd.motor.goto(speed=100,displace=1.20,acc=200) code = y_joint_nd.motor.fdbk() print(f'状态BIT值:0B{code:08b}') while not y_joint_nd.motor.status_dict["REACH_ON"]: code = y_joint_nd.motor.fdbk() print(y_joint_nd.motor.speed_rpm()) print(f'状态BIT值:0B{code:08b}') time.sleep(0.5) print(y_joint_nd.motor.postion()) # 当前距离坐标
浙公网安备 33010602011771号