fixes for grbl controller

.gitignore

@@ -12,3 +12,4 @@ test_data.txt
 **/.ipynb_checkpoints
 **/*.pkl
 **/*.log
+**/token

.vscode/settings.json

@@ -12,8 +12,8 @@
       "python.testing.pytestEnabled": true,
       "python.testing.cwd": "${workspaceFolder}/",
       "[python]": {
-            "editor.formatOnSave": true,
             "editor.defaultFormatter": "ms-python.black-formatter",
+            "editor.formatOnSave": true,
             "editor.codeActionsOnSave": {
                   "source.organizeImports": "explicit"
             },

spf/grbl/grbl_interactive.py

@@ -1,3 +1,4 @@
+import logging
 import sys
 import time
 
@@ -16,6 +17,15 @@
     [800, 1000],
 ]
 
+run_grbl = True
+
+
+def stop_grbl():
+    logging.info("STOP GRBL")
+    global run_grbl
+    run_grbl = False
+
+
 """
 MotorMountA                         MotorMountB
         .                           .
@@ -65,8 +75,10 @@ def __init__(self, calibration_point, pA, pB, bounding_box):
         if len(bounding_box) >= 3:
             hull = ConvexHull(bounding_box)
             if len(np.unique(hull.simplices)) != len(bounding_box):
-                print("Points do not form a simple hull, most likely non convex")
-                print(
+                logging.error(
+                    "Points do not form a simple hull, most likely non convex"
+                )
+                logging.error(
                     "Points in the hull are, "
                     + ",".join(
                         map(str, [bounding_box[x] for x in np.unique(hull.simplices)])
@@ -202,6 +214,7 @@ def random_direction(self):
         return np.array([np.sin(theta), np.cos(theta)])
 
     def bounce(self, start_p, n_bounces):
+        global run_grbl
         # if no previous direciton lets initialize one
         if self.current_direction is None:
             self.current_direction = self.random_direction()
@@ -213,13 +226,17 @@ def bounce(self, start_p, n_bounces):
         self.current_direction /= np.linalg.norm(self.current_direction)
 
         for _ in range(n_bounces):
+            if not run_grbl:
+                logging.info("Exiting bounce early")
+                break
             to_points, new_direction = self.single_bounce(
                 self.current_direction, start_p
             )
             assert len(to_points) > 0
             yield from to_points
             start_p = to_points[-1]
             self.current_direction = new_direction
+        logging.info("Exiting bounce")
 
 
 class GRBLController:
@@ -230,8 +247,8 @@ def __init__(self, serial_fn, dynamics, channel_to_motor_map):
             serial_fn, 115200, timeout=0.3, write_timeout=0.3
         )  # GRBL operates at 115200 baud. Leave that part alone.
         self.s.write("?".encode())
-        grbl_out = self.s.readline()  # get the response
-        print("GRBL ONLINE", grbl_out)
+        _ = self.s.readline()  # get the response
+        logging.info("GRBL ONLINE grbl_out")
         self.position = {"time": time.time(), "xy": np.zeros(2)}
         self.update_status()
         time.sleep(0.05)
@@ -255,7 +272,9 @@ def update_status(self, skip_write=False):
         try:
             motor_position_str = response.split("|")[1]
         except Exception as e:
-            print("FAILED TO PARSE", response, "|e|", e, time.time() - start_time)
+            logging.warning(
+                "FAILED TO PARSE", response, "|e|", e, time.time() - start_time
+            )
             return self.update_status(skip_write=not skip_write)
         b0_motor_steps, a0_motor_steps, b1_motor_steps, a1_motor_steps = map(
             float, motor_position_str[len("MPos:") :].split(",")
@@ -279,7 +298,8 @@ def update_status(self, skip_write=False):
         return self.position
 
     def wait_while_moving(self):
-        while True:
+        global run_grbl
+        while run_grbl:
             old_pos = self.update_status()
             time.sleep(0.05)
             new_pos = self.update_status()
@@ -291,8 +311,11 @@ def wait_while_moving(self):
             time.sleep(0.01)
 
     def move_to(self, points):  # takes in a list of points equal to length of map
+        global run_grbl
         gcode_move = ["G0"]
         for c in points:
+            if not run_grbl:
+                return
             motors = self.channel_to_motor_map[c]
             a_motor_steps, b_motor_steps = self.dynamics.to_steps(points[c])
             gcode_move += [
@@ -323,7 +346,8 @@ def targets_far_out(self, target_points, tolerance=80):
         return False
 
     def move_to_iter(self, points_by_channel):
-        while True:
+        global run_grbl
+        while run_grbl:
             next_points = get_next_points(points_by_channel)
             if len(next_points) == 0:
                 break
@@ -360,7 +384,7 @@ def bounce(self, n_bounces, direction=None):
         self.controller.move_to_iter(points_by_channel)
 
 
-def get_default_gm():
+def get_default_gm(serial_fn):
     dynamics = Dynamics(
         calibration_point=home_calibration_point,
         pA=home_pA,

spf/grbl_sdr_collect_v2.py

@@ -10,7 +10,7 @@
 
 import numpy as np
 import yaml
-from grbl.grbl_interactive import get_default_gm
+from grbl.grbl_interactive import get_default_gm, stop_grbl
 from tqdm import tqdm
 
 from spf.rf import beamformer
@@ -59,6 +59,7 @@ def signal_handler(sig, frame):
     global run_collection
     run_collection = False
     shutdown_radios()
+    stop_grbl()
 
 
 signal.signal(signal.SIGINT, signal_handler)
@@ -136,6 +137,8 @@ def bounce_grbl(gm):
             direction = gm.bounce(100, direction=direction)
         except Exception as e:
             logging.error(e)
+        if not run_collection:
+            break
         logging.info("TRY TO BOUNCE RET")
         time.sleep(10)  # cool off the motor
     logging.info("Exiting GRBL thread")
@@ -277,7 +280,7 @@ def bounce_grbl(gm):
     gm = None
     gm_thread = None
     if args.grbl_serial is not None:
-        gm = get_default_gm()
+        gm = get_default_gm(args.grbl_serial)
         gm_thread = threading.Thread(target=bounce_grbl, args=(gm,))
         gm_thread.start()
 
@@ -304,15 +307,16 @@ def bounce_grbl(gm):
             rx_pos = np.array([0, 0])
             tx_pos = np.array([0, 0])
             if gm is not None:
-                tx_pos = gm.position["xy"][target_tx_config.motor_channel]
-                rx_pos = gm.position["xy"][read_thread.pplus.rx_config.motor_channel]
+                tx_pos = gm.controller.position["xy"][target_tx_config.motor_channel]
+                rx_pos = gm.controller.position["xy"][
+                    read_thread.pplus.rx_config.motor_channel
+                ]
 
             read_thread.pplus.record_matrix[record_index] = prepare_record_entry(
                 ds=read_thread.data, rx_pos=rx_pos, tx_pos=tx_pos
             )
             ###
             read_thread.read_lock.release()
-
     logging.info("Shuttingdown: sending false to threads")
     for read_thread in read_threads:
         read_thread.run = False