From 60e7d3978a749c53d7eed6e6c30056ce591198b4 Mon Sep 17 00:00:00 2001
From: Misko <misko@cs.toronto.edu>
Date: Thu, 11 Jan 2024 19:27:21 -0800
Subject: [PATCH] grbl fixes and add cricle planner

fix planners

grbl routine fixes

remove unused import

fixes

fixes
---
 spf/grbl/grbl_interactive.py                  | 175 +++++++++++++-----
 spf/grbl_sdr_collect_v2.py                    |  95 ++++++----
 ...rray_v2_bounce.yaml => wall_array_v2.yaml} |   2 +-
 spf/v2_configs/wall_array_v2_calibration.yaml |  60 ------
 spf/v2_configs/wall_array_v2_noserial.yaml    |   2 +-
 tests/test_grbl.py                            |   4 +-
 6 files changed, 187 insertions(+), 151 deletions(-)
 rename spf/v2_configs/{wall_array_v2_bounce.yaml => wall_array_v2.yaml} (99%)
 delete mode 100644 spf/v2_configs/wall_array_v2_calibration.yaml

diff --git a/spf/grbl/grbl_interactive.py b/spf/grbl/grbl_interactive.py
index cf03838a..e18c5d42 100644
--- a/spf/grbl/grbl_interactive.py
+++ b/spf/grbl/grbl_interactive.py
@@ -28,6 +28,10 @@
 run_grbl = True
 
 
+def chord_length_to_angle(chord_length, radius):
+    return 2 * np.arcsin(chord_length / (2 * radius))
+
+
 def stop_grbl():
     logging.info("STOP GRBL")
     global run_grbl
@@ -64,13 +68,13 @@ def a_to_b_in_stepsize(a, b, step_size):
     if np.isclose(a, b).all():
         return [b]
     # move by step_size from where we are now to the target position
-    points = [a]
+    points = []
     direction = (b - a) / np.linalg.norm(b - a)
     distance = np.linalg.norm(b - a)
-    _l = 0
+    _l = step_size
     while _l < distance:
         points.append(_l * direction + a)
-        _l = _l + step_size
+        _l += step_size
     points.append(b)
     return points
 
@@ -232,7 +236,7 @@ def single_bounce(self, direction, p):
     def yield_points(self):
         yield from self.bounce(self.start_point)
 
-    def bounce(self, current_p):
+    def bounce(self, current_p, total_bounces=-1):
         global run_grbl
         # if no previous direciton lets initialize one
         if self.current_direction is None:
@@ -244,7 +248,8 @@ def bounce(self, current_p):
         ) * self.current_direction + percent_random * self.random_direction()
         self.current_direction /= np.linalg.norm(self.current_direction)
 
-        while True:
+        n_bounce = 0
+        while total_bounces < 0 or n_bounce < total_bounces:
             if not run_grbl:
                 logging.info("Exiting bounce early")
                 break
@@ -255,6 +260,7 @@ def bounce(self, current_p):
             yield from to_points
             current_p = to_points[-1]
             self.current_direction = new_direction
+            n_bounce += 1
         logging.info("Exiting bounce")
 
 
@@ -270,27 +276,49 @@ def yield_points(
     ):
         # move to the stationary point
         yield from a_to_b_in_stepsize(
-            self.start_position, self.stationary_point, step_size=self.step_size
+            self.start_point, self.stationary_point, step_size=self.step_size
         )
         # stay still
         while True:
             yield self.stationary_point
 
 
-class CalibrationCirclePlanner(Planner):
+class CirclePlanner(Planner):
     def __init__(
         self,
         dynamics,
         start_point,
         step_size=5,
-        circle_diameter=100,
+        circle_diameter=max_circle_diameter,
         circle_center=[0, 0],
     ):
         super().__init__(
             dynamics=dynamics, start_point=start_point, step_size=step_size
         )
         self.circle_center = circle_center
