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

.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/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)
+        _ = 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
@@ -352,21 +376,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 +398,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 +426,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)