add interface and debugging tools (#17)

src/main.py

@@ -18,7 +18,14 @@ from Controller import Controller
 import gcode2contour as gc
 
 
-def execute_plan(plan, controller, contours=None, confirm=False, skip_first=1):
+def execute_plan(plan,
+        controller,
+        contours=None,
+        confirm=False,
+        skip_first=1,
+        contour_speed=0.2,
+        move_speed=1,
+        first_n=None):
     """
     Executes a plan on MoveIt
 
@@ -31,6 +38,10 @@ def execute_plan(plan, controller, contours=None, confirm=False, skip_first=1):
     # execute trajectories in plan
     for i, t in enumerate(plan.trajs[0]):
 
+        if i == first_n:
+            print("Finishing prematurely, completed {} trajectories".format(i))
+            break
+
         if i < skip_first:
             print("skipping traj {}".format(i))
             continue
@@ -40,12 +51,13 @@ def execute_plan(plan, controller, contours=None, confirm=False, skip_first=1):
             if a== 'n':
                 break
 
-        if t.contour is not None:
+        if t.contour is not None and contours is not None:
             print("Printing {}th trajectory in the plan, which is contour {}".format(i, t.contour))
-            controller.exec_ctraj(speed_multiplier(t, 5), contour=contours[t.contour])
+            controller.exec_ctraj(speed_multiplier(t, 1/contour_speed), contour=contours[t.contour])
         else:
-            print("Moving arm, no extrusion")
-            controller.exec_ctraj(t)
+            print("Printing {}th trajectory in the plan, which is contour {}".format(i, t.contour))
+            print("Moving arm without extrusion")
+            controller.exec_ctraj(speed_multiplier(t, 1/move_speed))
     return
 
 
@@ -55,12 +67,12 @@ if __name__ == '__main__':
     plan2, flexirex_contours = load_plan('flexirex', contours=True)
 
     # Printing
-    con = Controller()
-    execute_plan(plan2, con, contours=flexirex_contours, confirm=True)
+    #con = Controller()
+    #execute_plan(plan2, con, contours=flexirex_contours, confirm=True)
 
     # no printing
-    #con = Controller(disable_extruder=True)
-    #execute_plan(calibration_test, con, confirm=True)
+    con = Controller(disable_extruder=True)
+    execute_plan(plan2, con, confirm=False, move_speed=5)
 
     # go to origin and stay (to check calibration)
     #plan05 = copy.deepcopy(plan1)