You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
118 lines
4.3 KiB
118 lines
4.3 KiB
from pymodbus.client import ModbusTcpClient
|
|
from pymodbus.exceptions import ModbusException
|
|
|
|
class RobotGripper:
|
|
def __init__(self, ip, port=502):
|
|
"""
|
|
Инициализация класса управления захватом робота.
|
|
:param ip: IP-адрес устройства
|
|
:param port: Порт Modbus TCP (по умолчанию 502)
|
|
"""
|
|
self.ip = ip
|
|
self.port = port
|
|
self.client = ModbusTcpClient(ip, port)
|
|
self.connected = False
|
|
|
|
def connect(self):
|
|
"""Подключение к захвату"""
|
|
try:
|
|
self.client.connect()
|
|
self.connected = True
|
|
except ModbusException as e:
|
|
print(f"Ошибка подключения: {e}")
|
|
|
|
def disconnect(self):
|
|
"""Отключение от захвата"""
|
|
if self.connected:
|
|
self.client.close()
|
|
self.connected = False
|
|
|
|
def initialize(self, full=False):
|
|
"""
|
|
Инициализация захвата.
|
|
:param full: Если True, выполнить полную инициализацию
|
|
:return: Ответ устройства
|
|
"""
|
|
if not self.connected:
|
|
raise ModbusException("Нет подключения к захвату.")
|
|
|
|
value = 0xA5 if full else 0x01
|
|
address = 0x0100
|
|
response = self.client.write_register(address, value, slave=1)
|
|
return response
|
|
|
|
def set_force(self, force):
|
|
"""
|
|
Установка силы захвата.
|
|
:param force: Значение силы (20-100%)
|
|
:return: Ответ устройства
|
|
"""
|
|
if not 20 <= force <= 100:
|
|
raise ValueError("Сила захвата должна быть в диапазоне 20-100%.")
|
|
|
|
if not self.connected:
|
|
raise ModbusException("Нет подключения к захвату.")
|
|
|
|
address = 0x0101
|
|
response = self.client.write_register(address, force, slave=1)
|
|
return response
|
|
|
|
def set_position(self, position):
|
|
"""
|
|
Установка позиции захвата.
|
|
:param position: Значение позиции (0-1000)
|
|
:return: Ответ устройства
|
|
"""
|
|
if not 0 <= position <= 1000:
|
|
raise ValueError("Позиция должна быть в диапазоне 0-1000.")
|
|
|
|
if not self.connected:
|
|
raise ModbusException("Нет подключения к захвату.")
|
|
|
|
address = 0x0103
|
|
response = self.client.write_register(address, position, slave=1)
|
|
return response
|
|
|
|
def read_register(self, address):
|
|
"""
|
|
Чтение значения из регистра.
|
|
:param address: Адрес регистра
|
|
:return: Значение регистра
|
|
"""
|
|
if not self.connected:
|
|
raise ModbusException("Нет подключения к захвату.")
|
|
|
|
response = self.client.read_holding_registers(address, count=1, slave=1)
|
|
if response.isError():
|
|
raise ModbusException(f"Ошибка чтения регистра {address}.")
|
|
return response.registers[0]
|
|
|
|
def get_status(self):
|
|
"""
|
|
Получение статуса захвата.
|
|
:return: Словарь статусов (инициализация, состояние захвата, позиция)
|
|
"""
|
|
if not self.connected:
|
|
raise ModbusException("Нет подключения к захвату.")
|
|
|
|
status = {
|
|
"initialization_state": self.read_register(0x0200),
|
|
"gripper_state": self.read_register(0x0201),
|
|
"current_position": self.read_register(0x0202)
|
|
}
|
|
return status
|
|
|
|
# Пример использования
|
|
if __name__ == "__main__":
|
|
gripper = RobotGripper("10.27.1.80")
|
|
try:
|
|
gripper.connect()
|
|
gripper.initialize(full=True)
|
|
gripper.set_force(50)
|
|
gripper.set_position(800)
|
|
status = gripper.get_status()
|
|
print("Статус захвата:", status)
|
|
except Exception as e:
|
|
print(f"Ошибка: {e}")
|
|
finally:
|
|
gripper.disconnect() |