369 lines
14 KiB
Python
369 lines
14 KiB
Python
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()
|