async def test_home_az(self):
daz = -2
est_ccw_duration = abs(daz / self.ctrl.az_vel)
curr_az = self.ctrl.az_actuator.position(salobj.current_tai())
home_azimuth = salobj.angle_wrap_nonnegative(curr_az + daz).deg
self.ctrl.home_az = home_azimuth
reply_lines = await self.send_cmd("HM")
self.assertEqual(reply_lines, [""])
# sleep until halfway through CCW motion and check status
await asyncio.sleep(est_ccw_duration / 2)
reply_lines = await self.send_cmd("+")
status = ATDome.Status(reply_lines)
self.assertEqual(status.move_code, 2 + 64)
self.assertAlmostEqual(self.ctrl.az_actuator.speed, self.ctrl.az_vel)
# sleep until halfway through CW motion and check status
await asyncio.sleep(self.ctrl.az_actuator.remaining_time() +
est_ccw_duration / 2)
reply_lines = await self.send_cmd("+")
status = ATDome.Status(reply_lines)
self.assertEqual(status.move_code, 1 + 64)
self.assertAlmostEqual(self.ctrl.az_actuator.speed,
self.ctrl.home_az_vel)
# sleep until we're done and check status
await asyncio.sleep(self.ctrl.az_actuator.remaining_time() + 0.1)
reply_lines = await self.send_cmd("+")
status = ATDome.Status(reply_lines)
self.assertEqual(status.move_code, 0)
self.assertAlmostEqual(self.ctrl.az_actuator.speed, self.ctrl.az_vel)
self.assertAlmostEqual(
self.ctrl.az_actuator.position(salobj.current_tai()),
self.ctrl.home_az)
def set_position(self,
position,
direction=base.Direction.NEAREST,
start_tai=None):
"""Set a new target position and return the move duration.
Parameters
----------
position : `float`
Commanded position, in degrees.
direction : `Direction`, optional
Direction of motion.
start_tai : `float` or `None`, optional
TAI date (unix seconds) of the start of the move.
If `None` use the current time.
"""
direction = base.Direction(direction)
if start_tai is None:
start_tai = salobj.current_tai()
start_position = salobj.angle_wrap_nonnegative(
self.position(start_tai)).deg
delta = salobj.angle_diff(position, start_position).deg
if direction is base.Direction.POSITIVE:
if delta < 0:
delta += 360
elif direction is base.Direction.NEGATIVE:
if delta > 0:
delta -= 360
elif direction is not base.Direction.NEAREST:
raise ValueError(f"Unsupported direction {direction!r}")
return self._set_position(
start_position=start_position,
start_tai=start_tai,
end_position=start_position + delta,
)
def position(self, tai=None):
"""Current position.
Parameters
----------
tai : `float` or `None`, optional
TAI date, unix seconds. Current time if `None`.
"""
return salobj.angle_wrap_nonnegative(super().position(tai)).deg
def check_set_target(self, start_segment, end_segment):
self.assertGreater(end_segment.tai, start_segment.tai)
max_velocity = 3
max_acceleration = 4
dtmax_track = 0.5
actuator = simactuators.CircularTrackingActuator(
max_velocity=max_velocity,
max_acceleration=max_acceleration,
dtmax_track=dtmax_track,
)
targets = dict()
paths = dict()
predicted_wrapped_target_position = salobj.angle_wrap_nonnegative(
end_segment.position).deg
for direction in simactuators.Direction:
actuator.target = start_segment
actuator.path = simactuators.path.Path(start_segment,
kind=actuator.Kind.Tracking)
actuator.set_target(
tai=end_segment.tai,
position=end_segment.position,
velocity=end_segment.velocity,
direction=direction,
)
self.assertEqual(actuator.target.tai, end_segment.tai)
self.assertEqual(actuator.target.velocity, end_segment.velocity)
self.assertAlmostEqual(actuator.target.position,
predicted_wrapped_target_position)
targets[direction] = actuator.target
paths[direction] = actuator.path
# Check that NEAREST direction chooses the path of shortest duration
end_tais = {
direction: path[-1].tai
for direction, path in paths.items()
}
min_end_tai = min(tai for tai in end_tais.values())
self.assertEqual(end_tais[simactuators.Direction.NEAREST], min_end_tai)
# Check that POSITIVE direction chooses positive direction at
# end_segment.tai for the target vs the current position at end_tai,
# and NEGATIVE the negative direction.
# Use a bit of slop for roundoff error when target = current.
for direction in (
simactuators.Direction.NEGATIVE,
simactuators.Direction.POSITIVE,
):
target_position = targets[direction].at(end_segment.tai).position
current_position = paths[direction].at(end_segment.tai).position
slop = 1e-5
if direction is simactuators.Direction.POSITIVE:
self.assertGreaterEqual(target_position + slop,
current_position)
else:
self.assertLessEqual(target_position - slop, current_position)
def __init__(self, speed, start_position=None):
# The private fields _start_position and _end_position are not
# necessarily in the range [0, 360). This is important for supporting
# the ``direction`` parameter. The public accessors all wrap
# position into the range [0, 360).
if start_position is None:
start_position = 0
super().__init__(
start_position=salobj.angle_wrap_nonnegative(start_position).deg,
speed=speed,
)
async def do_moveAzimuth(self, data):
"""Implement the ``moveAzimuth`` command."""
self.assert_enabled()
if self.evt_azimuthState.data.homing:
raise salobj.ExpectedError("Cannot move azimuth while homing")
if not self.evt_azimuthState.data.homed:
self.log.warning("The azimuth axis may not be homed.")
azimuth = salobj.angle_wrap_nonnegative(data.azimuth).deg
await self.run_command(f"{azimuth:0.3f} MV")
self.evt_azimuthCommandedState.set_put(
commandedState=AzimuthCommandedState.GOTOPOSITION,
azimuth=azimuth,
force_output=True,
)
self.status_sleep_task.cancel()
def test_constructor(self):
max_velocity = 3
max_acceleration = 4
dtmax_track = 0.5
nsettle = 1
tai = 0.5
actuator = simactuators.CircularTrackingActuator(
max_velocity=max_velocity,
max_acceleration=max_acceleration,
dtmax_track=dtmax_track,
nsettle=nsettle,
tai=tai,
)
self.assertEqual(actuator.max_velocity, max_velocity)
self.assertEqual(actuator.max_acceleration, max_acceleration)
self.assertEqual(actuator.dtmax_track, dtmax_track)
self.assertEqual(actuator.nsettle, nsettle)
self.assertEqual(actuator.target.tai, tai)
self.assertEqual(actuator.target.position, 0)
self.assertEqual(actuator.target.velocity, 0)
self.assertEqual(actuator.target.acceleration, 0)
self.assertEqual(actuator.target.jerk, 0)
self.assertEqual(actuator.path[0].tai, tai)
self.assertEqual(actuator.path[0].position, 0)
self.assertEqual(actuator.path[0].velocity, 0)
self.assertEqual(actuator.path[0].acceleration, 0)
self.assertEqual(actuator.path[0].jerk, 0)
self.assertEqual(len(actuator.path), 1)
self.assertEqual(actuator.path.kind, actuator.Kind.Stopped)
for start_position in (-10, -0.001, 1, 359, 360):
expected_start_position = salobj.angle_wrap_nonnegative(
start_position).deg
actuator = simactuators.CircularTrackingActuator(
max_velocity=max_velocity,
max_acceleration=max_acceleration,
dtmax_track=dtmax_track,
nsettle=nsettle,
tai=tai,
start_position=start_position,
)
self.assertAlmostEqual(actuator.path[-1].position,
expected_start_position)
self.assertAlmostEqual(actuator.target.position,
expected_start_position)
def set_target(self, tai, position, velocity, direction=base.Direction.NEAREST):
"""Set the target position, velocity and time.
The actuator will track, if possible, else slew to match the specified
path.
``target.position`` is wrapped into the range [0, 360).
Parameters
----------
tai : `float`
TAI time (unix seconds, e.g. from lsst.ts.salobj.current_tai()).
position : `float`
Position (deg)
velocity : `float`
Velocity (deg/sec)
direction : `Direction`
Desired direction for acquiring the target (which way to slew).
`Direction.NEAREST` picks the slew with shortest duration.
Raises
------
ValueError
If ``tai <= self.target.tai``,
where ``self.target.tai`` is the time of
the previous call to `set_target`.
Notes
-----
The actuator will track if the following is true:
* ``tai - self.target.tai < self.dtmax_track``
where ``self.target.tai`` is the time of
the previous call to `set_target`.
* The tracking segment path obeys the velocity and acceleration limits.
"""
wrapped_position = salobj.angle_wrap_nonnegative(position).deg
new_path = self._compute_directed_path(
tai=tai, position=wrapped_position, velocity=velocity, direction=direction
)
self.target = path.PathSegment(
tai=tai, position=wrapped_position, velocity=velocity
)
self.path = new_path
async def test_az_home_switch(self):
daz = -2
curr_az = self.ctrl.az_actuator.position(salobj.current_tai())
self.ctrl.home_az = salobj.angle_wrap_nonnegative(curr_az + daz).deg
# Move to left edge, center, and right edge of home switch
# and assert on the home switch each time.
for az in (
self.ctrl.home_az - self.ctrl.home_az_tolerance + 0.01,
self.ctrl.home_az + self.ctrl.home_az_tolerance - 0.01,
self.ctrl.home_az,
):
reply_lines = await self.send_cmd(f"{az:0.2f} MV")
self.assertEqual(reply_lines, [""])
# Sleep until motion finished, then check status.
# Wait a bit extra for Docker macOS clock non-monotonicity.
await asyncio.sleep(self.ctrl.az_actuator.remaining_time() + 0.2)
reply_lines = await self.send_cmd("+")
status = ATDome.Status(reply_lines)
self.assertEqual(status.move_code, 0)
self.assertAlmostEqual(status.az_pos, az)
self.assertTrue(status.az_home_switch)
# Move just off of the az switch in both directions
for az in (
self.ctrl.home_az - self.ctrl.home_az_tolerance - 0.01,
self.ctrl.home_az + self.ctrl.home_az_tolerance + 0.01,
):
reply_lines = await self.send_cmd(f"{az:0.2f} MV")
self.assertEqual(reply_lines, [""])
# Sleep until motion finished, then check status.
# Wait a bit extra for Docker macOS clock non-monotonicity.
await asyncio.sleep(self.ctrl.az_actuator.remaining_time() + 0.2)
reply_lines = await self.send_cmd("+")
status = ATDome.Status(reply_lines)
self.assertEqual(status.move_code, 0)
self.assertAlmostEqual(status.az_pos, az)
self.assertFalse(status.az_home_switch)
def __init__(
self,
max_velocity,
max_acceleration,
dtmax_track,
nsettle=2,
tai=None,
start_position=None,
):
if start_position is None:
wrapped_start_position = 0
else:
wrapped_start_position = salobj.angle_wrap_nonnegative(start_position).deg
super().__init__(
min_position=-math.inf,
max_position=math.inf,
max_velocity=max_velocity,
max_acceleration=max_acceleration,
dtmax_track=dtmax_track,
nsettle=nsettle,
tai=tai,
start_position=wrapped_start_position,
)
def end_position(self):
"""Ending position of move, in the range [0, 360) degrees."""
return salobj.angle_wrap_nonnegative(self._end_position).deg
def start_position(self):
"""Starting position of move in the range [0, 360) degrees."""
return salobj.angle_wrap_nonnegative(self._start_position).deg
def __post_init__(self):
if self.elevation < 0 or self.elevation > 90:
raise ValueError(
f"elevation={self.elevation} must be in range [0, 90]")
self.azimuth = salobj.angle_wrap_nonnegative(self.azimuth).deg
self.rotation = salobj.angle_wrap_center(self.rotation).deg