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

src/main2.py

@@ -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
+        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 = min(plan.cumul_time[0][i_r[0]], plan.cumul_time[1][i_r[1]])/move_speed
+        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")
+            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]))