def _parse_devices(self) -> None:
data = self._device_data
self._devices = []
for datum in data:
device_id = int(datum['id'])
name = datum['name'].split(' ', maxsplit=1)[1]
pos_x = float(datum['x'])
pos_y = float(datum['y'])
pos_z = float(datum['z'])
pos_p = float(datum['p'])
pos_t = float(datum['t'])
device_bounds = BoundingBox(
vec3(float(datum['min_x']), float(datum['min_y']),
float(datum['min_z'])),
vec3(float(datum['max_x']), float(datum['max_y']),
float(datum['max_z'])))
size = vec3(float(datum['size_x']), float(datum['size_y']),
float(datum['size_z']))
device_type = datum['type']
interfaces = datum['interfaces'].splitlines()
port = datum['port']
device = Device(device_id=device_id,
device_name=name,
device_type=device_type,
interfaces=interfaces,
position=Point5(pos_x, pos_y, pos_z, pos_p, pos_t),
initial_position=Point5(pos_x, pos_y, pos_z, pos_p,
pos_t),
device_bounds=device_bounds,
collision_bounds=size,
port=port)
self._devices.append(device)
def start(self):
"""Starts the controller as a result of connecting to it via serial."""
self._is_running = True
response_thread = threading.Thread(target=self._respond,
daemon=True,
name='response thread')
self._is_absolute_move_mode = [True] * 3
self._is_locked = [True] * 3
self._last_positions = [Point5()] * 3
now = datetime.datetime.now()
startup_start = [
'settings read from EEPROM', '**COPIS**',
'Version: SZRC_RC6 Tue Jun 08 15:38:58 2021', 'Device ID: 0',
'2 connected', 'id:1;state:255', 'id:2;state:255'
]
startup_end = ['9860 bytes available', 'COPIS_READY']
for line in startup_start:
resp = self._COPIS_RESPONSE(payload=line, report_on=now)
self._response_buffer.append(resp)
for i in range(len(self._last_positions)):
resp = self._COPIS_RESPONSE(
payload=self._get_formatted_response(i), report_on=now)
self._response_buffer.append(resp)
for line in startup_end:
resp = self._COPIS_RESPONSE(payload=line, report_on=now)
self._response_buffer.append(resp)
response_thread.start()
class SerialResponse:
"""Data structure that implements a parsed COPIS serial response."""
device_id: int = -1
system_status_number: int = -1
position: Point5 = Point5()
error: str = None
@property
def system_status_flags(self) -> str:
"""Returns the system status flags, on per binary digit."""
return f'{self.system_status_number:08b}' \
if self.system_status_number >= 0 else None
@property
def is_idle(self) -> bool:
"""Returns a flag indicating where the serial connection is idle."""
return self.system_status_number == 0
def _parse_response(self, resp) -> object:
line = resp.strip('\r\n')
if self._OBJECT_PATTERN.match(line):
result = SerialResponse()
for pair in self._PAIR_PATTERN.findall(line):
for key_val in self._KEY_VAL_PATTERN.findall(pair.rstrip(',')):
key = self._KEY_MAP[key_val[0]]
value = key_val[1]
if (key in ['device_id', 'system_status_number']):
value = int(value)
elif key == 'position':
x, y, z, p, t = [float(v) for v in [*key_val[1].split(',')]]
value = Point5(x, y, z, p ,t)
setattr(result, key, value)
return result
return line
def execute(self, cmd: bytes) -> int:
"""Executs a command written to the controller from serial."""
def to_dict(args):
obj = {
'x': None,
'y': None,
'z': None,
'p': None,
't': None,
'f': None,
's': None,
'v': None
}
for arg in args:
key = arg[0].lower()
val = arg[1]
val = 0.0 if not is_number(val) else float(val)
obj[key] = val
return obj
if cmd and len(cmd) > 0:
cmds = cmd.decode().strip('\r ').split('\r')
actions = [deserialize_command(c) for c in cmds]
pos = 'xyzpt'
for action in actions:
x, y, z, p, t = self._last_positions[action.device]
prev_position = {'x': x, 'y': y, 'z': z, 'p': p, 't': t}
position = prev_position.copy()
feedrate = None
action_timespan = self._DEFAULT_ACTION_TIMESPAN
data = None if action.argc == 0 else to_dict(action.args)
if self._is_locked[
action.device] and action.atype != ActionType.M511:
pass
elif action.atype in self._MODE_COMMANDS:
self._is_absolute_move_mode[
action.device] = action.atype == ActionType.G90
elif action.atype in self._RESET_COMMANDS:
for key in pos:
if data[key] is not None:
position[key] = data[key]
elif action.atype in self._CAMERA_COMMANDS:
if data['p']:
action_timespan = data['p'] * self._MSS_TO_SECS_RATIO
elif data['s']:
action_timespan = data['s']
else:
action_timespan = self._DEFAULT_SHUTTER_PRESS
elif action.atype in self._MOVE_COMMANDS:
if action.atype == ActionType.G1:
feedrate = self._MAX_FEEDRATE / 2 \
if data['f'] is None else min(data['f'], self._MAX_FEEDRATE)
for key in pos:
if data[key] is not None:
if self._is_absolute_move_mode[action.device]:
position[key] = data[key]
else:
position[key] = position[key] + data[key]
elif action.atype in self._HOME_COMANDS:
if data['f']:
feedrate = min(data['f'], self._MAX_FEEDRATE)
for key in pos:
if data[key] is not None:
low_bound = self._BOUNDS[key][0]
hi_bound = self._BOUNDS[key][1]
start = 0 if low_bound == 0 else random.randrange(
low_bound, 0)
finish = random.randrange(0, hi_bound)
position[key] = finish - start
elif action.atype == ActionType.M511:
self._is_locked[
action.device] = not self._is_locked[action.device]
elif action.atype == ActionType.M18:
# Disengage motors.
pass
else:
raise NotImplementedError(
f'Action {action.atype} not implemented.')
now = datetime.datetime.now()
if self._is_locked[action.device]:
resp = self._COPIS_RESPONSE(
payload=self._get_formatted_response(action.device),
report_on=now)
self._response_buffer.append(resp)
else:
delta = list(map(lambda start, end: abs(end - start), \
prev_position.values(), position.values()))
max_delta = max(delta)
travel_time = action_timespan
if feedrate and max_delta > 0:
travel_time = self._MINS_TO_SECS_RATIO * max_delta / feedrate
if action.atype == ActionType.G28:
for key in pos:
if data[key] is not None:
position[key] = 0.0
x, y, z, p, t = position.values()
start_resp = self._COPIS_RESPONSE(
payload=self._get_formatted_response(
action.device, False),
report_on=now)
self._last_positions[action.device] = Point5(x, y, z, p, t)
end_resp = self._COPIS_RESPONSE(
payload=self._get_formatted_response(
action.device, True),
report_on=now + datetime.timedelta(0, travel_time))
self._response_buffer.extend([start_resp, end_resp])
return sys.getsizeof(cmd)
class Device:
"""Data structure that implements a COPIS instrument imaging device."""
device_id: int = 0
device_name: str = ''
device_type: str = ''
interfaces: Optional[List[str]] = None
position: Point5 = Point5()
initial_position: Point5 = Point5()
max_feed_rates: Point5 = Point5()
device_bounds: BoundingBox = BoundingBox(vec3(inf), vec3(-inf))
collision_bounds: vec3 = vec3()
port: str = ''
_serial_response: SerialResponse = None
is_homed: bool = False
_is_writing: bool = False
_last_reported_on: datetime = None
@property
def is_writing(self) -> bool:
"""Returns the device's IsWriting flag."""
return self._is_writing
@property
def serial_response(self) -> SerialResponse:
"""Returns the device's last serial response."""
return self._serial_response
@property
def last_reported_on(self) -> datetime:
"""Returns the device's last serial response date."""
return self._last_reported_on
@property
def serial_status(self) -> ComStatus:
"""Returns the device's serial status based on its last serial response."""
if self.serial_response is None:
if self.is_homed:
return ComStatus.IDLE
return ComStatus.BUSY if self._is_writing else ComStatus.UNKNOWN
if self.serial_response.error:
return ComStatus.ERROR
if self._is_writing or not self.serial_response.is_idle:
return ComStatus.BUSY
if self.serial_response.is_idle:
return ComStatus.IDLE
raise ValueError('Unsupported device serial status code path.')
def set_is_writing(self) -> None:
"""Sets the device's IsWriting flag."""
self._is_writing = True
def set_serial_response(self, response: SerialResponse) -> None:
"""Sets the device's serial response."""
self._serial_response = response
if response:
self._last_reported_on = datetime.now()
else:
self._last_reported_on = None
self._is_writing = False
def as_dict(self):
"""Returns a dictionary representation of a Device instance."""
data = {
f'Camera {self.device_name}': {
'id': self.device_id,
'x': self.position.x,
'y': self.position.y,
'z': self.position.z,
'p': self.position.p,
't': self.position.t,
'min_x': self.device_bounds.lower.x,
'max_x': self.device_bounds.upper.x,
'min_y': self.device_bounds.lower.y,
'max_y': self.device_bounds.upper.y,
'min_z': self.device_bounds.lower.z,
'max_z': self.device_bounds.upper.z,
'size_x': self.collision_bounds.x,
'size_y': self.collision_bounds.y,
'size_z': self.collision_bounds.z,
'type': self.device_type,
'interfaces': '\n'.join(self.interfaces),
'port': self.port
}
}
return data
def on_device_deselected(self) -> None:
"""On core_d_deselected, clear and disable controls."""
self.update_machine_pos(Point5())
self.Disable()