watch over a minimize relative delay, cleanup (#5)

src/main2.py

@@ -40,45 +40,60 @@ def execute_plan(plan,
 
     s = time.time()
     expected_end_time = s + plan_len
-    i_r = [0,0]
-    while time.time() < expected_end_time:
+    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
+    while time.time() < expected_end_time + max(delays):
+
+        print("----------")
 
         # Find which arm and which trajectory is next to be executed
-        next_arm = np.argmin([plan.cumul_time[0][i_r[0]], plan.cumul_time[1][i_r[1]]])
+        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
-        i_r[next_arm] += 1
+        i_r[arm] += 1
 
         # if last trajectory has already been sent
-        if i_r[next_arm] == len(plan.trajs[next_arm]):
+        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
-        t = plan.trajs[next_arm][i_r[next_arm]]
+        t = plan.trajs[arm][i_r[arm]]
 
         # wait till time to execute next trajectory
-        sleep_time = next_time - (time.time() - s )
+        rel_delay_arm = max(0, -relative_delay * (-1)**arm)
+        sleep_time = next_time - (time.time() - s ) + rel_delay_arm
         if sleep_time > 0:
             print("Sleeping for {}s until time for next traj".format(
-                sleep_time
-                ))
+                sleep_time))
             time.sleep(sleep_time)
 
         # Execute next Trajectory
         print("Printing {}th trajectory of arm {}, which is contour {}".format(
-            i_r[next_arm], next_arm, t.contour
+            i_r[arm], arm, t.contour
             ))
         if t.contour is not None and contours is not None:
             traj2send = speed_multiplier(t, 1/contour_speed)
-            while not controllers[next_arm].exec_ctraj(
+            while not controllers[arm].exec_ctraj(
                     traj2send,
                     contour=contours[t.contour],
                     wait=False):
+                # while loop retries if too early to execute the first time
                 pass
         else:
             traj2send = speed_multiplier(t, 1/move_speed)
-            while not controllers[next_arm].exec_ctraj(traj2send, wait=False):
+            while not controllers[arm].exec_ctraj(traj2send, wait=False):
                 pass
+
+        # Traj started now, measure delay
+        delays[arm] = (time.time() - s ) - next_time
+        print("Arm {} is behind by {} when executing {}th Traj".format(
+            arm,
+            delays[arm],
+            i_r[arm]))
+        relative_delay = delays[0]-delays[1]
+        print("RELATIVE DELAY: arm 0 is {}s behind".format(relative_delay))
+
     return