feat(coordinator): add Swarm (PIBT+LNS+BVC) planner and more

firmware/MiniBot_Coordinator/config.py

@@ -9,18 +9,29 @@
 # BOARD — physical geometry (all units in mm unless noted)
 # ---------------------------------------------------------------------------
 class BOARD:
-    SQUARE_SIZE_MM = 50  # one chess square side length
+    # Square size = 2.25 in (57.15 mm), the FIDE/USCF regulation tournament square.
+    # This is a deliberate step up from a tighter 50 mm square, for two reasons:
+    #   1. Standard tournament sizing (real 2.25 in boards and pieces).
+    #   2. Collision-free motion headroom. A MiniBot is ~31 mm across, so at 50 mm
+    #      the gap between piece bodies on adjacent squares is only ~19 mm; the
+    #      buffered-Voronoi routing in the swarm planner then has almost no room to
+    #      slip a piece past its neighbours and deadlocks/detours far more often.
+    #      At 57.15 mm the gap grows to ~26 mm, which is what lets the planner keep
+    #      resets and dense piece shuffles collision-free and efficient. Measured
+    #      on scattered-reset benchmarks the larger square markedly improves
+    #      completion and reduces total travel. See planning/swarm/README.md.
+    SQUARE_SIZE_MM = 57.15  # one chess square side length (2.25 in tournament size)
     NUM_SQUARES = 8  # 8×8 board
-    PLAYING_AREA_MM = SQUARE_SIZE_MM * NUM_SQUARES  # 400 mm
+    PLAYING_AREA_MM = SQUARE_SIZE_MM * NUM_SQUARES  # 457.2 mm
 
     BORDER_TOP_MM = 10
     BORDER_BOTTOM_MM = 10
     BORDER_LEFT_MM = 100
     BORDER_RIGHT_MM = 100
 
     # Total canvas in mm (logical coordinate space)
-    CANVAS_WIDTH_MM = PLAYING_AREA_MM + BORDER_LEFT_MM + BORDER_RIGHT_MM  # 650
-    CANVAS_HEIGHT_MM = PLAYING_AREA_MM + BORDER_TOP_MM + BORDER_BOTTOM_MM  # 450
+    CANVAS_WIDTH_MM = PLAYING_AREA_MM + BORDER_LEFT_MM + BORDER_RIGHT_MM  # 657.2
+    CANVAS_HEIGHT_MM = PLAYING_AREA_MM + BORDER_TOP_MM + BORDER_BOTTOM_MM  # 477.2
 
     # Coordinate origin: (0,0) = bottom-left corner of the playing area
     # X increases → right, Y increases → up (standard math orientation)
@@ -62,7 +73,7 @@ class PIECES:
     # Origin (0,0) is bottom-left corner of the PLAYING AREA.
     # Squares are labeled a-h (columns, x) and 1-8 (rows, y).
     # Square center col c, row r  →  x = (c-0.5)*SQUARE_SIZE, y = (r-0.5)*SQUARE_SIZE
-    _S = 50  # shorthand for SQUARE_SIZE_MM
+    _S = BOARD.SQUARE_SIZE_MM  # shorthand; tracks BOARD so the two never drift
 
     # White pieces (IDs 0x01–0x11)
     # Standard back rank: R  N  B  Q  K  B  N  R  (cols 1–8)
@@ -299,6 +310,7 @@ class PLANNING:
             "planning.enhanced_conflict_planner",
             "EnhancedConflictPlanner",
         ),
+        "Swarm (PIBT+LNS)": ("planning.swarm_planner", "SwarmPlanner"),
         "Direct (debug only)": ("planning.direct_planner", "DirectPlanner"),
     }
 

firmware/MiniBot_Coordinator/gui/chessboard_widget.py

@@ -2,9 +2,9 @@
 gui/chessboard_widget.py  —  MiniBot Chess Swarm Coordinator
 
 Custom QWidget that renders the physical chessboard environment:
-  - 125mm left/right borders, 25mm top/bottom borders (all drawn to scale)
+  - Border margins around the play area (sizes from config.BOARD)
   - Hard outer outline rectangle