-        self.circle_diameter = circle_diameter
+        self.circle_radius = circle_diameter / 2
+        self.angle_increment = chord_length_to_angle(self.step_size, self.circle_radius)
+
+    def angle_to_pos(self, angle):
+        return self.circle_center + self.circle_radius * np.array(
+            [-np.sin(angle), np.cos(angle)]
+        )
+
+    def yield_points(self):
+        # start at the top of the circle
+        current_p = self.start_point
+
+        # start at the top
+        current_angle = 0
+        while current_angle < 360:
+            next_p = self.angle_to_pos(current_angle)
+            yield from a_to_b_in_stepsize(
+                current_p,
+                next_p,
+                step_size=self.step_size,
+            )
+            current_p = next_p
+            current_angle += self.angle_increment
 
 
 class CalibationV1Planner(Planner):
@@ -453,7 +481,7 @@ def move_to_iter(self, points_by_channel):
                 break
             self.move_to(next_points)
             while self.targets_far_out(next_points):
-                pass
+                time.sleep(0.1)
 
     def close(self):
         self.s.close()
@@ -470,28 +498,75 @@ def get_next_points(channel_iterators):
 
 
 class GRBLManager:
-    def __init__(self, controller, planners):
+    def __init__(self, controller):
         self.controller = controller
         self.channels = list(self.controller.channel_to_motor_map.keys())
-        self.planners = planners
-
-    def bounce(self, n_bounces, direction=None):
-        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.planners = [None for x in self.channels]
+        self.routines = {
+            "v1_calibrate": self.v1_calibrate,
+            "rx_circle": self.rx_circle,
+            "tx_circle": self.tx_circle,
+            "bounce": self.bounce,
         }
+
+    def run(self):
+        points_by_channel = {c: self.planners[c].yield_points() for c in self.channels}
         self.controller.move_to_iter(points_by_channel)
 
-    def calibrate(self):
-        start_positions = self.controller.update_status()["xy"]
-        points_by_channel = {
-            0: self.planners[0].stationary_point(
-                start_positions[0], np.array(rx_calibration_point)
+    def bounce(self):
+        self.planners = [
+            BouncePlanner(
+                self.controller.dynamics,
+                start_point=self.controller.position["xy"][c_idx],
+            )
+            for c_idx in range(len(self.planners))
+        ]
+        self.run()
+
+    def tx_circle(self):
+        self.planners = [
+            StationaryPlanner(
+                self.controller.dynamics,
+                start_point=self.controller.position["xy"][0],
+                stationary_point=circle_center,
             ),
-            1: self.planners[1].calibration_run(start_positions[1]),
-        }
-        self.controller.move_to_iter(points_by_channel)
+            CirclePlanner(
+                self.controller.dynamics,
+                start_point=self.controller.position["xy"][1],
+                circle_diameter=max_circle_diameter,
+                circle_center=circle_center,
+            ),
+        ]
+        self.run()
+
+    def rx_circle(self):
+        self.planners = [
+            CirclePlanner(
+                self.controller.dynamics,
+                start_point=self.controller.position["xy"][0],
+                circle_diameter=max_circle_diameter,
+                circle_center=circle_center,
+            ),
+            StationaryPlanner(
+                self.controller.dynamics,
+                start_point=self.controller.position["xy"][1],
+                stationary_point=circle_center,
+            ),
+        ]
+        self.run()
+
+    def v1_calibrate(self):
+        self.planners = [
+            StationaryPlanner(
+                self.controller.dynamics,
+                start_point=self.controller.position["xy"][0],
+                stationary_point=rx_calibration_point,
+            ),
+            BouncePlanner(
+                self.controller.dynamics, start_point=self.controller.position["xy"][1]
+            ),
+        ]
+        self.run()
 
 
 def get_default_gm(serial_fn):
@@ -502,13 +577,13 @@ def get_default_gm(serial_fn):
         bounding_box=home_bounding_box,
     )
 
-    planners = {0: Planner(dynamics), 1: Planner(dynamics)}
+    # planners = {0: Planner(dynamics), 1: Planner(dynamics)}
 
     controller = GRBLController(
         serial_fn, dynamics, channel_to_motor_map={0: "XY", 1: "ZA"}
     )
 
-    return GRBLManager(controller, planners)
+    return GRBLManager(controller)
 
 
 if __name__ == "__main__":
