Try and fix up grbl thread

spf/grbl/grbl_interactive.py

@@ -162,8 +162,8 @@ def get_boundary_vector_near_point(self, p):
 class Planner:
     def __init__(self, dynamics):
         self.dynamics = dynamics
-        self.current_direction = None  
-        self.epsilon = 1 # original was 0.001
+        self.current_direction = None
+        self.epsilon = 1  # original was 0.001
 
     def get_bounce_pos_and_new_direction(self, p, direction):
         distance_to_bounce = self.dynamics.binary_search_edge(
@@ -223,7 +223,7 @@ def bounce(self, start_p, n_bounces):
 
 
 class GRBLController:
-    def __init__(self, serial_fn, dyamics, channel_to_motor_map):
+    def __init__(self, serial_fn, dynamics, channel_to_motor_map):
         self.dynamics = dynamics
         # Open grbl serial port ==> CHANGE THIS BELOW TO MATCH YOUR USB LOCATION
         self.s = serial.Serial(
@@ -352,21 +352,15 @@ def __init__(self, controller, planners):
         self.planners = planners
 
     def bounce(self, n_bounces, direction=None):
-        start_positions = controller.update_status()["xy"]
+        start_positions = self.controller.update_status()["xy"]
         points_by_channel = {
             c: self.planners[c].bounce(start_positions[c], n_bounces)
             for c in self.channels
         }
         self.controller.move_to_iter(points_by_channel)
 
 
-if __name__ == "__main__":
-    if len(sys.argv) != 2:
-        print("grblman: %s device" % sys.argv[0])
-        sys.exit(1)
-
-    serial_fn = sys.argv[1]
-
+def get_default_gm():
     dynamics = Dynamics(
         calibration_point=home_calibration_point,
         pA=home_pA,
@@ -380,7 +374,18 @@ def bounce(self, n_bounces, direction=None):
         serial_fn, dynamics, channel_to_motor_map={0: "XY", 1: "ZA"}
     )
 
-    gm = GRBLManager(controller, planners)
+    return GRBLManager(controller, planners)
+
+
+if __name__ == "__main__":
+    if len(sys.argv) != 2:
+        print("grblman: %s device" % sys.argv[0])
+        sys.exit(1)
+
+    serial_fn = sys.argv[1]
+
+    gm = get_default_gm()
+
     print(
         """
         q = quit
@@ -397,17 +402,17 @@ def bounce(self, n_bounces, direction=None):
             # gm.bounce(20000)
             gm.bounce(40)
         elif line == "s":
-            p = controller.update_status()
+            p = gm.controller.update_status()
             print(p)
         else:
-            current_positions = controller.update_status()["xy"]
+            current_positions = gm.controller.update_status()["xy"]
             p_main = np.array([float(x) for x in line.split()])
             if True:
                 points_iter = {
                     c: iter(a_to_b_in_stepsize(current_positions[c], p_main, 5))
                     for c in [0, 1]
                 }
-                controller.move_to_iter(points_iter)
+                gm.controller.move_to_iter(points_iter)
             # except ValueError:
             #    print("Position is out of bounds!")
         time.sleep(0.01)

spf/grbl_sdr_collect_v2.py

@@ -10,7 +10,7 @@
 
 import numpy as np
 import yaml
-from grbl.grbl_interactive import GRBLManager
+from grbl.grbl_interactive import get_default_gm
 from tqdm import tqdm
 
 from spf.rf import beamformer
@@ -129,14 +129,16 @@ def read_forever(self):
 
 def bounce_grbl(gm):
     direction = None
-    while gm.collect:
+    global run_collection
+    while run_collection:
         logging.info("TRY TO BOUNCE")
         try:
             direction = gm.bounce(100, direction=direction)
         except Exception as e:
             logging.error(e)
         logging.info("TRY TO BOUNCE RET")
         time.sleep(10)  # cool off the motor
+    logging.info("Exiting GRBL thread")
 
 
 if __name__ == "__main__":
@@ -273,8 +275,9 @@ def bounce_grbl(gm):
 
     # setup GRBL
     gm = None
+    gm_thread = None
     if args.grbl_serial is not None:
-        gm = GRBLManager(args.grbl_serial)
+        gm = get_default_gm()
         gm_thread = threading.Thread(target=bounce_grbl, args=(gm,))
         gm_thread.start()
 
@@ -316,5 +319,8 @@ def bounce_grbl(gm):
     logging.info("Shuttingdown: start thread join!")
     for read_thread in read_threads:
         read_thread.t.join()
+    if gm_thread is not None:
+        logging.info("Grab grbl thread")
+        gm_thread.join()
 
     logging.info("Shuttingdown: done")