test calibration, update main1 to safer execution as implemented in main2 (#19)

src/main1.py

@@ -25,7 +25,9 @@ def execute_plan(plan,
         skip_first=1,
         contour_speed=0.2,
         move_speed=1,
-        n_trajs=None):
+        n_trajs=100,
+        retry_time=0.1,
+        max_traj_retries=20):
     """
     Executes a plan on MoveIt
 
@@ -54,28 +56,43 @@ def execute_plan(plan,
 
         print("Printing {}th trajectory in the plan, which is contour {}".format(i, t.contour))
 
+        # Extruding with trajectory
+        retries = 0
         if t.contour is not None and contours is not None:
-            while not controller.exec_ctraj(speed_multiplier(t, 1/contour_speed), contour=contours[t.contour]):
-                pass
+            traj2send = speed_multiplier(t, 1/contour_speed)
+            while not controller.exec_ctraj(
+                    traj2send,
+                    contour=contours[t.contour],
+                    wait=False) and retries < max_traj_retries:
+                retries += 1
+                time.sleep(retry_time)
+
+        # Movement only
         else:
-            #print("Moving arm without extrusion")
-            while not controller.exec_ctraj(speed_multiplier(t, 1/move_speed)):
-                pass
+            traj2send = speed_multiplier(t, 1/move_speed)
+            while not controller.exec_ctraj(traj2send, wait=False) \
+                    and retries < max_traj_retries:
+                retries += 1
+                time.sleep(retry_time)
+
+        if retries == max_traj_retries:
+            print("\nFailed after {} retries, quitting".format(max_traj_retries))
+            sys.exit(0)
     return
 
 
 if __name__ == '__main__':
     plan1 = load_plan('origin')
-    calibration_test = load_plan('cal_test')
+    calibration_test = load_plan('r1_cal_test')
     plan2, flexirex_contours = load_plan('flexirex', contours=True)
 
     # Printing
-    con = Controller()
-    execute_plan(plan2, con, contours=flexirex_contours, confirm=True, n_trajs=10)
+    #con = Controller()
+    #execute_plan(plan2, con, contours=flexirex_contours, confirm=True, n_trajs=10)
 
-    # no printing
-    #con = Controller(disable_extruder=True)
-    #execute_plan(plan2, con, confirm=False, n_trajs=200, move_speed=1)
+    # Test calibration
+    con = Controller(disable_extruder=True)
+    execute_plan(calibration_test, con, confirm=True)
 
     # go to origin and stay (to check calibration)
     #plan05 = copy.deepcopy(plan1)