add timing test, fix timing wait logic (#5)

src/main2.py

@@ -72,7 +72,7 @@ def execute_plan(plan,
 
         # 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
+        sleep_time = next_time - (time.time() - s) + rel_delay_arm + delays[arm]
         if sleep_time > 0:
             print("Sleeping for {}s until time for next traj".format(
                 sleep_time))
@@ -115,7 +115,7 @@ def execute_plan(plan,
 
 
 if __name__ == '__main__':
-    plan2arms = load_plan('two_arm_test2')
+    plan2arms = load_plan('two_arm_timing_test')
 
     r1_con = Controller(disable_extruder=True, robot=1)
     r2_con = Controller(disable_extruder=True, robot=2)