Skip to content

Commit

Permalink
Made trajectories have intermediate frame zero point
Browse files Browse the repository at this point in the history
  • Loading branch information
@coretl
coretl committed Sep 27, 2016
1 parent 0a869ea commit c7e8c1b
Show file tree
Hide file tree
Showing 2 changed files with 48 additions and 33 deletions.
63 changes: 39 additions & 24 deletions malcolm/parts/pmac/pmactrajectorypart.py
View file
Expand Up @@ -140,7 +140,7 @@ def update_step(self, scanned):
if completed_steps != self.completedSteps.value:
self.completedSteps.set_value(completed_steps)

def run_up_positions(self, point, fraction):
def run_up_positions(self, point):
"""Generate a dict of axis run up distances given a time
Args:
Expand All @@ -150,11 +150,12 @@ def run_up_positions(self, point, fraction):
"""
positions = {}

for axis_name in self.axis_mapping:
for axis_name, motor_info in self.axis_mapping.items():
full_distance = point.upper[axis_name] - point.lower[axis_name]
velocity = full_distance / self.exposure
# Divide by 2 as we are decelerating to zero
run_up = full_distance * fraction / 2.0
positions[axis_name] = run_up
accl_dist = motor_info.acceleration_time * velocity * 0.5
positions[axis_name] = accl_dist

return positions

Expand All @@ -166,13 +167,11 @@ def calculate_acceleration_time(self):
def build_start_profile(self, task, start_index):
"""Move to the run up position ready to start the scan"""
acceleration_time = self.calculate_acceleration_time()
fraction = acceleration_time / self.exposure
first_point = self.generator.get_point(start_index)
trajectory = {}
move_time = 0.0