-  - 8×8 checkerboard (50mm squares)
+  - 8×8 checkerboard (square size = config.BOARD.SQUARE_SIZE_MM, 57.15 mm)
   - 34 robot pieces as labeled circles with orientation markers
   - Selected-piece highlight and target-position indicator
 
@@ -46,9 +46,9 @@
 class ChessBoardWidget(QWidget):
     """Scaled chessboard canvas widget.
 
-    The logical coordinate space is (0,0) bottom-left of the playing area,
-    650 mm wide × 450 mm tall.  The canvas scales to fill the widget size
-    while preserving the aspect ratio.
+    The logical coordinate space is (0,0) bottom-left of the playing area; the
+    canvas spans the playing area plus its border margins (all from config.BOARD)
+    and scales to fill the widget size while preserving the aspect ratio.
     """
 
     piece_selected = pyqtSignal(int)  # piece_id
@@ -58,14 +58,15 @@ class ChessBoardWidget(QWidget):
     target_queued = pyqtSignal(
         int, float, float
     )  # piece_id, x_mm, y_mm  (right-click → immediate dispatch)
+    square_clicked = pyqtSignal(int, int)  # file(0-7), rank(0-7); only in chess mode
 
     # ------------------------------------------------------------------
     # Logical canvas constants (mm)
     # ------------------------------------------------------------------
