fix planners

spf/grbl/grbl_interactive.py

@@ -276,14 +276,14 @@ def yield_points(
     ):
         # move to the stationary point
         yield from a_to_b_in_stepsize(
-            self.start_position, self.stationary_point, step_size=self.step_size
+            self.start_point, self.stationary_point, step_size=self.step_size
         )
         # stay still
         while True:
             yield self.stationary_point
 
 
-class CalibrationCirclePlanner(Planner):
+class CirclePlanner(Planner):
     def __init__(
         self,
         dynamics,
@@ -297,16 +297,16 @@ def __init__(
         )
         self.circle_center = circle_center
         self.circle_radius = circle_diameter / 2
-        self.angle_increment = chord_length_to_angle(self.step_size, self.radius)
+        self.angle_increment = chord_length_to_angle(self.step_size, self.circle_radius)
 
     def angle_to_pos(self, angle):
-        return self.circle_center + self.radius * np.array(
+        return self.circle_center + self.circle_radius * np.array(
             [-np.sin(angle), np.cos(angle)]
         )
 
     def yield_points(self):
         # start at the top of the circle
-        current_p = self.start_position
+        current_p = self.start_point
 
         # start at the top
         current_angle = 0
@@ -501,11 +501,72 @@ def __init__(self, controller):
         self.controller = controller
         self.channels = list(self.controller.channel_to_motor_map.keys())
         self.planners = [None for x in self.channels]
+        self.routines = {
+            "v1_calibrate": self.v1_calibrate,
+            "rx_circle": self.rx_circle,
+            "tx_circle": self.tx_circle,
+            "bounce": self.bounce,
+        }
 
     def run(self):
         points_by_channel = {c: self.planners[c].yield_points() for c in self.channels}
         self.controller.move_to_iter(points_by_channel)
 
+    def bounce(self):
+        self.planners = [
+            BouncePlanner(
+                self.controller.dynamics,
+                start_point=self.controller.position["xy"][c_idx],
+            )
+            for c_idx in range(len(self.planners))
+        ]
+        self.run()
+
+    def tx_circle(self):
+        self.planners = [
+            StationaryPlanner(
+                self.controller.dynamics,
+                start_point=self.controller.position["xy"][0],
+                stationary_point=circle_center,
+            ),
+            CirclePlanner(
+                self.controller.dynamics,
+                start_point=self.controller.position["xy"][1],
+                circle_diameter=max_circle_diameter,
+                circle_center=circle_center,
+            ),
+        ]
+        self.run()
+
+    def rx_circle(self):
+        self.planners = [
+            CirclePlanner(
+                self.controller.dynamics,
+                start_point=self.controller.position["xy"][0],
+                circle_diameter=max_circle_diameter,
+                circle_center=circle_center,
+            ),
+            StationaryPlanner(
+                self.controller.dynamics,
+                start_point=self.controller.position["xy"][1],
+                stationary_point=circle_center,
+            ),
+        ]
+        self.run()
+
+    def v1_calibrate(self):
+        self.planners = [
+            StationaryPlanner(
+                self.controller.dynamics,
+                start_point=self.controller.position["xy"][0],
+                stationary_point=rx_calibration_point,
+            ),
+            BouncePlanner(
+                self.controller.dynamics, start_point=self.controller.position["xy"][1]
+            ),
+        ]
+        self.run()
+
 
 def get_default_gm(serial_fn):
     dynamics = Dynamics(
@@ -533,30 +594,30 @@ def get_default_gm(serial_fn):
 
     gm = get_default_gm(serial_fn)
 
+    if gm.controller.position["is_moving"]:
+        print("Waiting for grbl to stop moving before starting...")
+        while gm.controller.position["is_moving"]:
+            time.sleep(0.1)
+            gm.controller.update_status()
+
     print(
         """
         q = quit
-        bounce = bounce
         s = status
         X,Y = move to X,Y
+
+        Routines:
     """
     )
+    for k in gm.routines:
+        print("        ", k)
+
     for line in sys.stdin:
         line = line.strip()
-        if line == "q":
+        if line in gm.routines:
+            gm.routines[line]()
+        elif line == "q":
             sys.exit(1)
-        elif line == "bounce":
-            # gm.bounce(20000)
-            gm.planners = [BouncePlanner(gm.controller.dynamics) for x in gm.planners]
-            gm.run()
-        elif line == "calibrate":
-            gm.planners = [
-                StationaryPlanner(
-                    gm.controller.dynamics, start_point=rx_calibration_point
-                ),
-                BouncePlanner(gm.controller.dynamics),
-            ]
-            gm.run()
         elif line == "s":
             p = gm.controller.update_status()
             print(p)