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()