Commit 42fa6bb8 authored by Jayant Khatkar's avatar Jayant Khatkar

fix logic error for when only one arm has trajs left, cleanup (#5)

parent e56bc51e
......@@ -45,24 +45,32 @@ def execute_plan(plan,
delays = [0,0] # measure delays of each arm
relative_delay = 0 # how far arm 0 is behind arm 2
i_r = [0,0] # index of which traj we're up to for each arm
arm_complete= None
while time.time() < expected_end_time + max(delays):
print("----------")
# Find which arm and which trajectory is next to be executed
arm = np.argmin([plan.cumul_time[0][i_r[0]], plan.cumul_time[1][i_r[1]]])
next_time = min(plan.cumul_time[0][i_r[0]], plan.cumul_time[1][i_r[1]])/move_speed
if arm_complete is not None:
arm = 1 - arm_complete # if one arm complete, only need to do other
else:
arm = np.argmin([plan.cumul_time[0][i_r[0]], plan.cumul_time[1][i_r[1]]])
next_time = plan.cumul_time[arm][i_r[arm]]/move_speed
i_r[arm] += 1
# if last trajectory has already been sent
# if the last trajectory for this arm has already been sent
if i_r[arm] == len(plan.trajs[arm]):
print("Last Trajectory sent, wait for completion")
time.sleep(expected_end_time - time.time())
print("Complete")
break
print("Last Trajectory for this arm has been sent")
if arm_complete is None:
arm_complete = arm # mark this arm as complete
continue
else:
print("All arms done (unless Logic error), leaving main loop")
break
t = plan.trajs[arm][i_r[arm]]
# wait till time to execute next trajectory
# wait till it's time to execute the next trajectory
rel_delay_arm = max(0, -relative_delay * (-1)**arm)
sleep_time = next_time - (time.time() - s ) + rel_delay_arm
if sleep_time > 0:
......@@ -70,7 +78,7 @@ def execute_plan(plan,
sleep_time))
time.sleep(sleep_time)
# Execute next Trajectory
# Execute next trajectory
print("Printing {}th trajectory of arm {}, which is contour {}".format(
i_r[arm], arm, t.contour
))
......@@ -96,7 +104,7 @@ def execute_plan(plan,
# Traj started now, measure delay
delays[arm] = (time.time() - s ) - next_time
print("Arm {} is behind by {} when executing {}th Traj".format(
print("Arm {} is behind by {}s when executing {}th Traj".format(
arm,
delays[arm],
i_r[arm]))
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment