wall v2 semi stable; 3 radios online with grbl

Semi stable radio collection using three radios

update reqs

change epsilon to larger

Try and fix up grbl thread

fixes for grbl controller

fix reqs

fix grbl logging

fix for grbl parsing

add graceful shutdown on normal exit

dont need these files here

Update wall array v2 with correct rx antenna coordinates

more calibration frames

formatting

.gitignore

@@ -11,3 +11,5 @@ test_data.txt
 .ipynb_checkpoints
 **/.ipynb_checkpoints
 **/*.pkl
+**/*.log
+**/token

.vscode/settings.json

@@ -12,12 +12,16 @@
       "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"
             },
       },
+      "isort.args": [
+            "--profile",
+            "black"
+      ],
       "flake8.args": [
             "--max-line-length=120",
             "--ignore=E203"

requirements.txt

@@ -1,87 +1,60 @@
 aiohttp==3.9.1
 aiosignal==1.3.1
-appnope==0.1.3
-asttokens==2.4.1
 async-timeout==4.0.3
 attrs==23.1.0
 black==23.12.0
 certifi==2023.11.17
 charset-normalizer==3.3.2
 click==8.1.7
-comm==0.2.0
 compress-pickle==2.1.0
 contourpy==1.1.0
 cycler==0.11.0
-debugpy==1.8.0
-decorator==5.1.1
 exceptiongroup==1.2.0
-executing==2.0.1
 filelock==3.13.1
 flake8==6.1.0
 fonttools==4.42.0
 frozenlist==1.4.0
 fsspec==2023.12.2
 idna==3.6
-imageio==2.33.1
 importlib-resources==6.0.1
 iniconfig==2.0.0
-ipykernel==6.28.0
-ipython==8.19.0
-jedi==0.19.1
+isort==5.13.2
 Jinja2==3.1.2
 joblib==1.3.2
-jupyter_client==8.6.0
-jupyter_core==5.5.1
 kiwisolver==1.4.4
-lazy_loader==0.3
 libaio==0.9.1
 MarkupSafe==2.1.3
 matplotlib==3.7.2
-matplotlib-inline==0.1.6
 mccabe==0.7.0
 mpmath==1.3.0
 multidict==6.0.4
 mypy-extensions==1.0.0
-nest-asyncio==1.5.8
 networkx==3.2.1
 numpy==1.25.2
 packaging==23.1
-parso==0.8.3
 pathspec==0.12.1
-pexpect==4.9.0
 Pillow==10.0.0
 platformdirs==4.1.0
 pluggy==1.3.0
-prompt-toolkit==3.0.43
-psutil==5.9.7
-ptyprocess==0.7.0
-pure-eval==0.2.2
 pyadi-iio==0.0.16
 pycodestyle==2.11.1
 pyflakes==3.1.0
-Pygments==2.17.2
 pylibiio==0.25
 pyparsing==3.0.9
 pyserial==3.5
 pytest==7.4.3
 python-dateutil==2.8.2
-pyzmq==25.1.2
+PyYAML==6.0.1
 requests==2.31.0
-scikit-image==0.22.0
 scipy==1.11.4
 shapely==2.0.2
 six==1.16.0
-stack-data==0.6.3
 sympy==1.12
-tifffile==2023.12.9
 tomli==2.0.1
 torch==2.1.2
 torchvision==0.16.2
-tornado==6.4
 tqdm==4.66.1
-traitlets==5.14.0
-typing_extensions==4.9.0
+typing-extensions==4.9.0
 urllib3==2.1.0
-wcwidth==0.2.12
 yarl==1.9.4
 zipp==3.16.2

spf/dataset/spf_dataset.py

@@ -7,7 +7,7 @@
 
 import numpy as np
 import torch
-from compress_pickle import dump, load
+from compress_pickle import load
 from torch.utils.data import Dataset
 
 from spf.dataset.spf_generate import generate_session
@@ -205,7 +205,6 @@ def __init__(
             np.ones((self.snapshots_in_sample, 1), dtype=np.int32) * self.args.width
         )
         self.halfpis = -np.ones((self.snapshots_in_sample, 1)) * np.pi / 2
-        idx_to_fileidx_and_sampleidx = {}
         # print("WARNING BY DEFAULT FLIPPING RADIO FEATURE SINCE COORDS WERE WRONG IN PI COLLECT!")
 
     def idx_to_fileidx_and_startidx(self, idx):
@@ -362,10 +361,10 @@ def collate_fn_beamformer(_in):
     d = {k: torch.from_numpy(np.stack([x[k] for x in _in])) for k in _in[0]}
     b, s, n_sources, _ = d["source_positions_at_t"].shape
 
-    times = d["time_stamps"] / (0.00001 + d["time_stamps"].max(axis=2, keepdim=True)[0])
+    # times = d["time_stamps"] / (0.00001 + d["time_stamps"].max(axis=2, keepdim=True)[0])
 
     source_theta = d["source_theta_at_t"].mean(axis=2)
-    distances = d["source_distance_at_t_normalized"].mean(axis=2, keepdims=True)
+    # distances = d["source_distance_at_t_normalized"].mean(axis=2, keepdims=True)
     _, _, beam_former_bins = d["beam_former_outputs_at_t"].shape
     perfect_labels = torch.zeros((b, s, beam_former_bins))
 
@@ -407,9 +406,9 @@ def collate_fn_transformer_filter(_in):
     d = {k: torch.from_numpy(np.stack([x[k] for x in _in])) for k in _in[0]}
     b, s, n_sources, _ = d["source_positions_at_t"].shape
 
-    normalized_01_times = d["time_stamps"] / (
-        0.0000001 + d["time_stamps"].max(axis=1, keepdim=True)[0]
-    )
+    # normalized_01_times = d["time_stamps"] / (
+    #     0.0000001 + d["time_stamps"].max(axis=1, keepdim=True)[0]
+    # )
     normalized_times = (
         d["time_stamps"] - d["time_stamps"].max(axis=1, keepdims=True)[0]
     ) / 100

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)])
@@ -163,11 +175,11 @@ class Planner:
     def __init__(self, dynamics):
         self.dynamics = dynamics
         self.current_direction = None
+        self.epsilon = 1  # original was 0.001
 
     def get_bounce_pos_and_new_direction(self, p, direction):
-        epsilon = 0.001
         distance_to_bounce = self.dynamics.binary_search_edge(
-            0, 10000, p, direction, epsilon
+            0, 10000, p, direction, self.epsilon
         )
         last_point_before_bounce = distance_to_bounce * direction + p
 
@@ -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,25 +226,29 @@ 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:
-    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(
             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)
+        logging.info(f"GRBL ONLINE {grbl_out}")
         self.position = {"time": time.time(), "xy": np.zeros(2)}
         self.update_status()
         time.sleep(0.05)
@@ -247,15 +264,16 @@ def update_status(self, skip_write=False):
             self.s.write("?".encode())
         time.sleep(0.01)
 
-        start_time = time.time()
         response = self.s.readline().decode().strip()
         time.sleep(0.01)
         # print("STATUS",response)
         # <Idle|MPos:-3589.880,79.560,0.000,0.000|FS:0,0>
         try:
             motor_position_str = response.split("|")[1]
         except Exception as e:
-            print("FAILED TO PARSE", response, "|e|", e, time.time() - start_time)
+            if response.strip() == "ok" or response.strip() == "":
+                return self.update_status(skip_write=not skip_write)
+            logging.warning(f"Failed to parse grbl output, {response}, {e}")
             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 +297,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 +310,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 +345,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
@@ -352,21 +375,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(serial_fn):
     dynamics = Dynamics(
         calibration_point=home_calibration_point,
         pA=home_pA,
@@ -380,7 +397,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 +425,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)