add grbl get_ready

spf/grbl/grbl_interactive.py

@@ -1,5 +1,6 @@
 import logging
 import sys
+import threading
 import time
 from abc import ABC, abstractmethod
 
@@ -306,7 +307,7 @@ def __init__(
         self.angle_increment = chord_length_to_angle(self.step_size, self.circle_radius)
 
     def get_planner_start_position(self):
-        return circle_center
+        return self.angle_to_pos(0)
 
     def angle_to_pos(self, angle):
         return self.circle_center + self.circle_radius * np.array(
@@ -495,9 +496,10 @@ def move_to_iter(self, points_by_channel, return_once_stopped=False):
             self.move_to(next_points)
             while self.targets_far_out(next_points):
                 time.sleep(0.1)
-            if previous_points is not None:
-                breakpoint()
-                a = 1
+            if previous_points is not None and return_once_stopped:
+                if np.all([next_points[x] == previous_points[x] for x in next_points]):
+                    return
+            previous_points = next_points
 
     def close(self):
         self.s.close()
@@ -524,15 +526,38 @@ def __init__(self, controller):
             "tx_circle": self.tx_circle,
             "bounce": self.bounce,
         }
+        self.ready = threading.Lock()
+        self.ready.acquire()
 
-    def run(self):
+    def get_ready(self):
         # default start points are the current start points
-        start_points = gm.controller.position["xy"]
+        self.requested_start_points = self.controller.position["xy"][:]  # make a copy
         for c in self.channels:
             requested_point = self.planners[c].get_planner_start_position()
             if requested_point is not None:
-                start_points[c] = requested_point
-        breakpoint()
+                self.requested_start_points[c] = requested_point
+
+        logging.info("Moving into position")
+        # planners to move into place
+        move_into_place_planners = {
+            c_idx: StationaryPlanner(
+                self.controller.dynamics,
+                start_point=self.controller.position["xy"][c_idx],
+                stationary_point=self.requested_start_points[c_idx],
+            ).yield_points()
+            for c_idx in range(len(self.planners))
+        }
+        self.controller.move_to_iter(move_into_place_planners, return_once_stopped=True)
+        # wait for finish moving
+        while run_grbl and self.controller.position["is_moving"]:
+            self.controller.update_status()
+            time.sleep(0.1)
+
+    def run(self):
+        # planners to run routine
+        logging.info("Running routine")
+        for c_idx in range(len(self.planners)):
+            self.planners[c_idx].start_point = self.requested_start_points[c_idx]
         points_by_channel = {c: self.planners[c].yield_points() for c in self.channels}
         self.controller.move_to_iter(points_by_channel)
 
@@ -544,6 +569,7 @@ def bounce(self):
             )
             for c_idx in range(len(self.planners))
         ]
+        yield self.get_ready()
         self.run()
 
     def tx_circle(self):
@@ -560,7 +586,6 @@ def tx_circle(self):
                 circle_center=circle_center,
             ),
         ]
-        self.run()
 
     def rx_circle(self):
         self.planners = [
@@ -576,7 +601,6 @@ def rx_circle(self):
                 stationary_point=circle_center,
             ),
         ]
-        self.run()
 
     def v1_calibrate(self):
         self.planners = [
@@ -589,7 +613,6 @@ def v1_calibrate(self):
                 self.controller.dynamics, start_point=self.controller.position["xy"][1]
             ),
         ]
-        self.run()
 
 
 def get_default_gm(serial_fn):
@@ -636,6 +659,8 @@ def get_default_gm(serial_fn):
         line = line.strip()
         if line in gm.routines:
             gm.routines[line]()
+            gm.get_ready()
+            gm.run()
         elif line == "q":
             sys.exit(1)
         elif line == "s":

spf/grbl_sdr_collect_v2.py

@@ -10,20 +10,15 @@
 
 import numpy as np
 import yaml
-from grbl.grbl_interactive import get_default_gm, stop_grbl
 from tqdm import tqdm
 
+from grbl.grbl_interactive import get_default_gm, stop_grbl
 from spf.rf import beamformer
-from spf.sdrpluto.sdr_controller import (
-    EmitterConfig,
-    ReceiverConfig,
-    get_avg_phase,
-    get_pplus,
-    plot_recv_signal,
-    setup_rxtx,
-    setup_rxtx_and_phase_calibration,
-    shutdown_radios,
-)
+from spf.sdrpluto.sdr_controller import (EmitterConfig, ReceiverConfig,
+                                         get_avg_phase, get_pplus,
+                                         plot_recv_signal, setup_rxtx,
+                                         setup_rxtx_and_phase_calibration,
+                                         shutdown_radios)
 from spf.wall_array_v2 import v2_column_names
 
 faulthandler.enable()
@@ -149,6 +144,8 @@ def grbl_thread_runner(gm, routine):
                 if routine in gm.routines:
                     logging.info(f"RUNNING ROUTINE {routine}")
                     gm.routines[routine]()
+                    gm.get_ready()
+                    gm.run()
                 else:
                     raise ValueError(f"Unknown grbl routine f{routine}")
             except Exception as e:
@@ -339,11 +336,15 @@ def grbl_thread_runner(gm, routine):
     if run_collection:
         if args.grbl_serial is not None:
             gm = get_default_gm(args.grbl_serial)
+            gm.routines[yaml_config["routine"]]()  # setup run
+            logging.info("Waiting for GRBL to get into position")
+            gm.get_ready()  # move into position
             gm_thread = threading.Thread(
                 target=grbl_thread_runner, args=(gm, yaml_config["routine"])
             )
             gm_thread.start()
 
+    time.sleep(0.2)
     # setup read threads
     read_threads = []
     time_offset = time.time()