for axis_name, run_up in \
self.run_up_positions(first_point, fraction).items():
for axis_name, run_up in self.run_up_positions(first_point).items():
start_pos = first_point.lower[axis_name] - run_up
trajectory[axis_name] = [start_pos]
move_time = max(move_time, self.get_move_time(axis_name, start_pos))
Expand Down Expand Up @@ -252,7 +251,6 @@ def build_profile(self, task, time_array, velocity_mode, trajectory,

def build_generator_profile(self, part_tasks, start_index=0):
acceleration_time = self.calculate_acceleration_time()
fraction = acceleration_time / self.exposure
trajectory = {}
time_array = []
velocity_mode = []
Expand All @@ -269,17 +267,25 @@ def build_generator_profile(self, part_tasks, start_index=0):

# Check if we need to insert the lower bound point
if last_point is None:
lower_move_time = acceleration_time
lower_move_time, turnaround_midpoint = acceleration_time, None
else:
lower_move_time = self.need_lower_move_time(last_point, point)
lower_move_time, turnaround_midpoint = \
self.need_lower_move_time(last_point, point)

if lower_move_time:
if velocity_mode:
# set the previous point to not take this point into account
velocity_mode[-1] = PREV_TO_CURRENT
# and tell it that this was an empty frame
user_programs[-1] = TRIG_DEAD_FRAME
# Check if we need a turnaround midpoint
if turnaround_midpoint:
# set the previous point to not take this point into account
velocity_mode[-1] = PREV_TO_CURRENT
# and tell it that this was an empty frame
user_programs[-1] = TRIG_DEAD_FRAME

# Add a padding point
time_array.append(lower_move_time)
velocity_mode.append(PREV_TO_NEXT)
user_programs.append(TRIG_ZERO)
completed_steps.append(i)

if lower_move_time:
# Add lower bound
time_array.append(lower_move_time)
velocity_mode.append(CURRENT_TO_NEXT)
Expand All @@ -301,7 +307,9 @@ def build_generator_profile(self, part_tasks, start_index=0):
# Add the axis positions
for axis_name, cs_def in self.axis_mapping.items():
positions = trajectory.setdefault(axis_name, [])
# Add lower bound axis positions
# Add padding and lower bound axis positions
if turnaround_midpoint:
positions.append(turnaround_midpoint[axis_name])
if lower_move_time:
positions.append(point.lower[axis_name])
positions.append(point.positions[axis_name])
Expand All @@ -312,15 +320,15 @@ def build_generator_profile(self, part_tasks, start_index=0):
time_array.append(acceleration_time)
velocity_mode[-1] = PREV_TO_CURRENT
velocity_mode.append(ZERO_VELOCITY)
user_programs[-1] = TRIG_DEAD_FRAME
user_programs.append(TRIG_ZERO)
if i + 1 < self.generator.num:
# We broke, so don't add i + 1 to current step
completed_steps.append(i)
else:
completed_steps.append(i + 1)

for axis_name, tail_off in \
self.run_up_positions(last_point, fraction).items():
for axis_name, tail_off in self.run_up_positions(last_point).items():
positions = trajectory[axis_name]
positions.append(positions[-1] + tail_off)
# Store the last position in axis_mapping so that we can build
Expand All @@ -338,12 +346,16 @@ def _same_sign(self, a, b):
def need_lower_move_time(self, last_point, point):
# First point needs to insert lower bound point
lower_move_time = 0
run_ups = self.run_up_positions(last_point)
turnaround_midpoint = {}

for axis_name, motor_info in self.axis_mapping.items():
if last_point.upper[axis_name] != point.lower[axis_name]:
first = last_point.upper[axis_name]
second = point.lower[axis_name]
turnaround_midpoint[axis_name] = (second - first) * 0.5 + first
if first != second:
# axis needs to move during turnaround, so insert lower bound
move_time = self.get_move_time(
axis_name, last_point.upper[axis_name],
point.lower[axis_name])
move_time = self.get_move_time(axis_name, first, second)
lower_move_time = max(lower_move_time, move_time)
else:
# axis has been moving
Expand All @@ -353,7 +365,10 @@ def need_lower_move_time(self, last_point, point):
if not self._same_sign(direction, new_direction):
move_time = motor_info.acceleration_time * 2
lower_move_time = max(lower_move_time, move_time)
return lower_move_time
turnaround_midpoint[axis_name] = first + run_ups[axis_name]
if lower_move_time == 0:
turnaround_midpoint = None
return lower_move_time * 0.5, turnaround_midpoint

def external_axis_has_moved(self, last_point, point):
for axis_name in self.generator.position_units:
Expand Down
18 changes: 9 additions & 9 deletions tests/test_parts/test_pmac/test_pmactrajectorypart.py
View file
Expand Up @@ -87,20 +87,20 @@ def test_configure(self):
self.check_resolutions_and_use(task.put.call_args_list[2][0][0])
self.assertEqual(task.put.call_args_list[3][0][0], {
self.child["time_array"]: [
400, 2000, 2000, 2000, 2000, 2000, 2000,
800, 2000, 2000, 2000, 2000, 2000, 2000, 400],
400, 2000, 2000, 2000, 2000, 2000, 2000, 400,
400, 2000, 2000, 2000, 2000, 2000, 2000, 400],
self.child["velocity_mode"]: [
2, 0, 0, 0, 0, 0, 1,
2, 0, 0, 0, 0, 0, 1, 0,
2, 0, 0, 0, 0, 0, 1, 3],
self.child["user_programs"]: [
3, 4, 3, 4, 3, 4, 2,
3, 4, 3, 4, 3, 4, 3, 8],
self.child["num_points"]: 15,
3, 4, 3, 4, 3, 4, 2, 8,
3, 4, 3, 4, 3, 4, 2, 8],
self.child["num_points"]: 16,
self.child["positionsA"]: [
-0.125, 0.0, 0.125, 0.25, 0.375, 0.5, 0.625,
-0.125, 0.0, 0.125, 0.25, 0.375, 0.5, 0.625, 0.6375,
0.625, 0.5, 0.375, 0.25, 0.125, 0.0, -0.125, -0.1375],
self.child["positionsB"]: [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05,
0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]})

def test_run(self):
Expand Down Expand Up @@ -135,7 +135,7 @@ def test_build_next_stage(self):
self.assertEqual(task.put.call_args_list[3][0][0], {
self.child["time_array"]: [400, 2000, 2000, 2000, 2000, 400],
self.child["velocity_mode"]: [2, 0, 0, 0, 1, 3],
self.child["user_programs"]: [3, 4, 3, 4, 3, 8],
self.child["user_programs"]: [3, 4, 3, 4, 2, 8],
self.child["num_points"]: 6,
self.child["positionsA"]: [0.375, 0.25, 0.125, 0.0, -0.125, -0.1375],
self.child["positionsB"]: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]})
Expand Down

0 comments on commit c7e8c1b

Please sign in to comment.