From 4e5936d4847550e2ccf5bbbe8e71ab1628982f08 Mon Sep 17 00:00:00 2001 From: misko Date: Sun, 7 Jan 2024 02:29:06 +0000 Subject: [PATCH] fixes for grbl controller --- .gitignore | 1 + .vscode/settings.json | 2 +- spf/grbl/grbl_interactive.py | 40 ++++++++++++++++++++++++++++-------- spf/grbl_sdr_collect_v2.py | 14 ++++++++----- 4 files changed, 43 insertions(+), 14 deletions(-) diff --git a/.gitignore b/.gitignore index a3c7304b..19673af9 100644 --- a/.gitignore +++ b/.gitignore @@ -12,3 +12,4 @@ test_data.txt **/.ipynb_checkpoints **/*.pkl **/*.log +**/token \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index 73040feb..ec89e9a7 100644 --- a/.vscode/settings.json +++ b/.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" }, diff --git a/spf/grbl/grbl_interactive.py b/spf/grbl/grbl_interactive.py index 124c22ca..ed247b8d 100644 --- a/spf/grbl/grbl_interactive.py +++ b/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,6 +226,9 @@ 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 ) @@ -220,6 +236,7 @@ def bounce(self, start_p, n_bounces): 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, diff --git a/spf/grbl_sdr_collect_v2.py b/spf/grbl_sdr_collect_v2.py index 57babea3..8ed9e5ae 100644 --- a/spf/grbl_sdr_collect_v2.py +++ b/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