lgv/cmvr-head-client
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
master
cmvr-head-client/ui/face_servo_control.py
lgv 78578a1bed first commit
2026-08-05 16:42:59 +08:00

2343 lines
94 KiB
Python
Raw Permalink Blame History

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("<<ComboboxSelected>>", self._on_single_servo_id_changed)
single_id_entry.bind("<FocusOut>", self._on_single_servo_id_changed)
single_id_entry.bind("<Return>", 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("<Return>", self._send_single_from_event)
self.single_angle_scale.bind("<ButtonRelease-1>", 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("<Return>", 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("<Return>", 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("<Return>", 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("<Return>", 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("<<ComboboxSelected>>", lambda event, item=control: self._on_eye_control_id_changed(item, event))
id_combo.bind("<FocusOut>", lambda event, item=control: self._on_eye_control_id_changed(item, event))
id_combo.bind("<Return>", 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("<<ComboboxSelected>>", lambda event, item=control: self._on_eye_control_id_changed(item, event))
id_combo.bind("<FocusOut>", lambda event, item=control: self._on_eye_control_id_changed(item, event))
id_combo.bind("<Return>", 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("<<ComboboxSelected>>", lambda event, item=control: self._on_eye_control_id_changed(item, event))
id_combo.bind("<FocusOut>", lambda event, item=control: self._on_eye_control_id_changed(item, event))
id_combo.bind("<Return>", 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("<<ComboboxSelected>>", self._on_constraint_servo_id_changed)
entry.bind("<FocusOut>", self._on_constraint_servo_id_changed)
entry.bind("<Return>", self._on_constraint_servo_id_changed)
else:
entry = ttk.Entry(controls, textvariable=variable)
entry.bind("<Return>", 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("<Configure>", 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("<<ComboboxSelected>>", self._on_follow_servo_id_changed)
entry.bind("<FocusOut>", self._on_follow_servo_id_changed)
entry.bind("<Return>", self._on_follow_servo_id_changed)
else:
entry = ttk.Entry(controls, textvariable=variable)
entry.bind("<Return>", 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("<Configure>", 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()
Reference in New Issue View Git Blame Copy Permalink