@@ -520,37 +595,41 @@ def get_default_gm(serial_fn):
 
     gm = get_default_gm(serial_fn)
 
+    if gm.controller.position["is_moving"]:
+        print("Waiting for grbl to stop moving before starting...")
+        while gm.controller.position["is_moving"]:
+            time.sleep(0.1)
+            gm.controller.update_status()
     print(
-        """
-        q = quit
-        bounce = bounce
+        """q = quit
         s = status
         X,Y = move to X,Y
-    """
+
+        Routines:"""
     )
+    for k in gm.routines:
+        print("        ", k)
     for line in sys.stdin:
         line = line.strip()
-        if line == "q":
+        if line in gm.routines:
+            gm.routines[line]()
+        elif line == "q":
             sys.exit(1)
-        elif line == "bounce":
-            # gm.bounce(20000)
-            gm.bounce(40)
-        elif line == "calibrate":
-            gm.calibrate()
         elif line == "s":
             p = gm.controller.update_status()
             print(p)
         else:
-            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]
-                }
-                gm.controller.move_to_iter(points_iter)
-            # except ValueError:
-            #    print("Position is out of bounds!")
+            points_iter = {
+                c: iter(
+                    a_to_b_in_stepsize(
+                        gm.controller.update_status()["xy"][c],
+                        np.array([float(x) for x in line.split()]),
+                        5,
+                    )
+                )
+                for c in [0, 1]
+            }
+            gm.controller.move_to_iter(points_iter)
         time.sleep(0.01)
 
     gm.close()
diff --git a/spf/grbl_sdr_collect_v2.py b/spf/grbl_sdr_collect_v2.py
index fc88e344..b2e96904 100644
--- a/spf/grbl_sdr_collect_v2.py
+++ b/spf/grbl_sdr_collect_v2.py
@@ -136,23 +136,26 @@ def read_forever(self):
         logging.info(f"{str(self.pplus.rx_config.uri)} PPlus read_forever() exit!")
 
 
-def grbl_thread_runner(gm, planner):
-    direction = None
+def grbl_thread_runner(gm, routine):
     global run_collection
     while run_collection:
         logging.info("GRBL thread runner")
-        try:
-            if planner == "bounce":
-                gm.bounce(-1, direction=direction)
-            elif planner == "calibration":
-                gm.calibrate()
-            else:
-                raise ValueError(f"Unknown grbl planner f{planner}")
-        except Exception as e:
-            logging.error(e)
+        if routine is None:
+            logging.info("No routine to run, just spining")
+            while run_collection:
+                time.sleep(0.5)
+        else:
+            try:
+                if routine in gm.routines:
+                    logging.info(f"RUNNING ROUTINE {routine}")
+                    gm.routines[routine]()
+                else:
+                    raise ValueError(f"Unknown grbl routine f{routine}")
+            except Exception as e:
+                logging.error(e)
         if not run_collection:
             break
-        logging.info("TRY TO BOUNCE RET")
+        logging.info("GRBL thread runner loop")
         time.sleep(10)  # cool off the motor
     logging.info("Exiting GRBL thread")
 
@@ -174,6 +177,9 @@ def grbl_thread_runner(gm, planner):
         default="INFO",
         required=False,
     )
