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)
