import json import queue import subprocess import sys import threading import time import traceback import math from pathlib import Path import tkinter as tk from tkinter import filedialog, messagebox, ttk import erpc REPO_ROOT = Path(__file__).resolve().parents[1] if str(REPO_ROOT) not in sys.path: sys.path.insert(0, str(REPO_ROOT)) from file_service import client as file_service_client from servo_service import client as servo_service_client from servo_service import common as servo_service_common from s_curve import SCurve, SCurveError, SCurvePositionPlanner1D SERVO_CHANNEL_COUNT = 48 DEFAULT_FILE_CHUNK_SIZE = 4096 CONFIG_DIR = REPO_ROOT / "config" SERVO_CONFIG_YAML_PATH = CONFIG_DIR / "servo_config.yaml" SERVO_CONFIG_PATH = CONFIG_DIR / "servo_config.json" YAML_TO_JSON_SCRIPT = CONFIG_DIR / "yaml_to_json.py" DEFAULT_REMOTE_SERVO_CONFIG_PATH = "config/servo_config.json" BLINK_TRANSITION_MS = 90 BLINK_STEP_MS = 15 EYE_CONTROL_SPECS = ( {"title": "Left Upper Lid", "short": "Upper Lid", "default_id": "eye_l_up", "blink": True}, {"title": "Left Lower Lid", "short": "Lower Lid", "default_id": "eye_l_lower", "blink": True}, {"title": "Right Upper Lid", "short": "Upper Lid", "default_id": "eye_r_up", "blink": True}, {"title": "Right Lower Lid", "short": "Lower Lid", "default_id": "eye_r_lower", "blink": True}, {"title": "Eyes Horizontal", "short": "Horizontal", "default_id": "", "blink": False}, {"title": "Eyes Vertical", "short": "Vertical", "default_id": "", "blink": False}, ) class FaceServoRpcClient: def __init__(self): self._transport = None self._manager = None self._servo_client = None self._file_client = None @property def is_connected(self): return self._servo_client is not None and self._file_client is not None def connect(self, port, baud): self.close() self._transport = erpc.transport.SerialTransport(port, baud) self._manager = erpc.client.ClientManager( self._transport, erpc.basic_codec.BasicCodec, ) self._servo_client = servo_service_client.servo_serviceClient(self._manager) self._file_client = file_service_client.file_serviceClient(self._manager) def close(self): transport = self._transport self._servo_client = None self._file_client = None self._manager = None self._transport = None if transport and hasattr(transport, "close"): transport.close() def move(self, items): self._require_servo_client() cmds = [ servo_service_common.ServoCmd(id=servo_id, angle_rad=float(angle_rad)) for servo_id, angle_rad in items ] return self._servo_client.move(cmds) def move_j(self, angles_rad): self._require_servo_client() return self._servo_client.moveJ([float(value) for value in angles_rad]) def set_constraints(self, servo_id, max_velocity_rad, max_acceleration_rad, max_jerk_rad): self._require_servo_client() return self._servo_client.setConstraints( servo_id, float(max_velocity_rad), float(max_acceleration_rad), float(max_jerk_rad), ) def set_position_gain(self, servo_id, position_gain): self._require_servo_client() return self._servo_client.setPositionGain( servo_id, float(position_gain), ) def set_mode(self, mode): self._require_servo_client() return self._servo_client.setMode(int(mode)) def set_update_period_ms(self, ms): self._require_servo_client() return self._servo_client.setUpdatePeriodMs(int(ms)) def file_write_begin(self, path, total_size): self._require_file_client() return self._file_client.fileWriteBegin(path, int(total_size)) def file_write_chunk(self, data): self._require_file_client() return self._file_client.fileWriteChunk(data) def file_write_end(self): self._require_file_client() return self._file_client.fileWriteEnd() def file_write_abort(self): self._require_file_client() return self._file_client.fileWriteAbort() def upload_file(self, local_path, remote_path, chunk_size, progress=None, cancelled=None): self._require_file_client() file_path = Path(local_path) if not file_path.is_file(): raise FileNotFoundError(f"Local file not found: {file_path}") if not remote_path: raise ValueError("Remote path is required.") if chunk_size <= 0: raise ValueError("Chunk size must be > 0.") total_size = file_path.stat().st_size if total_size > 0xFFFFFFFF: raise ValueError("File is too large for uint32 total_size.") if callable(cancelled) and cancelled(): raise InterruptedError("Upload aborted before start.") begin_result = self.file_write_begin(remote_path, total_size) if begin_result != 0: raise RuntimeError(f"fileWriteBegin failed: {begin_result}") bytes_sent = 0 chunk_count = 0 try: with file_path.open("rb") as stream: while True: if callable(cancelled) and cancelled(): raise InterruptedError("Upload aborted by user.") chunk = stream.read(chunk_size) if not chunk: break result = self.file_write_chunk(chunk) if result != 0: raise RuntimeError( f"fileWriteChunk failed at offset {bytes_sent}: {result}" ) bytes_sent += len(chunk) chunk_count += 1 if callable(progress): progress(bytes_sent, total_size) end_result = self.file_write_end() if end_result != 0: raise RuntimeError(f"fileWriteEnd failed: {end_result}") except Exception: try: self.file_write_abort() except Exception: pass raise return { "local_path": str(file_path), "remote_path": remote_path, "total_size": total_size, "bytes_sent": bytes_sent, "chunk_count": chunk_count, } def _require_servo_client(self): if not self._servo_client: raise RuntimeError("Not connected. Configure serial and click Connect first.") def _require_file_client(self): if not self._file_client: raise RuntimeError("Not connected. Configure serial and click Connect first.") class FaceServoControlApp: def __init__(self, root): self.root = root self.root.title("Face Servo Console") self.root.geometry("1380x880") self.root.minsize(1180, 760) self.root.protocol("WM_DELETE_WINDOW", self.on_close) self.rpc = FaceServoRpcClient() self.events = queue.Queue() self.servo_configs, self.servo_config_error = self._load_servo_configs() self.servo_id_options = list(self.servo_configs.keys()) default_servo_id = self._get_preferred_servo_id() self.status_var = tk.StringVar(value="Disconnected") self.port_var = tk.StringVar(value="COM8") self.baud_var = tk.StringVar(value="115200") self.single_id_var = tk.StringVar(value=default_servo_id) self.single_angle_var = tk.DoubleVar(value=0.0) self.single_live_drag_var = tk.BooleanVar(value=True) self.single_live_rate_hz_var = tk.StringVar(value="50") self.rpc_mode_var = tk.StringVar(value="immediate") self.update_period_ms_var = tk.StringVar(value="10") self.file_local_path_var = tk.StringVar(value=str(SERVO_CONFIG_YAML_PATH)) self.file_remote_path_var = tk.StringVar(value=DEFAULT_REMOTE_SERVO_CONFIG_PATH) self.file_chunk_size_var = tk.StringVar(value=str(DEFAULT_FILE_CHUNK_SIZE)) self.file_status_var = tk.StringVar(value="Config upload idle") self.file_progress_var = tk.StringVar(value=f"Ready: {SERVO_CONFIG_YAML_PATH.name}") self.file_progress_percent_var = tk.DoubleVar(value=0.0) self.fill_angle_var = tk.DoubleVar(value=0.0) self.blink_hold_ms_var = tk.StringVar(value="120") self.last_result_var = tk.StringVar(value="Result: idle") self.constraint_servo_id_var = tk.StringVar(value=default_servo_id) self.scurve_max_velocity_var = tk.StringVar(value="3.0") self.scurve_max_acceleration_var = tk.StringVar(value="10.0") self.scurve_max_jerk_var = tk.StringVar(value="50.0") self.scurve_start_position_var = tk.StringVar(value="0.0") self.scurve_end_position_var = tk.StringVar(value="30.0") self.scurve_start_velocity_var = tk.StringVar(value="0.0") self.scurve_end_velocity_var = tk.StringVar(value="0.0") self.scurve_sample_dt_var = tk.StringVar(value="0.01") self.scurve_status_var = tk.StringVar(value="SCurve idle") self.scurve_summary_var = tk.StringVar(value="No SCurve plan yet.") self.scurve_segments_var = tk.StringVar(value="Segments: -") self.follow_servo_id_var = tk.StringVar(value=default_servo_id) self.position_planner_max_velocity_var = tk.StringVar(value="3.0") self.position_planner_max_acceleration_var = tk.StringVar(value="10.0") self.position_planner_max_jerk_var = tk.StringVar(value="50.0") self.position_planner_gain_var = tk.StringVar(value="1.0") self.position_planner_initial_position_var = tk.StringVar(value="0.0") self.position_planner_initial_velocity_var = tk.StringVar(value="0.0") self.position_planner_initial_acceleration_var = tk.StringVar(value="0.0") self.position_planner_target_position_var = tk.StringVar(value="30.0") self.position_planner_sample_dt_var = tk.StringVar(value="0.01") self.position_planner_duration_var = tk.StringVar(value="3.0") self.position_planner_status_var = tk.StringVar(value="Follow planner idle") self.position_planner_summary_var = tk.StringVar(value="No follow plan yet.") self.position_planner_target_var = tk.StringVar(value="Target: -") self.eye_controls = [ { **spec, "id_var": tk.StringVar(value=spec["default_id"]), "angle_var": tk.DoubleVar(value=0.0), } for spec in EYE_CONTROL_SPECS ] self.scurve = None self.position_planner = None self._single_live_job = None self._single_live_pending = False self._file_upload_cancel = threading.Event() self._file_upload_active = False self._blink_active = False self._blink_loop_job = None self._preview_update_job = None self.notebook = None self.single_angle_scale = None self.tab_frames = {} self.scurve_canvases = {} self.scurve_plot_series = { "position": None, "velocity": None, "acceleration": None, "jerk": None, } self.position_planner_canvases = {} self.position_planner_plot_series = { "position": None, "velocity": None, "acceleration": None, "jerk": None, } self.colors = { "app_bg": "#eef4f7", "panel_bg": "#ffffff", "panel_edge": "#d5e2ea", "hero_text": "#163243", "body_text": "#315164", "muted_text": "#6f8796", "input_bg": "#f7fbfd", "input_text": "#173446", "accent": "#32b3a4", "accent_active": "#54c6b8", "accent_text": "#ffffff", "ghost": "#e6f1f5", "ghost_active": "#d7e8ef", "ghost_text": "#21475b", "status": "#22906d", "notebook_tab": "#ddebf1", "notebook_tab_active": "#cfe3eb", "notebook_tab_selected": "#32b3a4", "log_bg": "#f4fafc", "log_text": "#224054", "plot_bg": "#f6fbfd", "plot_grid": "#d7e6ee", "plot_axis": "#a9c0cc", } self._build_style() self._build_layout() self._initialize_servo_config_ui() self._poll_events() def _build_style(self): c = self.colors self.root.configure(bg=c["app_bg"]) style = ttk.Style() style.theme_use("clam") style.configure( ".", background=c["app_bg"], foreground=c["body_text"], fieldbackground=c["input_bg"], bordercolor=c["panel_edge"], lightcolor=c["panel_edge"], darkcolor=c["panel_edge"], ) style.configure("TFrame", background=c["app_bg"]) style.configure("Panel.TFrame", background=c["panel_bg"], relief="flat") style.configure( "TLabel", background=c["app_bg"], foreground=c["body_text"], font=("Segoe UI", 10), ) style.configure( "Header.TLabel", background=c["app_bg"], foreground=c["hero_text"], font=("Bahnschrift SemiBold", 22), ) style.configure( "Subhead.TLabel", background=c["panel_bg"], foreground=c["hero_text"], font=("Bahnschrift SemiBold", 12), ) style.configure( "Muted.TLabel", background=c["panel_bg"], foreground=c["muted_text"], font=("Segoe UI", 9), ) style.configure( "TEntry", padding=7, foreground=c["input_text"], fieldbackground=c["input_bg"], insertcolor=c["input_text"], ) style.configure( "Accent.TButton", background=c["accent"], foreground=c["accent_text"], padding=(14, 8), borderwidth=0, font=("Bahnschrift SemiBold", 10), ) style.map( "Accent.TButton", background=[("active", c["accent_active"]), ("disabled", "#b5c6cf")], foreground=[("disabled", "#f6fbfd")], ) style.configure( "Ghost.TButton", background=c["ghost"], foreground=c["ghost_text"], padding=(14, 8), borderwidth=0, font=("Segoe UI Semibold", 10), ) style.map("Ghost.TButton", background=[("active", c["ghost_active"])]) style.configure( "Status.TLabel", background=c["panel_bg"], foreground=c["status"], font=("Segoe UI Semibold", 10), ) style.configure( "File.Horizontal.TProgressbar", background=c["accent"], troughcolor=c["ghost"], bordercolor=c["panel_edge"], lightcolor=c["accent"], darkcolor=c["accent"], thickness=12, ) style.configure("TNotebook", background=c["app_bg"], borderwidth=0, tabmargins=(0, 0, 0, 0)) style.configure( "TNotebook.Tab", background=c["notebook_tab"], foreground=c["ghost_text"], padding=(16, 8), font=("Segoe UI Semibold", 10), ) style.map( "TNotebook.Tab", background=[("selected", c["notebook_tab_selected"]), ("active", c["notebook_tab_active"])], foreground=[("selected", c["accent_text"])], ) def _build_layout(self): shell = ttk.Frame(self.root, padding=18) shell.pack(fill="both", expand=True) shell.columnconfigure(0, weight=1) shell.rowconfigure(1, weight=1) hero = ttk.Frame(shell) hero.grid(row=0, column=0, sticky="ew", pady=(0, 14)) hero.columnconfigure(0, weight=1) ttk.Label(hero, text="Face Servo Console", style="Header.TLabel").grid( row=0, column=0, sticky="w" ) ttk.Label( hero, text="eRPC serial controller for servo_service and file_service", style="TLabel", ).grid(row=1, column=0, sticky="w", pady=(4, 0)) main = ttk.Frame(shell) main.grid(row=1, column=0, sticky="nsew") main.columnconfigure(0, weight=0) main.columnconfigure(1, weight=1) main.rowconfigure(0, weight=1) left = ttk.Frame(main, style="Panel.TFrame", padding=16) left.grid(row=0, column=0, sticky="nsw", padx=(0, 14)) right = ttk.Frame(main, style="Panel.TFrame", padding=16) right.grid(row=0, column=1, sticky="nsew") right.columnconfigure(0, weight=1) right.rowconfigure(0, weight=1) self._build_connection_panel(left) self._build_motion_panel(left) self._build_single_panel(left) self._build_file_panel(left) self._build_log_panel(left) notebook = ttk.Notebook(right) notebook.grid(row=0, column=0, sticky="nsew") self.notebook = notebook servo_tab = ttk.Frame(notebook, style="Panel.TFrame", padding=16) servo_tab.columnconfigure(0, weight=1) servo_tab.rowconfigure(1, weight=1) notebook.add(servo_tab, text="Servo Matrix") scurve_tab = ttk.Frame(notebook, style="Panel.TFrame", padding=16) scurve_tab.columnconfigure(0, weight=1) scurve_tab.rowconfigure(1, weight=1) notebook.add(scurve_tab, text="SCurve Planner") position_planner_tab = ttk.Frame(notebook, style="Panel.TFrame", padding=16) position_planner_tab.columnconfigure(0, weight=1) position_planner_tab.rowconfigure(1, weight=1) notebook.add(position_planner_tab, text="Follow Planner") self.tab_frames = { "servo": servo_tab, "scurve": scurve_tab, "follow": position_planner_tab, } self._build_all_angles_panel(servo_tab) self._build_scurve_panel(scurve_tab) self._build_position_planner_panel(position_planner_tab) def _build_connection_panel(self, parent): panel = ttk.Frame(parent, style="Panel.TFrame") panel.pack(fill="x") panel.columnconfigure(1, weight=1) ttk.Label(panel, text="Connection", style="Subhead.TLabel").grid( row=0, column=0, columnspan=2, sticky="w" ) ttk.Label(panel, text="Serial link to the board", style="Muted.TLabel").grid( row=1, column=0, columnspan=2, sticky="w", pady=(0, 14) ) ttk.Label(panel, text="Port").grid(row=2, column=0, sticky="w", pady=4) ttk.Entry(panel, textvariable=self.port_var, width=18).grid( row=2, column=1, sticky="ew", pady=4 ) ttk.Label(panel, text="Baud").grid(row=3, column=0, sticky="w", pady=4) ttk.Entry(panel, textvariable=self.baud_var, width=18).grid( row=3, column=1, sticky="ew", pady=4 ) button_row = ttk.Frame(panel, style="Panel.TFrame") button_row.grid(row=4, column=0, columnspan=2, sticky="ew", pady=(10, 8)) button_row.columnconfigure(0, weight=1) button_row.columnconfigure(1, weight=1) ttk.Button( button_row, text="Connect", style="Accent.TButton", command=self.connect_clicked, ).grid(row=0, column=0, sticky="ew", padx=(0, 6)) ttk.Button( button_row, text="Disconnect", style="Ghost.TButton", command=self.disconnect_clicked, ).grid(row=0, column=1, sticky="ew", padx=(6, 0)) ttk.Label(panel, textvariable=self.status_var, style="Status.TLabel").grid( row=5, column=0, columnspan=2, sticky="w" ) ttk.Label(panel, textvariable=self.last_result_var, style="Muted.TLabel").grid( row=6, column=0, columnspan=2, sticky="w", pady=(4, 0) ) def _build_motion_panel(self, parent): panel = ttk.Frame(parent, style="Panel.TFrame") panel.pack(fill="x", pady=(22, 0)) panel.columnconfigure(1, weight=1) ttk.Label(panel, text="Motion Settings", style="Subhead.TLabel").grid( row=0, column=0, columnspan=2, sticky="w" ) ttk.Label(panel, text="Global setMode() and setUpdatePeriodMs()", style="Muted.TLabel").grid( row=1, column=0, columnspan=2, sticky="w", pady=(0, 14) ) ttk.Label(panel, text="Mode").grid(row=2, column=0, sticky="w", pady=4) mode_row = ttk.Frame(panel, style="Panel.TFrame") mode_row.grid(row=2, column=1, sticky="w", pady=4) ttk.Radiobutton( mode_row, text="Immediate", value="immediate", variable=self.rpc_mode_var, command=self._on_motion_mode_changed, ).grid(row=0, column=0, sticky="w") ttk.Radiobutton( mode_row, text="SCurve", value="scurve", variable=self.rpc_mode_var, command=self._on_motion_mode_changed, ).grid(row=0, column=1, sticky="w", padx=(12, 0)) ttk.Radiobutton( mode_row, text="Follow", value="follow", variable=self.rpc_mode_var, command=self._on_motion_mode_changed, ).grid(row=0, column=2, sticky="w", padx=(12, 0)) ttk.Label(panel, text="Period ms").grid(row=3, column=0, sticky="w", pady=4) ttk.Entry(panel, textvariable=self.update_period_ms_var).grid( row=3, column=1, sticky="ew", pady=4 ) button_row = ttk.Frame(panel, style="Panel.TFrame") button_row.grid(row=4, column=0, columnspan=2, sticky="ew", pady=(12, 0)) button_row.columnconfigure(0, weight=1) button_row.columnconfigure(1, weight=1) ttk.Button( button_row, text="Apply Settings", style="Accent.TButton", command=self.send_motion_settings_clicked, ).grid(row=0, column=0, sticky="ew", padx=(0, 6)) ttk.Button( button_row, text="Send Mode Only", style="Ghost.TButton", command=self.send_mode_clicked, ).grid(row=0, column=1, sticky="ew", padx=(6, 0)) def _build_single_panel(self, parent): panel = ttk.Frame(parent, style="Panel.TFrame") panel.pack(fill="x", pady=(22, 0)) panel.columnconfigure(1, weight=1) ttk.Label(panel, text="Single Servo", style="Subhead.TLabel").grid( row=0, column=0, columnspan=2, sticky="w" ) ttk.Label(panel, text="Call move([ServoCmd(id, angle_rad)])", style="Muted.TLabel").grid( row=1, column=0, columnspan=2, sticky="w", pady=(0, 14) ) ttk.Label(panel, text="Servo ID").grid(row=2, column=0, sticky="w", pady=4) single_id_entry = ttk.Combobox( panel, textvariable=self.single_id_var, values=self.servo_id_options, state="normal", ) single_id_entry.grid(row=2, column=1, sticky="ew", pady=4) single_id_entry.bind("<>", self._on_single_servo_id_changed) single_id_entry.bind("", self._on_single_servo_id_changed) single_id_entry.bind("", self._on_single_servo_id_changed) ttk.Label(panel, text="Angle").grid(row=3, column=0, sticky="w", pady=4) angle_row = ttk.Frame(panel, style="Panel.TFrame") angle_row.grid(row=3, column=1, sticky="ew", pady=4) angle_row.columnconfigure(0, weight=1) self.single_angle_scale = ttk.Scale( angle_row, variable=self.single_angle_var, from_=-math.pi, to=math.pi, orient="horizontal", command=self._on_single_scale_changed, ) self.single_angle_scale.grid(row=0, column=0, sticky="ew", padx=(0, 10)) single_angle_entry = ttk.Entry(angle_row, textvariable=self.single_angle_var, width=8) single_angle_entry.grid(row=0, column=1, sticky="e") single_angle_entry.bind("", self._send_single_from_event) self.single_angle_scale.bind("", self._on_single_scale_released) command_row = ttk.Frame(panel, style="Panel.TFrame") command_row.grid(row=4, column=0, columnspan=2, sticky="ew", pady=(10, 0)) command_row.columnconfigure(0, weight=1) command_row.columnconfigure(1, weight=0) command_row.columnconfigure(2, weight=0) ttk.Checkbutton( command_row, text="Live drag send", variable=self.single_live_drag_var, command=self._on_single_live_drag_toggled, ).grid(row=0, column=0, sticky="e") ttk.Entry(command_row, textvariable=self.single_live_rate_hz_var, width=6).grid( row=0, column=1, sticky="e", padx=(10, 6) ) ttk.Label(command_row, text="Hz", style="Muted.TLabel").grid(row=0, column=2, sticky="e") ttk.Label(panel, text="Angle unit: rad", style="Muted.TLabel").grid( row=5, column=0, columnspan=2, sticky="w", pady=(8, 0) ) single_button_row = ttk.Frame(panel, style="Panel.TFrame") single_button_row.grid(row=6, column=0, columnspan=2, sticky="ew", pady=(12, 0)) single_button_row.columnconfigure(0, weight=1) ttk.Button( single_button_row, text="Send move", style="Accent.TButton", command=self.send_single_clicked, ).grid(row=0, column=0, sticky="ew") def _build_log_panel(self, parent): panel = ttk.Frame(parent, style="Panel.TFrame") panel.pack(fill="both", expand=True, pady=(22, 0)) panel.columnconfigure(0, weight=1) panel.rowconfigure(1, weight=1) ttk.Label(panel, text="Activity", style="Subhead.TLabel").grid( row=0, column=0, sticky="w" ) self.log_widget = tk.Text( panel, height=12, bg=self.colors["log_bg"], fg=self.colors["log_text"], insertbackground=self.colors["log_text"], relief="flat", bd=0, wrap="word", font=("Consolas", 10), ) self.log_widget.grid(row=1, column=0, sticky="nsew", pady=(10, 0)) self.log_widget.insert("end", "Console ready.\n") self.log_widget.configure(state="disabled") def _build_file_panel(self, parent): panel = ttk.Frame(parent, style="Panel.TFrame") panel.pack(fill="x", pady=(22, 0)) panel.columnconfigure(1, weight=1) ttk.Label(panel, text="Servo Config Upload", style="Subhead.TLabel").grid( row=0, column=0, columnspan=2, sticky="w" ) ttk.Label( panel, text="Convert the local YAML config to JSON, then upload it with file_service.", style="Muted.TLabel", ).grid(row=1, column=0, columnspan=2, sticky="w", pady=(0, 14)) ttk.Label(panel, text="Local YAML").grid(row=2, column=0, sticky="w", pady=4) local_row = ttk.Frame(panel, style="Panel.TFrame") local_row.grid(row=2, column=1, sticky="ew", pady=4) local_row.columnconfigure(0, weight=1) local_entry = ttk.Entry(local_row, textvariable=self.file_local_path_var) local_entry.grid(row=0, column=0, sticky="ew", padx=(0, 8)) local_entry.bind("", self._send_file_from_event) ttk.Button( local_row, text="Browse", style="Ghost.TButton", command=self.browse_file_clicked, ).grid(row=0, column=1, sticky="e") ttk.Label(panel, text="Remote path").grid(row=3, column=0, sticky="w", pady=4) remote_entry = ttk.Entry(panel, textvariable=self.file_remote_path_var) remote_entry.grid(row=3, column=1, sticky="ew", pady=4) remote_entry.bind("", self._send_file_from_event) ttk.Label(panel, text="Chunk bytes").grid(row=4, column=0, sticky="w", pady=4) chunk_entry = ttk.Entry(panel, textvariable=self.file_chunk_size_var) chunk_entry.grid(row=4, column=1, sticky="ew", pady=4) chunk_entry.bind("", self._send_file_from_event) button_row = ttk.Frame(panel, style="Panel.TFrame") button_row.grid(row=5, column=0, columnspan=2, sticky="ew", pady=(12, 0)) button_row.columnconfigure(0, weight=1) button_row.columnconfigure(1, weight=1) ttk.Button( button_row, text="Upload Config", style="Accent.TButton", command=self.send_file_clicked, ).grid(row=0, column=0, sticky="ew", padx=(0, 6)) ttk.Button( button_row, text="Abort Upload", style="Ghost.TButton", command=self.abort_file_clicked, ).grid(row=0, column=1, sticky="ew", padx=(6, 0)) ttk.Label(panel, textvariable=self.file_status_var, style="Status.TLabel").grid( row=6, column=0, columnspan=2, sticky="w", pady=(8, 0) ) ttk.Progressbar( panel, style="File.Horizontal.TProgressbar", mode="determinate", maximum=100.0, variable=self.file_progress_percent_var, ).grid(row=7, column=0, columnspan=2, sticky="ew", pady=(8, 0)) ttk.Label(panel, textvariable=self.file_progress_var, style="Muted.TLabel").grid( row=8, column=0, columnspan=2, sticky="w", pady=(4, 0) ) def _build_all_angles_panel(self, parent): header = ttk.Frame(parent, style="Panel.TFrame") header.grid(row=0, column=0, sticky="ew") header.columnconfigure(0, weight=1) ttk.Label(header, text="Eye Servo Layout", style="Subhead.TLabel").grid( row=0, column=0, sticky="w" ) ttk.Label( header, text=( f"{len(EYE_CONTROL_SPECS)} eye controls with configurable string servo IDs; " f"send uses move([ServoCmd(id, angle_rad)]). IDs must match firmware names exactly." ), style="Muted.TLabel", ).grid(row=1, column=0, sticky="w", pady=(0, 14)) controls = ttk.Frame(header, style="Panel.TFrame") controls.grid(row=0, column=1, rowspan=2, sticky="e") controls.columnconfigure(0, weight=1) ttk.Entry(controls, textvariable=self.fill_angle_var, width=10).grid( row=0, column=0, sticky="e", padx=(0, 8) ) ttk.Button( controls, text="Fill Visible", style="Ghost.TButton", command=self.fill_all_clicked, ).grid(row=0, column=1, sticky="e", padx=(0, 8)) ttk.Button( controls, text="Zero Visible", style="Ghost.TButton", command=self.zero_all_clicked, ).grid(row=0, column=2, sticky="e", padx=(0, 8)) ttk.Button( controls, text="Send move", style="Accent.TButton", command=self.send_all_clicked, ).grid(row=0, column=3, sticky="e") ttk.Label(controls, text="Blink wait (ms)", style="Muted.TLabel").grid( row=1, column=0, sticky="e", padx=(0, 8), pady=(10, 0) ) blink_hold_entry = ttk.Entry(controls, textvariable=self.blink_hold_ms_var, width=10) blink_hold_entry.grid(row=1, column=1, sticky="e", padx=(0, 8), pady=(10, 0)) blink_hold_entry.bind("", self._blink_from_event) ttk.Button( controls, text="Start Blink", style="Ghost.TButton", command=self.blink_clicked, ).grid(row=1, column=2, sticky="e", padx=(0, 8), pady=(10, 0)) ttk.Button( controls, text="Stop Blink", style="Ghost.TButton", command=self.stop_blink_clicked, ).grid(row=1, column=3, sticky="e", pady=(10, 0)) body = ttk.Frame(parent, style="Panel.TFrame") body.grid(row=1, column=0, sticky="nsew", pady=(8, 0)) body.columnconfigure(0, weight=1) body.columnconfigure(1, weight=0) body.columnconfigure(2, weight=1) body.rowconfigure(1, weight=1) ( left_upper, left_lower, right_upper, right_lower, gaze_horizontal, gaze_vertical, ) = self.eye_controls self._build_eye_slider_card(body, left_upper).grid( row=0, column=0, sticky="ew", padx=(0, 10), pady=(0, 10) ) self._build_vertical_gaze_card(body, gaze_vertical).grid( row=0, column=1, rowspan=3, sticky="ns", padx=8, pady=(0, 10) ) self._build_eye_slider_card(body, right_upper).grid( row=0, column=2, sticky="ew", padx=(10, 0), pady=(0, 10) ) self._build_eye_preview_card(body, "Left Eye").grid( row=1, column=0, sticky="n", padx=(0, 10), pady=(0, 10) ) self._build_eye_preview_card(body, "Right Eye").grid( row=1, column=2, sticky="n", padx=(10, 0), pady=(0, 10) ) self._build_eye_slider_card(body, left_lower).grid( row=2, column=0, sticky="ew", padx=(0, 10), pady=(0, 10) ) self._build_eye_slider_card(body, right_lower).grid( row=2, column=2, sticky="ew", padx=(10, 0), pady=(0, 10) ) self._build_horizontal_gaze_card(body, gaze_horizontal).grid( row=3, column=0, columnspan=3, sticky="ew", pady=(6, 0) ) def _build_eye_slider_card(self, parent, control): card = ttk.Frame(parent, style="Panel.TFrame", padding=12) card.columnconfigure(0, weight=1) meta = ttk.Frame(card, style="Panel.TFrame") meta.grid(row=0, column=0, sticky="ew", pady=(0, 8)) meta.columnconfigure(0, weight=1) ttk.Label(meta, text=control["title"], style="Subhead.TLabel").grid( row=0, column=0, sticky="w" ) ttk.Label(meta, text="ID", style="Muted.TLabel").grid( row=0, column=1, sticky="e", padx=(12, 6) ) id_combo = ttk.Combobox( meta, textvariable=control["id_var"], values=self.servo_id_options, state="normal", width=16, ) id_combo.grid(row=0, column=2, sticky="e") id_combo.bind("<>", lambda event, item=control: self._on_eye_control_id_changed(item, event)) id_combo.bind("", lambda event, item=control: self._on_eye_control_id_changed(item, event)) id_combo.bind("", lambda event, item=control: self._on_eye_control_id_changed(item, event)) ttk.Label(card, text="Servo angle (rad)", style="Muted.TLabel").grid( row=1, column=0, sticky="w", pady=(0, 8) ) control["scale"] = ttk.Scale( card, variable=control["angle_var"], from_=-math.pi, to=math.pi, orient="horizontal", ) control["scale"].grid(row=2, column=0, sticky="ew") ttk.Entry(card, textvariable=control["angle_var"], width=10).grid( row=3, column=0, sticky="ew", pady=(8, 0) ) return card def _build_vertical_gaze_card(self, parent, control): card = ttk.Frame(parent, style="Panel.TFrame", padding=12) card.columnconfigure(0, weight=1) meta = ttk.Frame(card, style="Panel.TFrame") meta.grid(row=0, column=0, sticky="ew") meta.columnconfigure(0, weight=1) ttk.Label(meta, text=control["title"], style="Subhead.TLabel").grid( row=0, column=0, sticky="w" ) ttk.Label(meta, text="ID", style="Muted.TLabel").grid( row=0, column=1, sticky="e", padx=(12, 6) ) id_combo = ttk.Combobox( meta, textvariable=control["id_var"], values=self.servo_id_options, state="normal", width=16, ) id_combo.grid(row=0, column=2, sticky="e") id_combo.bind("<>", lambda event, item=control: self._on_eye_control_id_changed(item, event)) id_combo.bind("", lambda event, item=control: self._on_eye_control_id_changed(item, event)) id_combo.bind("", lambda event, item=control: self._on_eye_control_id_changed(item, event)) ttk.Label(card, text="Both eyes", style="Muted.TLabel").grid( row=1, column=0, sticky="w", pady=(2, 10) ) ttk.Label(card, text="Up", style="Muted.TLabel").grid( row=2, column=0, sticky="n", pady=(0, 6) ) control["scale"] = ttk.Scale( card, variable=control["angle_var"], from_=math.pi, to=-math.pi, orient="vertical", length=220, ) control["scale"].grid(row=3, column=0, sticky="ns") ttk.Label(card, text="Down", style="Muted.TLabel").grid( row=4, column=0, sticky="s", pady=(6, 0) ) ttk.Entry(card, textvariable=control["angle_var"], width=10).grid( row=5, column=0, sticky="ew", pady=(10, 0) ) return card def _build_horizontal_gaze_card(self, parent, control): card = ttk.Frame(parent, style="Panel.TFrame", padding=12) card.columnconfigure(1, weight=1) meta = ttk.Frame(card, style="Panel.TFrame") meta.grid(row=0, column=0, columnspan=3, sticky="ew") meta.columnconfigure(0, weight=1) ttk.Label(meta, text=control["title"], style="Subhead.TLabel").grid( row=0, column=0, sticky="w" ) ttk.Label(meta, text="ID", style="Muted.TLabel").grid( row=0, column=1, sticky="e", padx=(12, 6) ) id_combo = ttk.Combobox( meta, textvariable=control["id_var"], values=self.servo_id_options, state="normal", width=16, ) id_combo.grid(row=0, column=2, sticky="e") id_combo.bind("<>", lambda event, item=control: self._on_eye_control_id_changed(item, event)) id_combo.bind("", lambda event, item=control: self._on_eye_control_id_changed(item, event)) id_combo.bind("", lambda event, item=control: self._on_eye_control_id_changed(item, event)) ttk.Label(card, text="Both eyes", style="Muted.TLabel").grid( row=1, column=0, columnspan=3, sticky="w", pady=(2, 8) ) ttk.Label(card, text="Left", style="Muted.TLabel").grid( row=2, column=0, sticky="w", padx=(0, 8) ) control["scale"] = ttk.Scale( card, variable=control["angle_var"], from_=-math.pi, to=math.pi, orient="horizontal", ) control["scale"].grid(row=2, column=1, sticky="ew") ttk.Label(card, text="Right", style="Muted.TLabel").grid( row=2, column=2, sticky="e", padx=(8, 0) ) ttk.Entry(card, textvariable=control["angle_var"], width=10).grid( row=3, column=0, columnspan=3, sticky="e", pady=(10, 0) ) return card def _build_eye_preview_card(self, parent, title): card = ttk.Frame(parent, style="Panel.TFrame", padding=12) card.columnconfigure(0, weight=1) ttk.Label(card, text=title, style="Subhead.TLabel").grid( row=0, column=0 ) canvas = tk.Canvas( card, width=260, height=170, bg=self.colors["input_bg"], highlightthickness=1, highlightbackground=self.colors["panel_edge"], bd=0, ) canvas.grid(row=1, column=0, pady=(10, 0)) canvas.create_oval(24, 28, 236, 144, fill="#f3f8fb", outline="#aebfca", width=2) canvas.create_arc(24, 18, 236, 128, start=0, extent=180, style="arc", outline="#2b3440", width=12) canvas.create_arc(24, 44, 236, 154, start=180, extent=180, style="arc", outline="#2b3440", width=12) canvas.create_oval(78, 46, 182, 150, fill="#4169a8", outline="#1a2a44", width=2) canvas.create_oval(104, 72, 156, 124, fill="#2e4f86", outline="#16253c", width=1) canvas.create_oval(119, 87, 141, 109, fill="#dfe8f1", outline="#16253c", width=1) return card def _build_scurve_panel(self, parent): header = ttk.Frame(parent, style="Panel.TFrame") header.grid(row=0, column=0, sticky="ew") header.columnconfigure(0, weight=1) ttk.Label(header, text="SCurve Planner", style="Subhead.TLabel").grid( row=0, column=0, sticky="w" ) ttk.Label( header, text="Configure local SCurve planning and device setConstraints(id, v, a, j).", style="Muted.TLabel", ).grid(row=1, column=0, sticky="w", pady=(0, 14)) controls = ttk.Frame(header, style="Panel.TFrame") controls.grid(row=2, column=0, sticky="ew") for col in range(4): controls.columnconfigure(col, weight=1) entries = [ ("Servo ID", self.constraint_servo_id_var), ("Max velocity", self.scurve_max_velocity_var), ("Max accel", self.scurve_max_acceleration_var), ("Max jerk", self.scurve_max_jerk_var), ("Sample dt", self.scurve_sample_dt_var), ("Start pos", self.scurve_start_position_var), ("End pos", self.scurve_end_position_var), ("Start vel", self.scurve_start_velocity_var), ("End vel", self.scurve_end_velocity_var), ] for index, (label_text, variable) in enumerate(entries): row = (index // 4) * 2 col = index % 4 ttk.Label(controls, text=label_text).grid(row=row, column=col, sticky="w", padx=6, pady=(0, 4)) if label_text == "Servo ID": entry = ttk.Combobox( controls, textvariable=variable, values=self.servo_id_options, state="normal", ) entry.bind("<>", self._on_constraint_servo_id_changed) entry.bind("", self._on_constraint_servo_id_changed) entry.bind("", self._on_constraint_servo_id_changed) else: entry = ttk.Entry(controls, textvariable=variable) entry.bind("", self._plan_scurve_from_event) entry.grid(row=row + 1, column=col, sticky="ew", padx=6, pady=(0, 10)) button_row = ttk.Frame(header, style="Panel.TFrame") button_row.grid(row=3, column=0, sticky="ew", pady=(4, 0)) button_row.columnconfigure(0, weight=0) button_row.columnconfigure(1, weight=0) button_row.columnconfigure(2, weight=1) ttk.Button( button_row, text="Plan Curve", style="Accent.TButton", command=self.plan_scurve_clicked, ).grid(row=0, column=0, sticky="w") ttk.Button( button_row, text="Send Constraints", style="Ghost.TButton", command=self.send_scurve_constraints_clicked, ).grid(row=0, column=1, sticky="w", padx=(10, 0)) ttk.Label(button_row, textvariable=self.scurve_status_var, style="Status.TLabel").grid( row=0, column=2, sticky="w", padx=(14, 0) ) ttk.Label(header, textvariable=self.scurve_summary_var, style="Muted.TLabel").grid( row=4, column=0, sticky="w", pady=(10, 0) ) ttk.Label(header, textvariable=self.scurve_segments_var, style="Muted.TLabel").grid( row=5, column=0, sticky="w", pady=(4, 0) ) plot_area = ttk.Frame(parent, style="Panel.TFrame") plot_area.grid(row=1, column=0, sticky="nsew", pady=(16, 0)) plot_area.columnconfigure(0, weight=1) plot_area.rowconfigure(0, weight=1) plot_area.rowconfigure(1, weight=1) plot_area.rowconfigure(2, weight=1) plot_area.rowconfigure(3, weight=1) plot_specs = [ ("position", "Position", "#7ce0a0"), ("velocity", "Velocity", "#65c7ff"), ("acceleration", "Acceleration", "#ffd166"), ("jerk", "Jerk", "#ff8c7e"), ] for row_index, (key, title, color) in enumerate(plot_specs): card = ttk.Frame(plot_area, style="Panel.TFrame", padding=10) card.grid(row=row_index, column=0, sticky="nsew", pady=(0, 10) if row_index < len(plot_specs) - 1 else 0) card.columnconfigure(0, weight=1) card.rowconfigure(1, weight=1) ttk.Label(card, text=title, style="Subhead.TLabel").grid(row=0, column=0, sticky="w") ttk.Label(card, text="Time on X axis", style="Muted.TLabel").grid( row=0, column=1, sticky="e" ) canvas = tk.Canvas( card, height=180, bg=self.colors["plot_bg"], highlightthickness=0, bd=0, ) canvas.grid(row=1, column=0, columnspan=2, sticky="nsew", pady=(10, 0)) canvas.bind("", lambda _event, metric=key: self._redraw_curve_plot(metric)) self.scurve_canvases[key] = canvas self.scurve_plot_series[key] = { "title": title, "color": color, "times": [], "values": [], } def _build_position_planner_panel(self, parent): header = ttk.Frame(parent, style="Panel.TFrame") header.grid(row=0, column=0, sticky="ew") header.columnconfigure(0, weight=1) ttk.Label(header, text="Follow Planner", style="Subhead.TLabel").grid( row=0, column=0, sticky="w" ) ttk.Label( header, text="Preview SCurvePositionPlanner1D follow behavior and optionally send follow constraints and gain.", style="Muted.TLabel", ).grid(row=1, column=0, sticky="w", pady=(0, 14)) controls = ttk.Frame(header, style="Panel.TFrame") controls.grid(row=2, column=0, sticky="ew") for col in range(5): controls.columnconfigure(col, weight=1) entries = [ ("Servo ID", self.follow_servo_id_var), ("Max velocity", self.position_planner_max_velocity_var), ("Max accel", self.position_planner_max_acceleration_var), ("Max jerk", self.position_planner_max_jerk_var), ("Pos gain", self.position_planner_gain_var), ("Initial pos", self.position_planner_initial_position_var), ("Initial vel", self.position_planner_initial_velocity_var), ("Initial accel", self.position_planner_initial_acceleration_var), ("Target pos", self.position_planner_target_position_var), ("Sample dt", self.position_planner_sample_dt_var), ("Duration", self.position_planner_duration_var), ] for index, (label_text, variable) in enumerate(entries): row = (index // 5) * 2 col = index % 5 ttk.Label(controls, text=label_text).grid(row=row, column=col, sticky="w", padx=6, pady=(0, 4)) if label_text == "Servo ID": entry = ttk.Combobox( controls, textvariable=variable, values=self.servo_id_options, state="normal", ) entry.bind("<>", self._on_follow_servo_id_changed) entry.bind("", self._on_follow_servo_id_changed) entry.bind("", self._on_follow_servo_id_changed) else: entry = ttk.Entry(controls, textvariable=variable) entry.bind("", self._plan_position_planner_from_event) entry.grid(row=row + 1, column=col, sticky="ew", padx=6, pady=(0, 10)) button_row = ttk.Frame(header, style="Panel.TFrame") button_row.grid(row=3, column=0, sticky="ew", pady=(4, 0)) button_row.columnconfigure(0, weight=0) button_row.columnconfigure(1, weight=0) button_row.columnconfigure(2, weight=0) button_row.columnconfigure(3, weight=1) ttk.Button( button_row, text="Plan Follow Curve", style="Accent.TButton", command=self.plan_position_planner_clicked, ).grid(row=0, column=0, sticky="w") ttk.Button( button_row, text="Send Constraints", style="Ghost.TButton", command=self.send_follow_constraints_clicked, ).grid(row=0, column=1, sticky="w", padx=(10, 0)) ttk.Button( button_row, text="Send Gain", style="Ghost.TButton", command=self.send_follow_position_gain_clicked, ).grid(row=0, column=2, sticky="w", padx=(10, 0)) ttk.Label(button_row, textvariable=self.position_planner_status_var, style="Status.TLabel").grid( row=0, column=3, sticky="w", padx=(14, 0) ) ttk.Label(header, textvariable=self.position_planner_summary_var, style="Muted.TLabel").grid( row=4, column=0, sticky="w", pady=(10, 0) ) ttk.Label(header, textvariable=self.position_planner_target_var, style="Muted.TLabel").grid( row=5, column=0, sticky="w", pady=(4, 0) ) plot_area = ttk.Frame(parent, style="Panel.TFrame") plot_area.grid(row=1, column=0, sticky="nsew", pady=(16, 0)) plot_area.columnconfigure(0, weight=1) plot_area.rowconfigure(0, weight=1) plot_area.rowconfigure(1, weight=1) plot_area.rowconfigure(2, weight=1) plot_area.rowconfigure(3, weight=1) plot_specs = [ ("position", "Position", "#6abf69"), ("velocity", "Velocity", "#4da3ff"), ("acceleration", "Acceleration", "#f2b43c"), ("jerk", "Jerk", "#ef6c73"), ] for row_index, (key, title, color) in enumerate(plot_specs): card = ttk.Frame(plot_area, style="Panel.TFrame", padding=10) card.grid(row=row_index, column=0, sticky="nsew", pady=(0, 10) if row_index < len(plot_specs) - 1 else 0) card.columnconfigure(0, weight=1) card.rowconfigure(1, weight=1) ttk.Label(card, text=title, style="Subhead.TLabel").grid(row=0, column=0, sticky="w") ttk.Label(card, text="Time on X axis", style="Muted.TLabel").grid( row=0, column=1, sticky="e" ) canvas = tk.Canvas( card, height=180, bg=self.colors["plot_bg"], highlightthickness=0, bd=0, ) canvas.grid(row=1, column=0, columnspan=2, sticky="nsew", pady=(10, 0)) canvas.bind("", lambda _event, metric=key: self._redraw_position_planner_plot(metric)) self.position_planner_canvases[key] = canvas self.position_planner_plot_series[key] = { "title": title, "color": color, "times": [], "values": [], } def _load_servo_configs(self): try: raw = json.loads(SERVO_CONFIG_PATH.read_text(encoding="utf-8")) except Exception as exc: return {}, str(exc) configs = {} for item in raw.get("servos", []): servo_id = str(item.get("id", "")).strip() if servo_id: configs[servo_id] = item return configs, None def _get_preferred_servo_id(self): if "eye_l_up" in self.servo_configs: return "eye_l_up" if self.servo_id_options: return self.servo_id_options[0] return "" def _initialize_servo_config_ui(self): self._apply_single_servo_config(use_home=True) self._sync_planner_inputs_from_single() self._apply_constraint_servo_config() self._apply_follow_servo_config() for control in self.eye_controls: self._apply_eye_control_config(control, use_home=self._should_use_home_for_eye_control(control)) if self.servo_config_error: self._append_log(f"Servo config load failed: {self.servo_config_error}") return self._append_log( f"Loaded {len(self.servo_configs)} servo configs from {SERVO_CONFIG_PATH}" ) def _get_servo_config(self, servo_id): if not servo_id: return None return self.servo_configs.get(servo_id.strip()) def _get_servo_limits(self, servo_config): if not servo_config: return -math.pi, math.pi min_angle = float(servo_config.get("limit_min_angle_rad", -math.pi)) max_angle = float(servo_config.get("limit_max_angle_rad", math.pi)) if min_angle > max_angle: min_angle, max_angle = max_angle, min_angle return min_angle, max_angle def _apply_scale_limits(self, scale, variable, min_angle, max_angle, *, vertical=False, home_angle=None, use_home=False): if scale is not None: if vertical: scale.configure(from_=max_angle, to=min_angle) else: scale.configure(from_=min_angle, to=max_angle) value = float(variable.get()) if use_home and home_angle is not None: value = float(home_angle) value = max(min_angle, min(max_angle, value)) variable.set(value) def _apply_single_servo_config(self, use_home=False): servo_config = self._get_servo_config(self.single_id_var.get().strip()) min_angle, max_angle = self._get_servo_limits(servo_config) home_angle = servo_config.get("home_angle_rad") if servo_config else None self._apply_scale_limits( self.single_angle_scale, self.single_angle_var, min_angle, max_angle, home_angle=home_angle, use_home=use_home, ) def _apply_constraint_servo_config(self): servo_config = self._get_servo_config(self.constraint_servo_id_var.get().strip()) control = servo_config.get("control", {}) if servo_config else {} if not control: return self.scurve_max_velocity_var.set(str(control.get("max_velocity_rad", self.scurve_max_velocity_var.get()))) self.scurve_max_acceleration_var.set(str(control.get("max_acceleration_rad", self.scurve_max_acceleration_var.get()))) self.scurve_max_jerk_var.set(str(control.get("max_jerk_rad", self.scurve_max_jerk_var.get()))) def _apply_follow_servo_config(self): servo_config = self._get_servo_config(self.follow_servo_id_var.get().strip()) control = servo_config.get("control", {}) if servo_config else {} if not control: return self.position_planner_max_velocity_var.set( str(control.get("max_velocity_rad", self.position_planner_max_velocity_var.get())) ) self.position_planner_max_acceleration_var.set( str(control.get("max_acceleration_rad", self.position_planner_max_acceleration_var.get())) ) self.position_planner_max_jerk_var.set( str(control.get("max_jerk_rad", self.position_planner_max_jerk_var.get())) ) self.position_planner_gain_var.set( str(control.get("position_gain", self.position_planner_gain_var.get())) ) def _apply_eye_control_config(self, control, use_home=False): servo_config = self._get_servo_config(control["id_var"].get().strip()) min_angle, max_angle = self._get_servo_limits(servo_config) home_angle = servo_config.get("home_angle_rad") if servo_config else None self._apply_scale_limits( control.get("scale"), control["angle_var"], min_angle, max_angle, vertical=(control["title"] == "Eyes Vertical"), home_angle=home_angle, use_home=use_home, ) def _should_use_home_for_eye_control(self, control): return not bool(control.get("blink")) def _on_single_servo_id_changed(self, _event=None): self._apply_single_servo_config(use_home=True) self._sync_planner_inputs_from_single() self._apply_constraint_servo_config() self._apply_follow_servo_config() self._schedule_mode_preview_update(delay_ms=0) def _on_constraint_servo_id_changed(self, _event=None): self._apply_constraint_servo_config() def _on_follow_servo_id_changed(self, _event=None): self._apply_follow_servo_config() def _on_eye_control_id_changed(self, control, _event=None): self._apply_eye_control_config(control, use_home=self._should_use_home_for_eye_control(control)) def connect_clicked(self): port = self.port_var.get().strip() baud_text = self.baud_var.get().strip() if not port: messagebox.showerror("Missing port", "Please enter a serial port such as COM8.") return try: baud = int(baud_text) except ValueError: messagebox.showerror("Invalid baud", f"Baud must be an integer, got: {baud_text}") return self.status_var.set("Connecting...") self._run_rpc( f"Connect {port} @ {baud}", lambda: self.rpc.connect(port, baud), on_success=lambda _: self.status_var.set(f"Connected to {port} @ {baud}"), ) def disconnect_clicked(self): self.rpc.close() self.status_var.set("Disconnected") self.last_result_var.set("Result: disconnected") self._append_log("Disconnected from serial transport.") def send_mode_clicked(self): mode_name = self.rpc_mode_var.get() mode_value = self._get_motion_mode_value(mode_name) self._run_rpc( f"setMode({self._get_motion_mode_label(mode_name)})", lambda: self.rpc.set_mode(mode_value), ) def send_motion_settings_clicked(self): try: period_ms = self._parse_int(self.update_period_ms_var, "Period ms") if period_ms < 0: raise ValueError("Period ms must be >= 0.") except ValueError as exc: messagebox.showerror("Motion settings", str(exc)) return mode_name = self.rpc_mode_var.get() mode_value = self._get_motion_mode_value(mode_name) def action(): mode_result = self.rpc.set_mode(mode_value) period_result = self.rpc.set_update_period_ms(period_ms) return {"mode": mode_result, "period": period_result} self._run_rpc( f"setMode({self._get_motion_mode_label(mode_name)}) + setUpdatePeriodMs({period_ms})", action, ) def send_single_clicked(self): servo_id = self.single_id_var.get().strip() if not servo_id: messagebox.showerror("Missing ID", "Servo ID is required.") return angle_rad = self.single_angle_var.get() label, action = self._build_single_command(servo_id, angle_rad) self._run_rpc(label, action) def browse_file_clicked(self): file_path = filedialog.askopenfilename( title="Select servo YAML config", initialdir=str(SERVO_CONFIG_YAML_PATH.parent), filetypes=(("YAML files", "*.yaml *.yml"), ("All files", "*.*")), ) if not file_path: return self.file_local_path_var.set(file_path) if not self.file_remote_path_var.get().strip(): self.file_remote_path_var.set(DEFAULT_REMOTE_SERVO_CONFIG_PATH) try: size = Path(file_path).stat().st_size except OSError: self.file_progress_percent_var.set(0.0) self.file_progress_var.set(f"Selected: {file_path}") else: self.file_progress_percent_var.set(0.0) self.file_progress_var.set(f"Selected {size} bytes from {Path(file_path).name}") def send_file_clicked(self): local_path = self.file_local_path_var.get().strip() remote_path = self.file_remote_path_var.get().strip() if not local_path: messagebox.showerror("File upload", "Local file is required.") return if not remote_path: messagebox.showerror("File upload", "Remote path is required.") return try: yaml_path = Path(local_path).expanduser().resolve() if yaml_path.suffix.lower() not in {".yaml", ".yml"}: raise ValueError("Local config must be a .yaml or .yml file.") if not yaml_path.is_file(): raise FileNotFoundError(f"Local YAML config not found: {yaml_path}") chunk_size = self._parse_int(self.file_chunk_size_var, "Chunk bytes") if chunk_size <= 0: raise ValueError("Chunk bytes must be > 0.") yaml_size = yaml_path.stat().st_size except (OSError, ValueError) as exc: messagebox.showerror("File upload", str(exc)) return if self._file_upload_active: messagebox.showerror("File upload", "Another upload is already running.") return self._file_upload_cancel.clear() self._file_upload_active = True self.file_status_var.set("Converting YAML...") self.file_progress_percent_var.set(0.0) self.file_progress_var.set("Waiting for YAML conversion.") label = ( f"convert+fileWrite {yaml_path.name} -> {remote_path} " f"(chunk={chunk_size}, yaml_size={yaml_size})" ) def progress(bytes_sent, total_size): self.events.put(("file_progress", bytes_sent, total_size)) def action(): json_path = self._convert_yaml_config(yaml_path) if self._file_upload_cancel.is_set(): raise InterruptedError("Upload aborted after YAML conversion.") self.events.put(("file_conversion_complete", str(json_path), json_path.stat().st_size)) result = self.rpc.upload_file( json_path, remote_path, chunk_size, progress=progress, cancelled=self._file_upload_cancel.is_set, ) result["source_yaml"] = str(yaml_path) return result def on_success(result): self._file_upload_active = False self.file_status_var.set("Upload complete") self.file_progress_percent_var.set(100.0) self.file_progress_var.set( f"{result['bytes_sent']} / {result['total_size']} bytes in {result['chunk_count']} chunks" ) def on_error(exc): self._file_upload_active = False if isinstance(exc, InterruptedError): self.file_status_var.set("Upload aborted") self.file_progress_var.set("Upload aborted before completion.") return self.file_status_var.set("Upload failed") self._run_rpc(label, action, on_success=on_success, on_error=on_error) @staticmethod def _convert_yaml_config(yaml_path): if not YAML_TO_JSON_SCRIPT.is_file(): raise FileNotFoundError(f"YAML converter not found: {YAML_TO_JSON_SCRIPT}") json_path = Path(yaml_path).with_suffix(".json") completed = subprocess.run( [sys.executable, str(YAML_TO_JSON_SCRIPT), str(yaml_path), str(json_path)], cwd=str(YAML_TO_JSON_SCRIPT.parent), capture_output=True, text=True, check=False, ) if completed.returncode != 0: detail = completed.stderr.strip() or completed.stdout.strip() raise RuntimeError(detail or "YAML conversion failed without an error message.") if not json_path.is_file(): raise FileNotFoundError(f"Converted JSON was not created: {json_path}") return json_path def abort_file_clicked(self): if self._file_upload_active: self._file_upload_cancel.set() self.file_status_var.set("Aborting upload...") self._append_log("Abort requested for current upload.") return self._run_rpc( "fileWriteAbort()", lambda: self.rpc.file_write_abort(), on_success=lambda _result: self.file_status_var.set("Abort sent"), ) def _send_file_from_event(self, _event): self.send_file_clicked() def _send_single_from_event(self, _event): self.send_single_clicked() def _blink_from_event(self, _event): self.blink_clicked() def _on_single_scale_changed(self, _value): self._schedule_mode_preview_update() if not self.single_live_drag_var.get(): return servo_id = self.single_id_var.get().strip() if not servo_id or not self.rpc.is_connected: return self._single_live_pending = True if self._single_live_job is None: self._schedule_single_live_send() def _on_single_scale_released(self, _event): self._schedule_mode_preview_update(delay_ms=0) if self.single_live_drag_var.get(): if self._single_live_pending and self._single_live_job is None: self._schedule_single_live_send() return self.send_single_clicked() def _on_single_live_drag_toggled(self): if not self.single_live_drag_var.get(): self._cancel_single_live_send() self._single_live_pending = False return if self._single_live_pending and self._single_live_job is None: self._schedule_single_live_send() def _on_motion_mode_changed(self): self._select_mode_tab(self.rpc_mode_var.get()) self._schedule_mode_preview_update(delay_ms=0) def _select_mode_tab(self, mode_name): if not self.notebook: return if mode_name == "scurve": self.notebook.select(self.tab_frames["scurve"]) elif mode_name == "follow": self.notebook.select(self.tab_frames["follow"]) def _schedule_mode_preview_update(self, delay_ms=50): if self._preview_update_job is not None: self.root.after_cancel(self._preview_update_job) self._preview_update_job = self.root.after(delay_ms, self._update_mode_preview) def _update_mode_preview(self): self._preview_update_job = None self._sync_planner_inputs_from_single() mode_name = self.rpc_mode_var.get() if mode_name == "scurve": self._plan_scurve(show_errors=False) elif mode_name == "follow": self._plan_position_planner(show_errors=False) def _sync_planner_inputs_from_single(self): servo_id = self.single_id_var.get().strip() if servo_id: self.constraint_servo_id_var.set(servo_id) self.follow_servo_id_var.set(servo_id) self._apply_constraint_servo_config() self._apply_follow_servo_config() angle_rad = self.single_angle_var.get() angle_text = f"{angle_rad:.4f}" self.scurve_end_position_var.set(angle_text) self.position_planner_target_position_var.set(angle_text) def fill_all_clicked(self): value = self.fill_angle_var.get() for control in self.eye_controls: control["angle_var"].set(value) self._append_log( f"Filled {len(EYE_CONTROL_SPECS)} visible eye servos with {value:.4f} rad." ) def zero_all_clicked(self): for control in self.eye_controls: control["angle_var"].set(0.0) self._append_log("Visible eye servos set to 0.0000 rad locally.") def send_all_clicked(self): if not self.rpc.is_connected: messagebox.showerror("Eye Servo Layout", "Not connected. Configure serial and click Connect first.") return try: items = self._build_eye_move_items() except ValueError as exc: messagebox.showerror("Eye Servo Layout", str(exc)) return label = "move([" + ", ".join(f"{servo_id}: {angle_rad:.4f}" for servo_id, angle_rad in items) + "])" self.status_var.set("Sending eye move...") self._run_rpc( label, lambda: self.rpc.move(items), on_success=lambda _result: self.status_var.set("Eye move sent"), ) def blink_clicked(self): if self._blink_active: messagebox.showinfo("Blink", "Blink is already in progress.") return if not self.rpc.is_connected: messagebox.showerror("Blink", "Not connected. Configure serial and click Connect first.") return self._blink_active = True self.status_var.set("Blinking...") self._start_blink_cycle() def stop_blink_clicked(self): if not self._blink_active: return had_pending_cycle = self._blink_loop_job is not None if self._blink_loop_job is not None: self.root.after_cancel(self._blink_loop_job) self._blink_loop_job = None self._blink_active = False self.status_var.set("Blink stopped" if had_pending_cycle else "Stopping blink...") def send_scurve_constraints_clicked(self): try: servo_id = self.constraint_servo_id_var.get().strip() max_velocity = self._parse_float(self.scurve_max_velocity_var, "Max velocity") max_acceleration = self._parse_float(self.scurve_max_acceleration_var, "Max accel") max_jerk = self._parse_float(self.scurve_max_jerk_var, "Max jerk") except ValueError as exc: messagebox.showerror("SCurve constraints", str(exc)) return if not servo_id: messagebox.showerror("SCurve constraints", "Servo ID is required.") return self._run_rpc( f"setConstraints({servo_id}, {max_velocity:.4f}, {max_acceleration:.4f}, {max_jerk:.4f})", lambda: self.rpc.set_constraints(servo_id, max_velocity, max_acceleration, max_jerk), on_success=lambda _result: self.scurve_status_var.set("SCurve constraints sent"), ) def plan_scurve_clicked(self): self._plan_scurve(show_errors=True) def _plan_scurve(self, show_errors): try: max_velocity = self._parse_float(self.scurve_max_velocity_var, "Max velocity") max_acceleration = self._parse_float(self.scurve_max_acceleration_var, "Max accel") max_jerk = self._parse_float(self.scurve_max_jerk_var, "Max jerk") start_position = self._parse_float(self.scurve_start_position_var, "Start pos") end_position = self._parse_float(self.scurve_end_position_var, "End pos") start_velocity = self._parse_float(self.scurve_start_velocity_var, "Start vel") end_velocity = self._parse_float(self.scurve_end_velocity_var, "End vel") sample_dt = self._parse_float(self.scurve_sample_dt_var, "Sample dt") if sample_dt <= 0: raise ValueError("Sample dt must be > 0.") planner = self._ensure_scurve() planner.setConstraints(max_velocity, max_acceleration, max_jerk) profile = planner.calculateProfile( start_position, end_position, start_velocity, end_velocity, ) estimated_points = max(1, int(profile.total_time / sample_dt) + 1) if estimated_points > 20000: raise ValueError( f"Sample dt is too small for this move. It would create about {estimated_points} points." ) samples = planner.sampleTrajectory(profile, sample_dt) except (ValueError, FileNotFoundError, OSError, SCurveError) as exc: self.scurve_status_var.set("SCurve error") self.scurve_summary_var.set(str(exc)) self.scurve_segments_var.set("Segments: -") self._clear_curve_plots() if show_errors: messagebox.showerror("SCurve planner", str(exc)) return self.scurve_status_var.set("SCurve ready") self.scurve_summary_var.set( f"distance={profile.distance:.4f} | total={profile.total_time:.4f}s | " f"v_cruise={profile.v_cruise:.4f} | a_limit={profile.a_limit:.4f} | " f"samples={len(samples['times'])}" ) self.scurve_segments_var.set( "Segments: " f"t1={profile.t1:.4f}, t2={profile.t2:.4f}, t3={profile.t3:.4f}, " f"t4={profile.t4:.4f}, t5={profile.t5:.4f}, t6={profile.t6:.4f}, t7={profile.t7:.4f}" ) self.scurve_plot_series["position"] = { "title": "Position", "color": "#7ce0a0", "times": samples["times"], "values": samples["positions"], } self.scurve_plot_series["velocity"] = { "title": "Velocity", "color": "#65c7ff", "times": samples["times"], "values": samples["velocities"], } self.scurve_plot_series["acceleration"] = { "title": "Acceleration", "color": "#ffd166", "times": samples["times"], "values": samples["accelerations"], } self.scurve_plot_series["jerk"] = { "title": "Jerk", "color": "#ff8c7e", "times": samples["times"], "values": samples["jerks"], } self._redraw_all_curve_plots() self._append_log( f"SCurve planned: total={profile.total_time:.4f}s, samples={len(samples['times'])}, " f"constraints=({max_velocity:.3f}, {max_acceleration:.3f}, {max_jerk:.3f})" ) def _plan_scurve_from_event(self, _event): self.plan_scurve_clicked() def send_follow_constraints_clicked(self): try: servo_id = self.follow_servo_id_var.get().strip() max_velocity = self._parse_float(self.position_planner_max_velocity_var, "Max velocity") max_acceleration = self._parse_float(self.position_planner_max_acceleration_var, "Max accel") max_jerk = self._parse_float(self.position_planner_max_jerk_var, "Max jerk") except ValueError as exc: messagebox.showerror("Follow planner constraints", str(exc)) return if not servo_id: messagebox.showerror("Follow planner constraints", "Servo ID is required.") return self._run_rpc( f"setConstraints({servo_id}, {max_velocity:.4f}, {max_acceleration:.4f}, {max_jerk:.4f})", lambda: self.rpc.set_constraints(servo_id, max_velocity, max_acceleration, max_jerk), on_success=lambda _result: self.position_planner_status_var.set("Follow constraints sent"), ) def send_follow_position_gain_clicked(self): try: servo_id = self.follow_servo_id_var.get().strip() position_gain = self._parse_float(self.position_planner_gain_var, "Pos gain") except ValueError as exc: messagebox.showerror("Follow planner gain", str(exc)) return if not servo_id: messagebox.showerror("Follow planner gain", "Servo ID is required.") return self._run_rpc( f"setPositionGain({servo_id}, {position_gain:.4f})", lambda: self.rpc.set_position_gain(servo_id, position_gain), on_success=lambda _result: self.position_planner_status_var.set("Follow gain sent"), ) def plan_position_planner_clicked(self): self._plan_position_planner(show_errors=True) def _plan_position_planner(self, show_errors): try: max_velocity = self._parse_float(self.position_planner_max_velocity_var, "Max velocity") max_acceleration = self._parse_float(self.position_planner_max_acceleration_var, "Max accel") max_jerk = self._parse_float(self.position_planner_max_jerk_var, "Max jerk") position_gain = self._parse_float(self.position_planner_gain_var, "Pos gain") initial_position = self._parse_float(self.position_planner_initial_position_var, "Initial pos") initial_velocity = self._parse_float(self.position_planner_initial_velocity_var, "Initial vel") initial_acceleration = self._parse_float(self.position_planner_initial_acceleration_var, "Initial accel") target_position = self._parse_float(self.position_planner_target_position_var, "Target pos") sample_dt = self._parse_float(self.position_planner_sample_dt_var, "Sample dt") duration = self._parse_float(self.position_planner_duration_var, "Duration") if sample_dt <= 0: raise ValueError("Sample dt must be > 0.") if duration < 0: raise ValueError("Duration must be >= 0.") planner = self._ensure_position_planner() planner.setConstraints(max_velocity, max_acceleration, max_jerk) planner.setPositionGain(position_gain) samples = planner.sampleTrajectory( target_position=target_position, dt=sample_dt, duration=duration, initial_position=initial_position, initial_velocity=initial_velocity, initial_acceleration=initial_acceleration, position_gain=position_gain, ) except (ValueError, FileNotFoundError, OSError, SCurveError) as exc: self.position_planner_status_var.set("Follow planner error") self.position_planner_summary_var.set(str(exc)) self.position_planner_target_var.set("Target: -") self._clear_position_planner_plots() if show_errors: messagebox.showerror("Follow planner", str(exc)) return final_position = samples["positions"][-1] final_velocity = samples["velocities"][-1] final_acceleration = samples["accelerations"][-1] self.position_planner_status_var.set("Follow planner ready") self.position_planner_summary_var.set( f"duration={duration:.4f}s | samples={len(samples['times'])} | " f"final_pos={final_position:.4f} | final_vel={final_velocity:.4f} | final_acc={final_acceleration:.4f}" ) self.position_planner_target_var.set( f"Target: {target_position:.4f} | initial={initial_position:.4f} | gain={position_gain:.4f}" ) self.position_planner_plot_series["position"] = { "title": "Position", "color": "#6abf69", "times": samples["times"], "values": samples["positions"], } self.position_planner_plot_series["velocity"] = { "title": "Velocity", "color": "#4da3ff", "times": samples["times"], "values": samples["velocities"], } self.position_planner_plot_series["acceleration"] = { "title": "Acceleration", "color": "#f2b43c", "times": samples["times"], "values": samples["accelerations"], } self.position_planner_plot_series["jerk"] = { "title": "Jerk", "color": "#ef6c73", "times": samples["times"], "values": samples["jerks"], } self._redraw_all_position_planner_plots() self._append_log( f"Follow planner: target={target_position:.4f}, duration={duration:.4f}s, " f"samples={len(samples['times'])}, constraints=({max_velocity:.3f}, {max_acceleration:.3f}, {max_jerk:.3f})" ) def _plan_position_planner_from_event(self, _event): self.plan_position_planner_clicked() def _ensure_scurve(self): if self.scurve is None: self.scurve = SCurve() return self.scurve def _ensure_position_planner(self): if self.position_planner is None: self.position_planner = SCurvePositionPlanner1D() return self.position_planner def _parse_float(self, variable, label): try: return float(variable.get().strip()) except ValueError as exc: raise ValueError(f"{label} must be a valid number.") from exc def _parse_int(self, variable, label): try: return int(variable.get().strip()) except ValueError as exc: raise ValueError(f"{label} must be a valid integer.") from exc def _build_eye_move_items(self): items = [] used_ids = set() for control in self.eye_controls: servo_id = control["id_var"].get().strip() if not servo_id: raise ValueError(f"{control['title']} ID is required.") if servo_id in used_ids: raise ValueError(f"Duplicate eye control ID: {servo_id}.") used_ids.add(servo_id) items.append((servo_id, float(control["angle_var"].get()))) return items def _build_blink_move_items(self): close_items = [] open_items = [] used_ids = set() has_motion = False for control in self.eye_controls: if not control.get("blink"): continue servo_id = control["id_var"].get().strip() if not servo_id: raise ValueError(f"{control['title']} ID is required for blink.") if servo_id in used_ids: raise ValueError(f"Duplicate blink servo ID: {servo_id}.") servo_config = self._get_servo_config(servo_id) if not servo_config: raise ValueError(f"Blink servo config not found for ID: {servo_id}.") if "home_angle_rad" not in servo_config: raise ValueError(f"Blink servo {servo_id} is missing home_angle_rad.") used_ids.add(servo_id) min_angle, max_angle = self._get_servo_limits(servo_config) close_angle = max(min_angle, min(max_angle, float(servo_config["home_angle_rad"]))) open_angle = max(min_angle, min(max_angle, float(control["angle_var"].get()))) if abs(open_angle - close_angle) > 1e-6: has_motion = True close_items.append((servo_id, close_angle)) open_items.append((servo_id, open_angle)) if not close_items: raise ValueError("No blink-enabled eye servos are configured.") if not has_motion: raise ValueError("Blink open angles match home_angle_rad. Adjust the eyelid sliders to an open-eye position first.") return close_items, open_items def _build_blink_transition_frames(self, start_items, end_items): if len(start_items) != len(end_items): raise ValueError("Blink frame build failed: item count mismatch.") steps = max(1, int(math.ceil(BLINK_TRANSITION_MS / BLINK_STEP_MS))) frames = [] for step_index in range(1, steps + 1): ratio = step_index / steps frame = [] for (start_id, start_angle), (end_id, end_angle) in zip(start_items, end_items): if start_id != end_id: raise ValueError("Blink frame build failed: servo order mismatch.") angle = start_angle + (end_angle - start_angle) * ratio frame.append((start_id, angle)) frames.append(frame) return frames def _start_blink_cycle(self): if not self._blink_active: return try: hold_ms = self._parse_int(self.blink_hold_ms_var, "Blink delay ms") if hold_ms < 0: raise ValueError("Blink delay ms must be >= 0.") close_items, open_items = self._build_blink_move_items() except ValueError as exc: self._blink_active = False self.status_var.set("Blink failed") messagebox.showerror("Blink", str(exc)) return self._blink_loop_job = None self._run_rpc( f"blink(delay={hold_ms} ms)", lambda: self._run_blink_sequence(close_items, open_items, hold_ms), on_success=lambda result, delay_ms=hold_ms: self._on_blink_success(result, delay_ms), on_error=self._on_blink_error, ) def _schedule_blink_cycle(self, delay_ms): if not self._blink_active: return if self._blink_loop_job is not None: self.root.after_cancel(self._blink_loop_job) self._blink_loop_job = self.root.after(delay_ms, self._start_blink_cycle) def _run_blink_sequence(self, close_items, open_items, hold_ms): close_frames = self._build_blink_transition_frames(open_items, close_items) open_frames = self._build_blink_transition_frames(close_items, open_items) step_sleep_s = BLINK_STEP_MS / 1000.0 close_result = 0 for frame in close_frames: close_result = self.rpc.move(frame) if step_sleep_s > 0: time.sleep(step_sleep_s) if hold_ms > 0: time.sleep(hold_ms / 1000.0) open_result = 0 for frame in open_frames: open_result = self.rpc.move(frame) if step_sleep_s > 0: time.sleep(step_sleep_s) return { "close_result": close_result, "open_result": open_result, "hold_ms": hold_ms, } def _on_blink_success(self, _result, delay_ms): if not self._blink_active: self.status_var.set("Blink stopped") return self.status_var.set("Blinking...") self._schedule_blink_cycle(delay_ms) def _on_blink_error(self, exc): self._blink_active = False if self._blink_loop_job is not None: self.root.after_cancel(self._blink_loop_job) self._blink_loop_job = None self.status_var.set("Blink failed") messagebox.showerror("Blink", str(exc)) def _get_single_live_interval_ms(self): try: rate_hz = float(self.single_live_rate_hz_var.get().strip()) except ValueError: rate_hz = 50.0 if rate_hz <= 0: rate_hz = 50.0 return max(1, int(round(1000.0 / rate_hz))) def _schedule_single_live_send(self): interval_ms = self._get_single_live_interval_ms() self._single_live_job = self.root.after(interval_ms, self._flush_single_live_send) def _flush_single_live_send(self): self._single_live_job = None if not self.single_live_drag_var.get() or not self._single_live_pending: return servo_id = self.single_id_var.get().strip() if not servo_id or not self.rpc.is_connected: self._single_live_pending = False return angle_rad = self.single_angle_var.get() label, action = self._build_single_command(servo_id, angle_rad) self._single_live_pending = False self._run_rpc(label, action, log_request=False) if self.single_live_drag_var.get() and self._single_live_pending: self._schedule_single_live_send() def _cancel_single_live_send(self): if self._single_live_job is not None: self.root.after_cancel(self._single_live_job) self._single_live_job = None def _get_motion_mode_value(self, mode_name): if mode_name == "follow": return servo_service_common.RpcMotionMode.RpcMotionModeFollow if mode_name == "scurve": return servo_service_common.RpcMotionMode.RpcMotionModeSCurve return servo_service_common.RpcMotionMode.RpcMotionModeImmediate def _get_motion_mode_label(self, mode_name): if mode_name == "follow": return "Follow" if mode_name == "scurve": return "SCurve" return "Immediate" def _build_single_command(self, servo_id, angle_rad): return ( f"move([{servo_id}: {angle_rad:.4f} rad])", lambda: self.rpc.move([(servo_id, angle_rad)]), ) def _clear_curve_plots(self): for key, series in self.scurve_plot_series.items(): series["times"] = [] series["values"] = [] self._redraw_curve_plot(key) def _clear_position_planner_plots(self): for key, series in self.position_planner_plot_series.items(): series["times"] = [] series["values"] = [] self._redraw_position_planner_plot(key) def _redraw_all_curve_plots(self): for key in self.scurve_canvases: self._redraw_curve_plot(key) def _redraw_all_position_planner_plots(self): for key in self.position_planner_canvases: self._redraw_position_planner_plot(key) def _redraw_curve_plot(self, key): canvas = self.scurve_canvases.get(key) series = self.scurve_plot_series.get(key) self._render_plot_series(canvas, series) def _redraw_position_planner_plot(self, key): canvas = self.position_planner_canvases.get(key) series = self.position_planner_plot_series.get(key) self._render_plot_series(canvas, series) def _render_plot_series(self, canvas, series): if not canvas or not series: return canvas.delete("all") width = max(canvas.winfo_width(), 10) height = max(canvas.winfo_height(), 10) if width < 80 or height < 80: return if not series["times"] or not series["values"]: canvas.create_text( width / 2, height / 2, text="Plan curve to preview.", fill=self.colors["muted_text"], font=("Segoe UI", 11), ) return left = 60 top = 16 right = width - 18 bottom = height - 30 plot_width = max(1, right - left) plot_height = max(1, bottom - top) times = series["times"] values = series["values"] min_time = times[0] max_time = times[-1] if times[-1] > times[0] else times[0] + 1.0 min_value = min(values) max_value = max(values) if abs(max_value - min_value) < 1e-9: padding = max(1.0, abs(max_value) * 0.1 + 1.0) min_value -= padding max_value += padding else: padding = (max_value - min_value) * 0.1 min_value -= padding max_value += padding for step in range(6): x = left + plot_width * step / 5 y = top + plot_height * step / 5 canvas.create_line(x, top, x, bottom, fill=self.colors["plot_grid"], width=1) canvas.create_line(left, y, right, y, fill=self.colors["plot_grid"], width=1) if min_value < 0 < max_value: zero_y = top + (max_value / (max_value - min_value)) * plot_height canvas.create_line(left, zero_y, right, zero_y, fill=self.colors["plot_axis"], width=1) canvas.create_rectangle(left, top, right, bottom, outline=self.colors["plot_axis"], width=1) points = [] value_span = max_value - min_value time_span = max_time - min_time for t_value, y_value in zip(times, values): x = left + ((t_value - min_time) / time_span) * plot_width y = bottom - ((y_value - min_value) / value_span) * plot_height points.extend((x, y)) if len(points) >= 4: canvas.create_line(*points, fill=series["color"], width=2) canvas.create_text(left, top - 8, anchor="sw", text=series["title"], fill=self.colors["hero_text"], font=("Segoe UI Semibold", 10)) canvas.create_text(left - 8, top, anchor="ne", text=f"{max_value:.4f}", fill=self.colors["muted_text"], font=("Consolas", 9)) canvas.create_text(left - 8, bottom, anchor="ne", text=f"{min_value:.4f}", fill=self.colors["muted_text"], font=("Consolas", 9)) canvas.create_text(left, bottom + 8, anchor="nw", text=f"{min_time:.4f}s", fill=self.colors["muted_text"], font=("Consolas", 9)) canvas.create_text(right, bottom + 8, anchor="ne", text=f"{max_time:.4f}s", fill=self.colors["muted_text"], font=("Consolas", 9)) def _run_rpc(self, label, action, on_success=None, on_error=None, log_request=True): if log_request: self._append_log(f"> {label}") def worker(): started_at = time.perf_counter() try: result = action() except Exception as exc: elapsed_ms = (time.perf_counter() - started_at) * 1000.0 self.events.put(("error", label, exc, traceback.format_exc(), elapsed_ms, on_error)) else: elapsed_ms = (time.perf_counter() - started_at) * 1000.0 self.events.put(("success", label, result, on_success, elapsed_ms)) threading.Thread(target=worker, daemon=True).start() def _poll_events(self): while True: try: event = self.events.get_nowait() except queue.Empty: break self._handle_event(event) self.root.after(120, self._poll_events) def _handle_event(self, event): kind = event[0] if kind == "file_conversion_complete": _, json_path, total_size = event self.file_status_var.set("Uploading JSON...") self.file_progress_var.set(f"Converted {Path(json_path).name}: {total_size} bytes") self._append_log(f"Converted YAML to {json_path} ({total_size} bytes)") return if kind == "file_progress": _, bytes_sent, total_size = event percent = (bytes_sent / total_size * 100.0) if total_size else 100.0 self.file_progress_percent_var.set(percent) self.file_status_var.set( f"Uploading... {bytes_sent}/{total_size} bytes" ) self.file_progress_var.set( f"{bytes_sent} / {total_size} bytes ({percent:.1f}%)" ) return if kind == "success": _, label, result, on_success, elapsed_ms = event self.last_result_var.set(f"Result: {result} ({elapsed_ms:.3f} ms)") self._append_log(f"< {label} -> {result} [{elapsed_ms:.3f} ms]") if callable(on_success): on_success(result) return _, label, exc, trace_text, elapsed_ms, on_error = event self.status_var.set("Error") self.last_result_var.set(f"Result: error ({exc}) [{elapsed_ms:.3f} ms]") self._append_log(f"! {label} failed: {exc} [{elapsed_ms:.3f} ms]") self._append_log(trace_text.strip()) if callable(on_error): on_error(exc) def _append_log(self, text): self.log_widget.configure(state="normal") self.log_widget.insert("end", text + "\n") self.log_widget.see("end") self.log_widget.configure(state="disabled") def on_close(self): self.rpc.close() self.root.destroy() def main(): root = tk.Tk() app = FaceServoControlApp(root) root.mainloop() if __name__ == "__main__": main()