+    parser.add_argument(
+        "-r", "--routine", type=str, help="GRBL routine", required=False, default=None
+    )
     parser.add_argument(
         "-s",
         "--grbl-serial",
@@ -197,7 +203,11 @@ def grbl_thread_runner(gm, planner):
     with open(args.yaml_config, "r") as stream:
         yaml_config = yaml.safe_load(stream)
 
-    output_files_prefix = f"wallarrayv2_{run_started_at}_nRX{len(yaml_config['receivers'])}_{yaml_config['planner']}"
+    # add in our current config
+    if args.routine is not None:
+        yaml_config["routine"] = args.routine
+
+    output_files_prefix = f"wallarrayv2_{run_started_at}_nRX{len(yaml_config['receivers'])}_{yaml_config['routine']}"
 
     # setup logging
     logging.basicConfig(
@@ -208,33 +218,40 @@ def grbl_thread_runner(gm, planner):
         format="%(asctime)s:%(levelname)s:%(message)s",
         level=getattr(logging, args.logging_level.upper(), None),
     )
+    if args.grbl_serial is None:
+        logging.info("Running without GRBL SERIAL!!")
+        for x in range(50):
+            if not run_collection:
+                break
+            time.sleep(0.1)
 
-    with open(f"{output_files_prefix}.yaml", "w") as outfile:
-        yaml.dump(yaml_config, outfile, default_flow_style=False)
-
-    # record matrix
-    column_names = v2_column_names(nthetas=yaml_config["n-thetas"])
-    record_matrix = np.memmap(
-        f"{output_files_prefix}.npy",
-        dtype="float32",
-        mode="w+",
-        shape=(
-            2,  # TODO should be nreceivers
-            yaml_config["n-records-per-receiver"],
-            len(column_names),
-        ),  # t,tx,ty,rx,ry,rtheta,rspacing /  avg1,avg2 /  sds
-    )
+    if run_collection:
+        with open(f"{output_files_prefix}.yaml", "w") as outfile:
+            yaml.dump(yaml_config, outfile, default_flow_style=False)
+
+        # record matrix
+        column_names = v2_column_names(nthetas=yaml_config["n-thetas"])
+        record_matrix = np.memmap(
+            f"{output_files_prefix}.npy",
+            dtype="float32",
+            mode="w+",
+            shape=(
+                2,  # TODO should be nreceivers
+                yaml_config["n-records-per-receiver"],
+                len(column_names),
+            ),  # t,tx,ty,rx,ry,rtheta,rspacing /  avg1,avg2 /  sds
+        )
 
-    logging.info(json.dumps(yaml_config, sort_keys=True, indent=4))
+        logging.info(json.dumps(yaml_config, sort_keys=True, indent=4))
 
-    # lets open all the radios
-    radio_uris = ["ip:%s" % yaml_config["emitter"]["receiver-ip"]]
-    for receiver in yaml_config["receivers"]:
-        radio_uris.append("ip:%s" % receiver["receiver-ip"])
-    for radio_uri in radio_uris:
-        get_pplus(uri=radio_uri)
+        # lets open all the radios
+        radio_uris = ["ip:%s" % yaml_config["emitter"]["receiver-ip"]]
+        for receiver in yaml_config["receivers"]:
+            radio_uris.append("ip:%s" % receiver["receiver-ip"])
+        for radio_uri in radio_uris:
+            get_pplus(uri=radio_uri)
 
-    time.sleep(0.1)
+        time.sleep(0.1)
 
     # get radios online
     receiver_pplus = []
@@ -275,7 +292,7 @@ def grbl_thread_runner(gm, planner):
             )
             if pplus_rx is None or pplus_tx is None:
                 logging.info("Failed to bring RXTX online, shuttingdown")
-                run_collection=False
+                run_collection = False
                 break
             else:
                 logging.debug("RX online!")
@@ -323,7 +340,7 @@ def grbl_thread_runner(gm, planner):
         if args.grbl_serial is not None:
             gm = get_default_gm(args.grbl_serial)
             gm_thread = threading.Thread(
-                target=grbl_thread_runner, args=(gm, yaml_config["planner"])
+                target=grbl_thread_runner, args=(gm, yaml_config["routine"])
             )
             gm_thread.start()
 
@@ -332,7 +349,7 @@ def grbl_thread_runner(gm, planner):
     time_offset = time.time()
     if run_collection:
         for pplus_rx in receiver_pplus:
