########################################################################### # # Copyright (c) 2022-2024 Diality Inc. - All Rights Reserved. # # THIS CODE MAY NOT BE COPIED OR REPRODUCED IN ANY FORM, IN PART OR IN # WHOLE, WITHOUT THE EXPLICIT PERMISSION OF THE COPYRIGHT OWNER. # # @file ejector.py # # @author (last) Micahel Garthwaite # @date (last) 10-Mar-2023 # @author (original) Micahel Garthwaite # @date (original) 01-Nov-2024 # ############################################################################ import struct from logging import Logger from leahi_dialin.common.constants import NO_RESET from leahi_dialin.common.msg_defs import MsgIds, MsgFieldPositions from leahi_dialin.common.override_templates import cmd_generic_broadcast_interval_override, cmd_generic_override from leahi_dialin.common.td_defs import td_enum_repository from leahi_dialin.protocols.CAN import DenaliChannels from leahi_dialin.utils.base import AbstractSubSystem, publish from leahi_dialin.utils.conversions import integer_to_bytearray, float_to_bytearray class TDEjector(AbstractSubSystem): """ TDEjector Treatment Delivery (TD) Dialin API sub-class for ejector motor related commands. """ def __init__(self, can_interface, logger: Logger): """ @param can_interface: Denali Can Messenger object """ super().__init__() self.can_interface = can_interface self.logger = logger if self.can_interface is not None: channel_id = DenaliChannels.td_sync_broadcast_ch_id self.msg_id_td_ejector_data = MsgIds.MSG_ID_TD_EJECTOR_DATA.value self.can_interface.register_receiving_publication_function(channel_id, self.msg_id_td_ejector_data, self._handler_ejector_sync) self.h5_motor = { td_enum_repository.TDEjectorNames.H5_MOTOR.name: { td_enum_repository.TDEjectorAttributes.STATE.name: 0, td_enum_repository.TDEjectorAttributes.SET_SPEED.name: 0.0, td_enum_repository.TDEjectorAttributes.OPT_SNSR_RETRACT: 0, # H25 Optical Sensor td_enum_repository.TDEjectorAttributes.OPT_SNSR_ENGAGE: 0 # H24 Optical Sensor } } self.td_ejector_timestamp = 0 @publish(["msg_id_td_ejector_data", "h5_motor", "td_ejector_timestamp"]) def _handler_ejector_sync(self, message, timestamp=0.0): """ Handles published ejector data messages. @param message: published ejector data message as: ejector state @return: None """ self.h5_motor[td_enum_repository.TDEjectorNames.H5_MOTOR.name][td_enum_repository.TDEjectorAttributes.STATE.name] = struct.unpack('i', bytearray( message['message'][MsgFieldPositions.START_POS_FIELD_1:MsgFieldPositions.END_POS_FIELD_1]))[0] self.h5_motor[td_enum_repository.TDEjectorNames.H5_MOTOR.name][td_enum_repository.TDEjectorAttributes.SET_SPEED.name] = struct.unpack('f', bytearray( message['message'][MsgFieldPositions.START_POS_FIELD_2:MsgFieldPositions.END_POS_FIELD_2]))[0] self.h5_motor[td_enum_repository.TDEjectorNames.H5_MOTOR.name][td_enum_repository.TDEjectorAttributes.OPT_SNSR_RETRACT.name] = struct.unpack('i', bytearray( message['message'][MsgFieldPositions.START_POS_FIELD_3:MsgFieldPositions.END_POS_FIELD_3]))[0] self.h5_motor[td_enum_repository.TDEjectorNames.H5_MOTOR.name][td_enum_repository.TDEjectorAttributes.OPT_SNSR_ENGAGE.name] = struct.unpack('i', bytearray( message['message'][MsgFieldPositions.START_POS_FIELD_4:MsgFieldPositions.END_POS_FIELD_4]))[0] self.td_ejector_timestamp = timestamp def cmd_ejector_data_broadcast_interval_override(self, ms: int, reset: int = NO_RESET) -> int: """ Constructs and sends the ejector motor data broadcast interval override command Constraints: Must be logged into TD. Given interval must be non-zero and a multiple of the TD general task interval (50 ms). @param ms: integer - interval (in ms) to override with @param reset: integer - 1 to reset a previous override, 0 to override @return: 1 if successful, zero otherwise """ return cmd_generic_broadcast_interval_override( ms = ms, reset = reset, channel_id = DenaliChannels.dialin_to_td_ch_id, msg_id = MsgIds.MSG_ID_TD_EJECTOR_PUBLISH_INTERVAL_OVERRIDE_REQUEST, module_name = 'TD Ejector', logger = self.logger, can_interface = self.can_interface) def cmd_ejector_request_command(self, cmd: int) -> int: """ Constructs and sends the ejector request command. EJECTOR_OPERATION_HOME = 0, ///< Ejector home operation EJECTOR_OPERATION_RETRACT, ///< Ejector retract operation EJECTOR_OPERATION_EXTEND, ///< Ejector extend operation Constraints: Must be logged into TD. @param cmd: integer - ID of an ejector operation that is being requested @return: 1 if successful, zero otherwise """ idx = integer_to_bytearray(cmd) payload = idx return cmd_generic_override( payload = payload, reset = NO_RESET, channel_id = DenaliChannels.dialin_to_td_ch_id, msg_id = MsgIds.MSG_ID_TD_EJECTOR_COMMAND, entity_name = f'TD Ejector state', override_text = f'{str(cmd)}', logger = self.logger, can_interface = self.can_interface) def cmd_ejector_set_motor_speed_request(self, speed: int) -> int: """ Constructs and sends the ejector set speed command. Negative values imply reverse ( retract ) direction. Constraints: Must be logged into TD. @param speed: float - value to set the motor speed in RPM. @return: 1 if successful, zero otherwise """ spd = float_to_bytearray(speed) payload = spd return cmd_generic_override( payload = payload, reset = NO_RESET, channel_id = DenaliChannels.dialin_to_td_ch_id, msg_id = MsgIds.MSG_ID_TD_EJECTOR_MOTOR_SET_SPEED_REQUEST, entity_name = f'TD Ejector speed', override_text = f'{str(speed)}', logger = self.logger, can_interface = self.can_interface) def cmd_ejector_optical_sensor_override(self, sensor: int, sensor_active: int, reset: int = NO_RESET) -> int: """ Constructs and sends the ejector optical sensor override command . Constraints: Must be logged into TD. Optical sensor retract ID = 0 Optical sensor engage ID = 1 Sensor active (engage) = 0 Sensor inactive (retract) = 1 @param sensor: int - sensor ID @param sensor_active: int - override to be active or inactive @param reset: integer - 1 to reset a previous override, 0 to override @return: 1 if successful, zero otherwise """ rst = integer_to_bytearray(reset) sen = integer_to_bytearray(sensor) active = integer_to_bytearray(sensor_active) payload = rst + active + sen sensor_name = 'H25 Retract' if sensor == 0 else 'H24 Engage' return cmd_generic_override( payload = payload, reset = NO_RESET, channel_id = DenaliChannels.dialin_to_td_ch_id, msg_id = MsgIds.MSG_ID_TD_EJECTOR_OPT_SENSOR_OVERRIDE_REQUEST, entity_name = f'TD Ejector {sensor_name} Optical Sensor', override_text = f'{str(sensor_active)}', logger = self.logger, can_interface = self.can_interface)