def decode(data): """Decodes message data into alpha and beta moves.""" alpha_steps = bytes_to_int(data[0:4], dtype="i4") beta_steps = bytes_to_int(data[4:8], dtype="i4") return motor_steps_to_angle(alpha_steps, beta_steps)
def getPosition(posID): sendMsg(GET_ACTUAL_POSITION, posID) arb, data = getReply() assert arb[0] == posID beta = bytes_to_int(data[4:]) alpha = bytes_to_int(data[0:4]) return alpha, beta
def gotoPosition(posID, alphaDeg, betaDeg): alpha_steps, beta_steps = motor_steps_to_angle(alphaDeg, betaDeg, inverse=True) data = int_to_bytes(alpha_steps) + int_to_bytes(beta_steps) sendMsg(GO_TO_ABSOLUTE_POSITION, posID, data) arb, data = getReply() betaTime = bytes_to_int(data[4:]) alphaTime = bytes_to_int(data[0:4]) assert arb[0] == posID print("times to move", betaTime * 0.0005, alphaTime * 0.0005)
def get_positioner_status(self) -> Dict[int, int]: """Returns the status flag for each positioner that replied.""" return { reply.positioner_id: bytes_to_int(reply.data) for reply in self.replies }
def get_currents(self) -> Dict[int, Tuple[int, int]]: """Returns a dictionary of current of alpha and beta for each positioner. Raises ------ ValueError If no reply has been received or the data cannot be parsed. """ currents = {} for reply in self.replies: data = reply.data alpha = bytes_to_int(data[0:4], dtype="i4") beta = bytes_to_int(data[4:], dtype="i4") currents[reply.positioner_id] = (alpha, beta) return currents
def get_holding_currents(self) -> Dict[int, numpy.ndarray]: """Returns the alpha and beta holding currents, in percent. Raises ------ ValueError If no reply has been received or the data cannot be parsed. """ currents = {} for reply in self.replies: data = reply.data alpha = bytes_to_int(data[0:4], dtype="i4") beta = bytes_to_int(data[4:], dtype="i4") currents[reply.positioner_id] = numpy.array([alpha, beta]) return currents
def get_positions(self) -> Dict[int, Tuple[float, float]]: """Returns the positions of alpha and beta in degrees. Raises ------ ValueError If no reply has been received or the data cannot be parsed. """ positions = {} for reply in self.replies: pid = reply.positioner_id data = reply.data beta = bytes_to_int(data[4:], dtype="i4") alpha = bytes_to_int(data[0:4], dtype="i4") positions[pid] = motor_steps_to_angle(alpha, beta) return positions
def get_offsets(self) -> Dict[int, numpy.ndarray]: """Returns the alpha and beta offsets, in degrees. Raises ------ ValueError If no reply has been received or the data cannot be parsed. """ offsets = {} for reply in self.replies: pid = reply.positioner_id data = reply.data alpha = bytes_to_int(data[0:4], dtype="i4") beta = bytes_to_int(data[4:], dtype="i4") offsets[pid] = numpy.array(motor_steps_to_angle(alpha, beta)) return offsets
def get_move_time(self): """Returns the time needed to move to the commanded position. Raises ------ ValueError If no reply has been received or the data cannot be parsed. """ move_times = {} for reply in self.replies: data = reply.data alpha = bytes_to_int(data[0:4], dtype="i4") beta = bytes_to_int(data[4:], dtype="i4") move_times[reply.positioner_id] = [ alpha * TIME_STEP, beta * TIME_STEP ] return move_times
def get_temperatures(self) -> Dict[int, int]: """Returns the temperature in Celsius. Raises ------ ValueError If no reply has been received or the data cannot be parsed. """ temperatures = {} for reply in self.replies: data = self.replies[0].data rawT = bytes_to_int(data, dtype="u4") # Raw temperature temperatures[reply.positioner_id] = rawT return temperatures
def get_number_trajectories(self) -> Dict[int, int | None]: """Returns the number of trajectories. Raises ------ ValueError If no reply has been received or the data cannot be parsed. """ number_trajectories = {} for reply in self.replies: data = reply.data if data == bytearray(): number_trajectories[reply.positioner_id] = None else: number_trajectories[reply.positioner_id] = bytes_to_int(data) return number_trajectories
async def update_status( self, status: maskbits.PositionerStatus | int = None, timeout=1.0, ): """Updates the status of the positioner.""" assert self.fps, "FPS is not set." # Need to update the firmware to make sure we get the right flags. if not self.firmware: await self.update_firmware_version() if not status: command = await self.send_command( CommandID.GET_STATUS, timeout=timeout, error="cannot get status.", ) n_replies = len(command.replies) if n_replies == 1: status = int(bytes_to_int(command.replies[0].data)) else: self.status = self.flags.UNKNOWN raise PositionerError( f"GET_STATUS received {n_replies} replies.") if not self.is_bootloader(): self.flags = self.get_positioner_flags() else: self.flags = maskbits.BootloaderStatus self.status = self.flags(int(status)) # Checks if the positioner is collided. If so, locks the FPS. # if not self.is_bootloader() and self.collision and not self.fps.locked: # await self.fps.lock() # raise PositionerError("collision detected. Locking the FPS.") return True
async def process_message(self, msg, positioner_id, command_id, uid): """Processes incoming commands from the bus.""" command_id = CommandID(command_id) command = command_id.get_command_class() if positioner_id == 0 and not command.broadcastable: self.reply( command_id, uid, response_code=ResponseCode.INVALID_BROADCAST_COMMAND, ) return if command_id == CommandID.GET_ID: self.reply(command_id, uid) elif command_id == CommandID.GET_FIRMWARE_VERSION: data_firmware = command.encode(self.firmware) # type: ignore self.reply(command_id, uid, data=data_firmware) elif command_id == CommandID.GET_STATUS: data_status = utils.int_to_bytes(self.status) self.reply(command_id, uid, data=data_status) elif command_id == CommandID.GET_NUMBER_TRAJECTORIES: data_status = utils.int_to_bytes(self.number_trajectories) self.reply(command_id, uid, data=data_status) elif command_id in [ CommandID.GO_TO_ABSOLUTE_POSITION, CommandID.GO_TO_RELATIVE_POSITION, ]: asyncio.create_task(self.process_goto(msg)) elif command_id == CommandID.GET_ACTUAL_POSITION: data_position = command.encode(*self.position) # type: ignore self.reply(command_id, uid, data=data_position) elif command_id == CommandID.SET_SPEED: data_speed = command.encode(*self.speed) # type: ignore self.reply(command_id, uid, data=data_speed) elif command_id == CommandID.START_FIRMWARE_UPGRADE: if not self.is_bootloader(): self.reply( command_id, uid, response_code=ResponseCode.INVALID_COMMAND, ) return try: data = msg.data firmware_size = utils.bytes_to_int(data[0:4], "u4") crc32 = utils.bytes_to_int(data[4:9], "u4") except Exception: self.reply( command_id, uid, response_code=ResponseCode.INVALID_COMMAND, ) return self._firmware_size = firmware_size self._crc32 = crc32 self._firmware_received = b"" self.reply(command_id, uid) elif command_id == CommandID.SEND_FIRMWARE_DATA: asyncio.create_task(self.process_firmware_data(uid, msg.data)) else: # Should be a valid command or CommandID(command_id) would # have failed. Just return OK. self.reply(command_id, uid)