-            if pplus_rx == None:
+            if pplus_rx is None:
                 continue
             read_thread = ThreadedRX(
                 pplus_rx, time_offset, nthetas=yaml_config["n-thetas"]
diff --git a/spf/v2_configs/wall_array_v2_bounce.yaml b/spf/v2_configs/wall_array_v2.yaml
similarity index 99%
rename from spf/v2_configs/wall_array_v2_bounce.yaml
rename to spf/v2_configs/wall_array_v2.yaml
index b8fc16e8..a12a1283 100644
--- a/spf/v2_configs/wall_array_v2_bounce.yaml
+++ b/spf/v2_configs/wall_array_v2.yaml
@@ -56,5 +56,5 @@ receivers:
 n-thetas: 65
 n-records-per-receiver: 300000
 width: 4000
-planner: bounce
 calibration-frames: 800
+routine: null
diff --git a/spf/v2_configs/wall_array_v2_calibration.yaml b/spf/v2_configs/wall_array_v2_calibration.yaml
deleted file mode 100644
index a7779130..00000000
--- a/spf/v2_configs/wall_array_v2_calibration.yaml
+++ /dev/null
@@ -1,60 +0,0 @@
-# The ip of the emitter
-# When the emitter is brought online it is verified
-# by a receiver that it actually is broadcasting
-emitter:
-  receiver-ip: 192.168.1.15
-  emitter-ip: 192.168.1.15
-  tx-gain: -10
-  rx-gain-mode: fast_attack
-  rx-gain: -3
-  buffer-size: 4096 # 2**12
-  f-intermediate: 100000 #1.0e5
-  f-carrier: 2500000000 #2.5e9
-  f-sampling: 16000000 # 16.0e6
-  bandwidth: 300000 #3.0e5
-  motor_channel: 1
-
-# Two receivers each with two antennas
-# When a receiver is brought online it performs
-# phase calibration using an emitter equidistant from
-# both receiver antenna
-# The orientation of the receiver is described in 
-# multiples of pi
-receivers:
-  - receiver-ip: 192.168.1.17
-    emitter-ip: 192.168.1.17
-    theta-in-pis: -0.25
-    antenna-spacing-m: 0.065 # 62.5 mm -> 6.5cm -> 0.065m
-    nelements: 2
-    array-type: linear
-    rx-gain-mode: slow_attack
-    rx-buffers: 4
-    rx-gain: -3
-    buffer-size: 4096 # 2**12
-    f-intermediate: 100000 #1.0e5
-    f-carrier: 2500000000 #2.5e9
-    f-sampling: 16000000 # 16.0e6
-    bandwidth: 300000 #3.0e5
-    motor_channel: 0
-  - receiver-ip: 192.168.1.18
-    emitter-ip: 192.168.1.17
-    theta-in-pis: 1.25
-    antenna-spacing-m: 0.065 # 62.5 mm -> 6.5cm -> 0.065m
-    nelements: 2
-    array-type: linear
-    rx-gain-mode: slow_attack
-    rx-buffers: 4
-    rx-gain: -3
-    buffer-size: 4096 # 2**12
-    f-intermediate: 100000 #1.0e5
-    f-carrier: 2500000000 #2.5e9
-    f-sampling: 16000000 # 16.0e6
-    bandwidth: 300000 #3.0e5
-    motor_channel: 0
-
-
-n-thetas: 65
-n-records-per-receiver: 300000
-width: 4000
-planner: calibration
-calibration-frames: 800
diff --git a/spf/v2_configs/wall_array_v2_noserial.yaml b/spf/v2_configs/wall_array_v2_noserial.yaml
index 99499d0e..5c1756a6 100644
--- a/spf/v2_configs/wall_array_v2_noserial.yaml
+++ b/spf/v2_configs/wall_array_v2_noserial.yaml
@@ -57,5 +57,5 @@ output-file: wallarrayv2___DATE__.npy
 n-thetas: 65
 n-records-per-receiver: 300000
 width: 4000
-planner: calibration
+routine: null
  
diff --git a/tests/test_grbl.py b/tests/test_grbl.py
index 162f9df5..4857a7a1 100644
--- a/tests/test_grbl.py
+++ b/tests/test_grbl.py
@@ -5,8 +5,8 @@
 from shapely import geometry
 
 from spf.grbl.grbl_interactive import (
+    BouncePlanner,
     Dynamics,
-    Planner,
     home_bounding_box,
     home_calibration_point,
     home_pA,
@@ -98,7 +98,7 @@ def test_binary_search_edge():
         pB=home_pB,
         bounding_box=home_bounding_box,
     )
-    planner = Planner(dynamics)
+    planner = BouncePlanner(dynamics, start_point=[0, 0])
 
     direction = np.array([3, 1])
     p = np.array([1500, 900])