import math import tkinter as tk from tkinter import ttk import time # Try to import serial try: import serial import serial.tools.list_ports SERIAL_AVAILABLE = True except ImportError: SERIAL_AVAILABLE = False serial = None print("Warning: pyserial not found. Serial communication will be disabled.") print("Install with: pip install pyserial") # Serial communication protocol functions SYNC0 = 0xA5 SYNC1 = 0x5A BAUD_RATE = 1000000 # 1 Mbps TAG_MSET = 'MSET' # Set motor positions # Motor IDs LEFT_EYE_ID = 14 RIGHT_EYE_ID = 15 # Eye position range EYE_MIN_POS = 1800 EYE_MAX_POS = 2500 EYE_CENTER_POS = 2200 # Midpoint of 1000-3000 # Angle range for eyes (degrees) EYE_ANGLE_MIN = -90.0 EYE_ANGLE_MAX = 90.0 def crc16_ccitt(data, init=0xFFFF): """CRC16-CCITT calculation""" crc = init for byte in data: crc ^= byte << 8 for i in range(8): if crc & 0x8000: crc = ((crc << 1) ^ 0x1021) & 0xFFFF else: crc = (crc << 1) & 0xFFFF return crc def tag_to_bytes(tag): """Convert 4-character tag string to bytes""" return tag.encode('ascii')[:4].ljust(4, b' ')[:4] def send_packet(ser, tag, payload=b''): """Send a packet with the protocol format""" payload_array = bytes(payload) if isinstance(payload, (list, tuple)) else payload length = len(payload_array) seq = 0 # Simple sequence number # Build packet packet = bytearray() packet.append(SYNC0) packet.append(SYNC1) packet.extend(tag_to_bytes(tag)) packet.append(length & 0xFF) packet.append((length >> 8) & 0xFF) packet.append(seq & 0xFF) packet.append((seq >> 8) & 0xFF) packet.extend(payload_array) # Calculate CRC over tag + length + seq + payload crc_data = packet[2:] # Everything after sync bytes crc = crc16_ccitt(crc_data) packet.append(crc & 0xFF) packet.append((crc >> 8) & 0xFF) packet_bytes = bytes(packet) ser.write(packet_bytes) ser.flush() def send_motor_positions(ser, motor_positions): """ Send multiple motor position updates in ONE MSET packet. Args: ser: Serial connection object motor_positions: List of (motor_id, position) tuples Format: For each motor: [motor_id: 1 byte][position: 2 bytes little-endian] """ if not motor_positions: return # Build payload: [motor_id (1 byte), position_low (1 byte), position_high (1 byte)] for each motor payload = bytearray() for motor_id, position in motor_positions: payload.append(motor_id) payload.append(position & 0xFF) # Low byte payload.append((position >> 8) & 0xFF) # High byte motor_str = ", ".join([f"{mid}:{pos}" for mid, pos in motor_positions]) print(f"Sending motor positions: [{motor_str}]") send_packet(ser, TAG_MSET, bytes(payload)) def angle_to_eye_position(angle_degrees): """ Convert angle in degrees to eye motor position (1000-3000). -90 degrees -> 1000 0 degrees -> 2000 (center) +90 degrees -> 3000 """ # Normalize angle to -1.0 to 1.0 normalized = max(-1.0, min(1.0, angle_degrees / 90.0)) # Map to 1000-3000 range position = EYE_CENTER_POS + (normalized * (EYE_MAX_POS - EYE_CENTER_POS)) return int(position) def lerp(start, end, t): """Linear interpolation between start and end, t is 0.0 to 1.0""" return start + (end - start) * t class EyeControlGUI: def __init__(self, root): self.root = root self.root.title("Eye Control") self.root.geometry("500x350") # Serial connection self.serial_connection = None self.last_send_time = 0.0 self.min_send_interval = 1.0 / 60.0 # 60 updates per second # Animation state self.lerping = False self.lerp_start_time = 0.0 self.lerp_duration = 0.5 # Calculated from distance and max speed self.eye_start_pos = EYE_CENTER_POS self.eye_target_pos = EYE_CENTER_POS self.max_speed = 500.0 # Max speed in position units per second # Current motor positions (for display and lerping) self.current_left_eye_pos = EYE_CENTER_POS self.current_right_eye_pos = EYE_CENTER_POS # Create main frame main_frame = ttk.Frame(root, padding="10") main_frame.grid(row=0, column=0, sticky=(tk.W, tk.E, tk.N, tk.S)) main_frame.columnconfigure(1, weight=1) root.columnconfigure(0, weight=1) root.rowconfigure(0, weight=1) row = 0 # Angle control section angle_frame = ttk.LabelFrame(main_frame, text="Target Angle", padding="10") angle_frame.grid(row=row, column=0, columnspan=2, sticky=(tk.W, tk.E), pady=5) angle_frame.columnconfigure(1, weight=1) row += 1 ttk.Label(angle_frame, text="Angle (degrees):").grid(row=0, column=0, sticky=tk.W, pady=5) self.angle_var = tk.DoubleVar(value=0.0) self.angle_scale = ttk.Scale(angle_frame, from_=-90, to=90, variable=self.angle_var, orient=tk.HORIZONTAL, command=self.on_angle_change) self.angle_scale.grid(row=0, column=1, sticky=(tk.W, tk.E), pady=5, padx=5) self.angle_label = ttk.Label(angle_frame, text="0.0°", width=8) self.angle_label.grid(row=0, column=2, padx=5) # Speed control section speed_frame = ttk.LabelFrame(main_frame, text="Movement Speed", padding="10") speed_frame.grid(row=row, column=0, columnspan=2, sticky=(tk.W, tk.E), pady=5) speed_frame.columnconfigure(1, weight=1) row += 1 ttk.Label(speed_frame, text="Max Speed (units/sec):").grid(row=0, column=0, sticky=tk.W, pady=5) self.max_speed_var = tk.DoubleVar(value=500.0) self.speed_scale = ttk.Scale(speed_frame, from_=50, to=2000, variable=self.max_speed_var, orient=tk.HORIZONTAL, command=self.on_max_speed_change) self.speed_scale.grid(row=0, column=1, sticky=(tk.W, tk.E), pady=5, padx=5) self.speed_label = ttk.Label(speed_frame, text="500", width=8) self.speed_label.grid(row=0, column=2, padx=5) # Status section status_frame = ttk.LabelFrame(main_frame, text="Status", padding="10") status_frame.grid(row=row, column=0, columnspan=2, sticky=(tk.W, tk.E), pady=5) row += 1 self.status_label = ttk.Label(status_frame, text="Disconnected", foreground="red") self.status_label.grid(row=0, column=0, sticky=tk.W) self.animation_status_label = ttk.Label(status_frame, text="Ready", foreground="green") self.animation_status_label.grid(row=1, column=0, sticky=tk.W, pady=5) # Motor positions display positions_frame = ttk.LabelFrame(main_frame, text="Motor Positions", padding="10") positions_frame.grid(row=row, column=0, columnspan=2, sticky=(tk.W, tk.E), pady=5) row += 1 ttk.Label(positions_frame, text="Left Eye (ID 14):").grid(row=0, column=0, sticky=tk.W, pady=5) self.left_eye_label = ttk.Label(positions_frame, text=str(EYE_CENTER_POS), font=("Arial", 12, "bold")) self.left_eye_label.grid(row=0, column=1, sticky=tk.W, padx=10) ttk.Label(positions_frame, text="Right Eye (ID 15):").grid(row=1, column=0, sticky=tk.W, pady=5) self.right_eye_label = ttk.Label(positions_frame, text=str(EYE_CENTER_POS), font=("Arial", 12, "bold")) self.right_eye_label.grid(row=1, column=1, sticky=tk.W, padx=10) # Handle window close self.root.protocol("WM_DELETE_WINDOW", self.on_closing) # Connect to serial port self.connect_to_serial() # Start animation update loop self.update_animation() def connect_to_serial(self): """Find and connect to the only available COM port""" if not SERIAL_AVAILABLE: self.status_label.config(text="pyserial not installed", foreground="red") print("pyserial not available") return try: ports = list(serial.tools.list_ports.comports()) if not ports: self.status_label.config(text="No COM port found", foreground="red") print("No COM port found") return if len(ports) > 1: port_names = [p.device for p in ports] self.status_label.config(text=f"Multiple ports found: {', '.join(port_names)}", foreground="orange") print(f"Warning: Multiple ports found: {port_names}, using first: {port_names[0]}") port = ports[0].device print(f"Connecting to {port}...") self.serial_connection = serial.Serial( port=port, baudrate=BAUD_RATE, timeout=2.0, write_timeout=2.0 ) self.status_label.config(text=f"Connected to {port}", foreground="green") print(f"Connected to {port}") except serial.SerialException as e: self.status_label.config(text=f"Connection failed: {str(e)}", foreground="red") print(f"Connection failed: {e}") self.serial_connection = None except Exception as e: self.status_label.config(text=f"Error: {str(e)}", foreground="red") print(f"Error connecting: {e}") self.serial_connection = None def on_closing(self): """Handle window close event""" if SERIAL_AVAILABLE and self.serial_connection and self.serial_connection.is_open: try: self.serial_connection.close() print("Serial connection closed") except: pass self.root.destroy() def on_max_speed_change(self, *args): """Called when max speed slider changes""" speed = self.max_speed_var.get() self.speed_label.config(text=f"{int(speed)}") self.max_speed = speed def on_angle_change(self, *args): """Called when angle slider changes""" angle = self.angle_var.get() self.angle_label.config(text=f"{angle:.1f}°") # Calculate target eye position from angle target_pos = angle_to_eye_position(angle) # Start lerping from current position to target position if target_pos != self.eye_target_pos: self.eye_start_pos = self.current_left_eye_pos # Both eyes same position self.eye_target_pos = target_pos # Calculate duration based on distance and max speed # For smoothstep S-curve, peak velocity = 1.5 * distance / duration # So: duration = 1.5 * distance / max_speed distance = abs(self.eye_target_pos - self.eye_start_pos) if distance > 0 and self.max_speed > 0: self.lerp_duration = (1.5 * distance) / self.max_speed # Minimum duration to prevent instant jumps self.lerp_duration = max(0.016, self.lerp_duration) else: self.lerp_duration = 0.016 # Minimum 1 frame self.lerping = True self.lerp_start_time = time.time() self.animation_status_label.config(text=f"Moving... ({self.lerp_duration:.2f}s)", foreground="blue") print(f"[DEBUG] New target: {target_pos}, distance: {distance}, duration: {self.lerp_duration:.3f}s") def update_animation(self): """Update animation loop - called periodically""" if self.lerping: current_time = time.time() elapsed = current_time - self.lerp_start_time if elapsed >= self.lerp_duration: # Lerp complete self.current_left_eye_pos = self.eye_target_pos self.current_right_eye_pos = self.eye_target_pos self.lerping = False self.animation_status_label.config(text="Ready", foreground="green") print(f"[DEBUG] Lerp complete at {self.eye_target_pos}") else: # Continue lerping t = elapsed / self.lerp_duration # Smooth interpolation (ease in-out) - S-curve t_smooth = t * t * (3.0 - 2.0 * t) # Lerp eyes to target position self.current_left_eye_pos = int(lerp(self.eye_start_pos, self.eye_target_pos, t_smooth)) self.current_right_eye_pos = self.current_left_eye_pos print(f"[DEBUG] Lerping: t={t:.3f}, pos={self.current_left_eye_pos} (from {self.eye_start_pos} to {self.eye_target_pos})") # Update display self.left_eye_label.config(text=str(self.current_left_eye_pos)) self.right_eye_label.config(text=str(self.current_right_eye_pos)) # Send motor updates self.send_motor_updates() # Schedule next update (60 FPS) self.root.after(16, self.update_animation) def send_motor_updates(self): """Send motor position updates if connected and throttled""" if not SERIAL_AVAILABLE or not self.serial_connection or not self.serial_connection.is_open: print(f"[DEBUG] Not sending - connection: {SERIAL_AVAILABLE and self.serial_connection is not None}") return current_time = time.time() time_since_last_send = current_time - self.last_send_time if time_since_last_send >= self.min_send_interval: try: motor_positions = [ (LEFT_EYE_ID, self.current_left_eye_pos), (RIGHT_EYE_ID, self.current_right_eye_pos) ] print(f"[DEBUG] Sending: L={self.current_left_eye_pos}, R={self.current_right_eye_pos}, interval={time_since_last_send*1000:.1f}ms") send_motor_positions(self.serial_connection, motor_positions) self.last_send_time = current_time except Exception as e: print(f"Error sending motor positions: {e}") self.status_label.config(text=f"Send error: {str(e)}", foreground="red") else: print(f"[DEBUG] Throttled - only {time_since_last_send*1000:.1f}ms since last send") if __name__ == "__main__": root = tk.Tk() app = EyeControlGUI(root) root.mainloop()