-    _LW = BOARD.CANVAS_WIDTH_MM  # 650
-    _LH = BOARD.CANVAS_HEIGHT_MM  # 450
-    _OX = BOARD.BORDER_LEFT_MM  # 125  offset of playing area origin
-    _OY = BOARD.BORDER_BOTTOM_MM  # 25
+    _LW = BOARD.CANVAS_WIDTH_MM  # full canvas width (playing area + L/R borders)
+    _LH = BOARD.CANVAS_HEIGHT_MM  # full canvas height (playing area + T/B borders)
+    _OX = BOARD.BORDER_LEFT_MM  # x offset of playing-area origin within the canvas
+    _OY = BOARD.BORDER_BOTTOM_MM  # y offset of playing-area origin within the canvas
 
     def __init__(
         self, board_state: BoardState, parent: Optional[QWidget] = None
@@ -79,6 +80,10 @@ def __init__(
         self._show_electromagnets: bool = False
         # Plan visualization: list of (x0, y0, x1, y1, wave_idx, total_waves)
         self._plan_arrows: List[Tuple[float, float, float, float, int, int]] = []
+        # Chess mode: route clicks as board squares and highlight legal moves.
+        self._chess_mode: bool = False
+        self._legal_squares: Set[Tuple[int, int]] = set()  # (file, rank) dots
+        self._sel_square: Optional[Tuple[int, int]] = None  # (file, rank) ring
 
         self.setMinimumSize(
             int(BOARD.CANVAS_WIDTH_MM * GUI.SCALE_FACTOR),
@@ -162,13 +167,48 @@ def set_target(self, piece_id: int, x_mm: float, y_mm: float) -> None:
         self._target = (x_mm, y_mm)
         self.update()
 
+    def set_chess_mode(self, on: bool) -> None:
+        """Route board clicks as chess squares (Play Chess tab) when on."""
+        self._chess_mode = on
+        self._selected_id = None
+        self._target = None
+        if not on:
+            self._legal_squares = set()
+            self._sel_square = None
+        self.update()
+
+    def set_legal_highlights(
+        self,
+        squares: Set[Tuple[int, int]],
+        selected: Optional[Tuple[int, int]] = None,
+    ) -> None:
+        """Highlight legal destination squares (dots) and the selected one (ring)."""
+        self._legal_squares = set(squares)
+        self._sel_square = selected
+        self.update()
+
+    def clear_legal_highlights(self) -> None:
+        """Clear all chess move highlights."""
+        self._legal_squares = set()
+        self._sel_square = None
+        self.update()
+
     # ------------------------------------------------------------------
     # Mouse interaction
     # ------------------------------------------------------------------
 
     def mousePressEvent(self, event: QMouseEvent) -> None:
         self.setFocus()
         board_x, board_y = self._widget_to_mm(event.position())
+
+        # ── Chess mode: emit the clicked board square, bypass planning clicks ──
+        if self._chess_mode:
+            if event.button() == Qt.MouseButton.LeftButton:
+                sq = self._square_at_mm(board_x, board_y)
+                if sq is not None:
+                    self.square_clicked.emit(sq[0], sq[1])
+            return
+
         clicked_pid = self._piece_at(board_x, board_y)
         btn = event.button()
 
@@ -258,6 +298,7 @@ def paintEvent(self, event: QPaintEvent) -> None:
         self._draw_background(painter)
         self._draw_outer_outline(painter)
         self._draw_grid(painter)
+        self._draw_chess_highlights(painter)
         self._draw_plan_paths(painter)
         self._draw_electromagnets(painter)
         self._draw_target_indicator(painter)
@@ -324,6 +365,27 @@ def _draw_grid(self, p: QPainter) -> None:
             _, cy = self._pa_to_canvas(0, float(row * s))
             p.drawLine(QPointF(cx0, cy), QPointF(cx1, cy))
 
+    def _draw_chess_highlights(self, p: QPainter) -> None:
+        """Draw legal-move dots and a ring on the selected square (chess mode)."""
+        if not self._chess_mode:
+            return
+        s = BOARD.SQUARE_SIZE_MM
+        if self._sel_square is not None:
+            f, r = self._sel_square
+            cx, cy = self._pa_to_canvas((f + 0.5) * s, (r + 0.5) * s)
+            pen = QPen(QColor(GUI.SELECTED_HIGHLIGHT_COLOR))
+            pen.setWidthF(2.5)
+            p.setPen(pen)
+            p.setBrush(Qt.BrushStyle.NoBrush)
+            half = s / 2.0 - 2.0
+            p.drawRect(QRectF(cx - half, cy - half, 2 * half, 2 * half))
+        dot_r = s * 0.16
+        p.setPen(Qt.PenStyle.NoPen)
+        p.setBrush(QBrush(QColor(0, 200, 90, 150)))
+        for f, r in self._legal_squares:
+            cx, cy = self._pa_to_canvas((f + 0.5) * s, (r + 0.5) * s)
+            p.drawEllipse(QPointF(cx, cy), dot_r, dot_r)
+
     def _draw_plan_paths(self, p: QPainter) -> None:
         """Draw colored wave arrows for planned moves."""
         if not self._plan_arrows:
@@ -538,6 +600,17 @@ def _piece_at(self, pa_x: float, pa_y: float) -> Optional[int]:
 
         return closest_id
 
+    def _square_at_mm(self, x_mm: float, y_mm: float) -> Optional[Tuple[int, int]]:
+        """Playing-area mm → (file, rank) on the 8x8 board, or None if off-board."""
+        s = BOARD.SQUARE_SIZE_MM
+        if x_mm < 0 or y_mm < 0:
+            return None
+        f = int(x_mm // s)
+        r = int(y_mm // s)
+        if 0 <= f < BOARD.NUM_SQUARES and 0 <= r < BOARD.NUM_SQUARES:
+            return (f, r)
+        return None
+
     # ------------------------------------------------------------------
     # Initialise scale fields so _widget_to_mm works before first paint
     # ------------------------------------------------------------------

firmware/MiniBot_Coordinator/gui/main_window.py

@@ -55,6 +55,7 @@
 from gui.tabs.system_control_tab import SystemControlTab
 from gui.tabs.position_tracker_tab import PositionTrackerTab
 from gui.tabs.command_looper_tab import CommandLooperTab
+from gui.tabs.play_chess_tab import PlayChessTab
 
 # ---------------------------------------------------------------------------
 # Application-wide dark theme stylesheet
@@ -418,8 +419,10 @@ def _build_ui(self) -> None:
         self._sys_tab = SystemControlTab(self)
         self._track_tab = PositionTrackerTab(self._board, self)
         self._looper_tab = CommandLooperTab(self)
+        self._chess_tab = PlayChessTab(self._board, self._board_widget, self)
 
         self._tabs.addTab(self._path_tab, "Path Planning")
+        self._tabs.addTab(self._chess_tab, "Play Chess")
         self._tabs.addTab(self._debug_tab, "Debug")
         self._tabs.addTab(self._sys_tab, "System Control")
         self._tabs.addTab(self._track_tab, "Position Tracker")
@@ -442,6 +445,12 @@ def _wire_signals(self) -> None:
         # Path planning → send (serial or sim depending on mode)
         self._path_tab.send_commands.connect(self._on_send_move_commands)
 
+        # Play Chess: board square clicks route here in chess mode; moves dispatch
+        # through the same handler so they animate with the selected planner.
+        self._board_widget.square_clicked.connect(self._chess_tab.on_square_clicked)
+        self._chess_tab.send_commands.connect(self._on_send_move_commands)
+        self._chess_tab.status_log.connect(self._debug_tab.on_sim_log)
+
         # Path planning → board visualization
         self._path_tab.plan_visualized.connect(self._on_plan_visualized)
 
@@ -545,20 +554,58 @@ def _on_looper_request_move(
         else:
             self._send_serial_move(cmd)
 
-    @pyqtSlot(list)
-    def _on_send_move_commands(self, commands: List[MoveCommand]) -> None:
-        """Dispatch commands in sequence-number waves.
+    @pyqtSlot(list, bool)
+    def _on_send_move_commands(
+        self, commands: List[MoveCommand], is_trajectory: bool = False
+    ) -> None:
+        """Dispatch commands to the simulator or serial link.
 
-        All commands sharing the same sequence_num are sent in parallel as one
-        wave. Later waves are delayed until the active wave is considered done.
+        A continuous-trajectory plan in simulator mode is played back smoothly
+        (all pieces advance in lockstep). Everything else uses the sequence-number
+        wave dispatcher: commands sharing a sequence_num go out together, and later
+        waves wait for the active wave to finish.
         """
         if not commands:
             return
 
         self._reset_wave_dispatch()
+        if is_trajectory and self._sim_mode:
+            self._play_trajectory_sim(commands)
+            return
         self._pending_waves = self._group_by_sequence(commands)
         self._dispatch_next_wave()
 
+    def _play_trajectory_sim(self, commands: List[MoveCommand]) -> None:
+        """Reconstruct per-piece time-synced paths and play them continuously.
+
+        Each piece's path is its start position followed by, for every wave, the
+        target it was commanded that wave or (forward-filled) its previous
+        position. The final heading is the last non-None target_theta commanded.
+        """
+        self._simulator.speed_mm_s = self._debug_tab.simulator_speed_mm_s
+        waves: Dict[int, Dict[int, MoveCommand]] = {}
+        for cmd in commands:
+            waves.setdefault(cmd.sequence_num, {})[cmd.piece_id] = cmd
+        wave_nums = sorted(waves)
+        piece_ids = {cmd.piece_id for cmd in commands}
+
+        paths: Dict[int, List[Tuple[float, float]]] = {}
+        final_theta: Dict[int, float] = {}
+        for pid in piece_ids:
+            piece = self._board.get_piece(pid)
+            prev = (piece.x_mm, piece.y_mm) if piece is not None else (0.0, 0.0)
+            seq = [prev]
+            for wnum in wave_nums:
+                cmd = waves[wnum].get(pid)
+                if cmd is not None:
+                    prev = (cmd.target_x_mm, cmd.target_y_mm)
+                    if cmd.target_theta is not None:
+                        final_theta[pid] = cmd.target_theta
+                seq.append(prev)
+            paths[pid] = seq
+
+        self._simulator.play_trajectory(paths, final_theta)
+
     @pyqtSlot(bytes)
     def _on_debug_send_raw(self, data: bytes) -> None:
         """Route a raw bytes send from the debug tab to serial or simulator."""