Adding debug mode

README.md

@@ -19,6 +19,10 @@ Theremini turns Reachy Mini into a music instrument. The robot's head roll sets
 
 When the app starts, Reachy performs a short guided motion (center the head, oscillate the roll, sweep the antenna) so musicians immediately hear how gestures map to sound before the motors are relaxed.
 
+## Debug mode
+
+For latency benchmarking without manual gestures, launch `python main.py --debug`. The robot will loop through a small roll sine sweep followed by a two-second pause, while the console prints motion commands and corresponding sound-trigger times so you can compare movement-to-audio delays on the CM4.
+
 ## Controls
 
 | Control            | Range                     | Target             |

Theremini/main.py

@@ -1,5 +1,6 @@
 from __future__ import annotations
 
+import argparse
 import contextlib
 import io
 import math
@@ -51,7 +52,7 @@ class Theremini(ReachyMiniApp):
     custom_app_url: str | None = f"http://localhost:{DASHBOARD_PORT}/index.html"
     dont_start_webserver = True
 
-    def __init__(self) -> None:
+    def __init__(self, debug_mode: bool = False) -> None:
         super().__init__()
         self._session = Session(max_threads=1024)
         self._parts_cache: dict[str, Any] = {}
@@ -62,6 +63,10 @@ class Theremini(ReachyMiniApp):
         self._current_prog: int | None = None
         self._dashboard_port = DASHBOARD_PORT
         self.custom_app_url = f"http://localhost:{self._dashboard_port}/index.html"
+        self._debug_mode = debug_mode
+        self._debug_cycle_id = 0
+        self._debug_last_motion_command: float | None = None
+        self._debug_last_motion_log: float | None = None
 
     def _get_part_for_prog(self, prog: int):
         name = self._active_parts[prog]
@@ -177,15 +182,20 @@ class Theremini(ReachyMiniApp):
             self._stop_note()
             return
 
+        debug_event: str | None = None
         if self._note_handle is None:
             self._note_handle = self._theremin.start_note(target_pitch, int(amp * 100))
             self._current_pitch = target_pitch
+            debug_event = "start"
         elif target_pitch != self._current_pitch:
             self._stop_note()
             self._note_handle = self._theremin.start_note(target_pitch, int(amp * 100))
             self._current_pitch = target_pitch
+            debug_event = "pitch-change"
         else:
             _send_cc(self._theremin, 11, int(amp * 127))
+        if self._debug_mode and debug_event:
+            self._log_debug_sound_event(debug_event, target_pitch, amp)
 
     def _tick_from_robot(self, reachy_mini: ReachyMini) -> None:
         self._ensure_active_parts_fresh()
@@ -325,6 +335,82 @@ class Theremini(ReachyMiniApp):
                 break
             time.sleep(0.02)
 
+    def _log_debug_sound_event(self, event: str, pitch: int, amp: float) -> None:
+        if not self._debug_mode:
+            return
+        now = time.time()
+        since_motion = (
+            now - self._debug_last_motion_command
+            if self._debug_last_motion_command is not None
+            else float("nan")
+        )
+        print(
+            f"[debug][sound] cycle={self._debug_cycle_id} event={event} pitch={pitch}"
+            f" amp={amp:.2f} dt_since_motion={since_motion:.3f}s"
+        )
+
+    def _debug_motion_cycle(
+        self,
+        reachy_mini: ReachyMini,
+        stop_event: threading.Event,
+        amplitude_deg: float = 15.0,
+        sweep_duration: float = 1.0,
+        pause_duration: float = 2.0,
+    ) -> None:
+        self._debug_cycle_id += 1
+        cycle_id = self._debug_cycle_id
+        print(f"[debug] Starting sweep cycle {cycle_id}")
+        start = time.time()
+        self._debug_last_motion_log = None
+        while not stop_event.is_set():
+            elapsed = time.time() - start
+            if elapsed >= sweep_duration:
+                break
+            roll = amplitude_deg * math.sin(2 * math.pi * elapsed / sweep_duration)
+            pose = create_head_pose(z=0.0, roll=roll, degrees=True)
+            reachy_mini.set_target(head=pose)
+            now = time.time()
+            self._debug_last_motion_command = now
+            if self._debug_mode:
+                if self._debug_last_motion_log is None or now - self._debug_last_motion_log >= 0.2:
+                    print(
+                        f"[debug][motion] cycle={cycle_id} roll={roll:.2f}°"
+                        f" t={now:.3f}"
+                    )
+                    self._debug_last_motion_log = now
+            self._tick_from_robot(reachy_mini)
+            time.sleep(0.02)
+        print(f"[debug] Cycle {cycle_id} pause {pause_duration}s")
+        pause_end = time.time() + pause_duration
+        while not stop_event.is_set() and time.time() < pause_end:
+            self._tick_from_robot(reachy_mini)
+            time.sleep(0.02)
+
+    def run_debug(self, reachy_mini: ReachyMini, stop_event: threading.Event) -> None:
+        print("\nDebug mode: scripted sine sweep with 2s gaps to benchmark latency.\n")
+        self._debug_mode = True
+
+        dashboard_server = DashboardServer(port=self._dashboard_port)
+        dashboard_server.start()
+        self._dashboard_port = dashboard_server.port_in_use
+        self.custom_app_url = f"http://localhost:{self._dashboard_port}/index.html"
+        if self._dashboard_port != DASHBOARD_PORT:
+            print(
+                f"Dashboard port {DASHBOARD_PORT} busy, now serving on http://localhost:{self._dashboard_port}/index.html"
+            )
+        set_status(self._build_status_payload(0.0, 0.0, 0.0, 0.0, None, None, False))
+
+        reachy_mini.enable_motors()
+        try:
+            while not stop_event.is_set():
+                self._debug_motion_cycle(reachy_mini, stop_event)
+        finally:
+            reachy_mini.disable_motors()
+            self._stop_note()
+            set_status({"playing": False})
+            dashboard_server.stop()
+            print("\nDebug Theremin loop stopped.")
+
     def _angle_for_instrument_index(self, index: int) -> float:
         if len(self._active_parts) <= 1:
             return 0.0
@@ -395,14 +481,26 @@ class Theremini(ReachyMiniApp):
 
 
 if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description="Reachy Mini Theremin controller")
+    parser.add_argument(
+        "--debug",
+        action="store_true",
+        help="Run a scripted sine-sweep loop and log timings instead of the interactive mode",
+    )
+    args = parser.parse_args()
+
     with ReachyMini(media_backend="no_media") as mini:
-        app = Theremini()
+        app = Theremini(debug_mode=args.debug)
         stop = threading.Event()
         try:
-            print("Running 'Theremini' a ReachyMiniApp...")
-            print("Press Ctrl+C to stop the app.")
-            app.run(mini, stop)
-            print("App has stopped.")
+            if args.debug:
+                print("Press Ctrl+C to stop the debug loop.")
+                app.run_debug(mini, stop)
+            else:
+                print("Running 'Theremini' a ReachyMiniApp...")
+                print("Press Ctrl+C to stop the app.")
+                app.run(mini, stop)
+                print("App has stopped.")
         except KeyboardInterrupt:
             print("Stopping the app...")
             stop.set()