41 lines
1.3 KiB
Python
41 lines
1.3 KiB
Python
import time
|
|
from dataclasses import dataclass
|
|
|
|
from drivers.modbus_device import ModbusDevice, ModbusDeviceError
|
|
|
|
|
|
@dataclass
|
|
class Position:
|
|
x: int
|
|
y: int
|
|
z: int
|
|
|
|
def __str__(self):
|
|
return f"(x={self.x}, y={self.y}, z={self.z})mm"
|
|
|
|
|
|
class JigMovementError(ModbusDeviceError):
|
|
pass
|
|
|
|
|
|
class Jig(ModbusDevice):
|
|
def __init__(self, port, registers, slave_id=0x10):
|
|
super().__init__(port, "Jig", registers, slave_id=slave_id)
|
|
|
|
def move_to_position(self, position, wait_time=5):
|
|
try:
|
|
self.write_holding_register(self._addr("X POSITION"), position.x)
|
|
self.write_holding_register(self._addr("Y POSITION"), position.y)
|
|
self.write_holding_register(self._addr("Z POSITION"), position.z)
|
|
self.write_coil(self._addr("MOVE TO POSITION"), True, verify=False)
|
|
if wait_time > 0:
|
|
time.sleep(wait_time)
|
|
except ModbusDeviceError as exc:
|
|
raise JigMovementError(f"failed to move to {position}") from exc
|
|
|
|
def get_current_position(self):
|
|
x = self.read_holding_register(self._addr("X POSITION"))
|
|
y = self.read_holding_register(self._addr("Y POSITION"))
|
|
z = self.read_holding_register(self._addr("Z POSITION"))
|
|
return Position(x, y, z)
|