eye tracking more or less works, no neck movement and not calibrated
parent
7e3218afb6
commit
914dc97eba
|
|
@ -63,9 +63,11 @@ bool FocusBehavior::update() {
|
|||
targetDetectedTime = 0;
|
||||
neckRotating = false;
|
||||
|
||||
// Smoothly interpolate to center
|
||||
// Smoothly interpolate eyes to center
|
||||
eyePosition = lerp(eyePosition, EYE_POSITION_CENTER, INTERPOLATION_SPEED);
|
||||
neckPosition = lerp(neckPosition, NECK_POSITION_CENTER, INTERPOLATION_SPEED);
|
||||
|
||||
// Keep neck at center (no movement)
|
||||
neckPosition = NECK_POSITION_CENTER;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
|
@ -85,50 +87,19 @@ bool FocusBehavior::update() {
|
|||
// Active tracking - calculate target positions from radar angle
|
||||
isActive = true;
|
||||
uint16_t targetEyePos = calculateEyePositionFromRadarAngle(target.angle);
|
||||
uint16_t targetNeckPos = calculateNeckPositionFromRadarAngle(target.angle);
|
||||
|
||||
// Eyes track immediately
|
||||
targetEyePosition = targetEyePos;
|
||||
|
||||
// Check if this is a new target (angle changed significantly or first detection)
|
||||
bool newTarget = (targetDetectedTime == 0) ||
|
||||
(abs((int16_t)targetEyePosition - (int16_t)eyePosition) > 50);
|
||||
// Neck disabled for now - keep it centered
|
||||
targetNeckPosition = NECK_POSITION_CENTER;
|
||||
neckRotating = false;
|
||||
|
||||
if (newTarget) {
|
||||
// Reset timing for new target
|
||||
targetDetectedTime = now;
|
||||
neckStartTime = now + NECK_DELAY_MS;
|
||||
neckRotating = false;
|
||||
}
|
||||
|
||||
// Check if neck should start rotating
|
||||
if (!neckRotating && now >= neckStartTime && targetDetectedTime > 0) {
|
||||
neckRotating = true;
|
||||
targetNeckPosition = targetNeckPos;
|
||||
}
|
||||
|
||||
// If neck is rotating, gradually center the eyes as neck approaches target
|
||||
if (neckRotating) {
|
||||
// Neck tracks the target
|
||||
targetNeckPosition = targetNeckPos;
|
||||
|
||||
// Calculate how far the neck has moved toward its target (0 = just started, 1 = at target)
|
||||
int16_t neckDistanceToTarget = abs((int16_t)targetNeckPosition - (int16_t)neckPosition);
|
||||
int16_t totalNeckRange = NECK_POSITION_MAX - NECK_POSITION_MIN;
|
||||
float neckProgress = 1.0f - ((float)neckDistanceToTarget / (float)totalNeckRange);
|
||||
neckProgress = (neckProgress < 0.0f) ? 0.0f : (neckProgress > 1.0f) ? 1.0f : neckProgress;
|
||||
|
||||
// As neck gets closer to target, eyes gradually center
|
||||
// Interpolate eye target between current target position and center based on neck progress
|
||||
float eyeTargetBlend = 1.0f - neckProgress; // 1.0 = at target angle, 0.0 = centered
|
||||
int16_t eyeTargetOffset = (int16_t)targetEyePos - (int16_t)EYE_POSITION_CENTER;
|
||||
targetEyePosition = EYE_POSITION_CENTER + (uint16_t)(eyeTargetOffset * eyeTargetBlend);
|
||||
}
|
||||
|
||||
// Smoothly interpolate current positions toward targets
|
||||
// Use different speeds for eyes and neck (neck is slower/smoother)
|
||||
// Smoothly interpolate eye position toward target
|
||||
eyePosition = lerp(eyePosition, targetEyePosition, INTERPOLATION_SPEED);
|
||||
neckPosition = lerp(neckPosition, targetNeckPosition, NECK_INTERPOLATION_SPEED);
|
||||
|
||||
// Keep neck at center (no movement)
|
||||
neckPosition = NECK_POSITION_CENTER;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
|
@ -151,37 +122,41 @@ bool FocusBehavior::getMotorPosition(uint8_t motorID, uint16_t& position) {
|
|||
|
||||
uint16_t FocusBehavior::calculateEyePositionFromRadarAngle(float radarAngle) {
|
||||
// Calculate eye motor position from radar angle (in degrees)
|
||||
// Angle range: approximately -45 to +45 degrees (typical radar FOV)
|
||||
// Map to eye motor position range: 1700 to 2300 (center 2047)
|
||||
// Angle range: -50 to +50 degrees, mapped to full eye range (1700-2500, center 2200)
|
||||
|
||||
// Clamp angle to reasonable range (can extend later if needed)
|
||||
constexpr float ANGLE_MIN = -45.0f;
|
||||
constexpr float ANGLE_MAX = 45.0f;
|
||||
constexpr float ANGLE_MIN = -50.0f;
|
||||
constexpr float ANGLE_MAX = 50.0f;
|
||||
|
||||
// Clamp angle to -50 to +50 range
|
||||
if (radarAngle < ANGLE_MIN) radarAngle = ANGLE_MIN;
|
||||
if (radarAngle > ANGLE_MAX) radarAngle = ANGLE_MAX;
|
||||
|
||||
// Normalize angle to -1.0 to 1.0 range
|
||||
float normalizedAngle = radarAngle / ANGLE_MAX;
|
||||
float normalized = radarAngle / 50.0f;
|
||||
|
||||
// Calculate range from center
|
||||
float rangeLeft = EYE_POSITION_CENTER - EYE_POSITION_MIN; // 347
|
||||
float rangeRight = EYE_POSITION_MAX - EYE_POSITION_CENTER; // 253
|
||||
// Calculate range from center in each direction
|
||||
// Left range: 2200 - 1700 = 500, Right range: 2500 - 2200 = 300
|
||||
float rangeLeft = (float)(EYE_POSITION_CENTER - EYE_POSITION_MIN); // 500
|
||||
float rangeRight = (float)(EYE_POSITION_MAX - EYE_POSITION_CENTER); // 300
|
||||
|
||||
uint16_t position;
|
||||
if (normalizedAngle < 0.0f) {
|
||||
// Left side: use left range
|
||||
position = EYE_POSITION_CENTER + (uint16_t)(normalizedAngle * rangeLeft);
|
||||
// Use different ranges for left (negative) and right (positive) to use full range
|
||||
float positionFloat;
|
||||
if (normalized < 0.0f) {
|
||||
// Negative angle: use left range (500 units)
|
||||
positionFloat = (float)EYE_POSITION_CENTER + (normalized * rangeLeft);
|
||||
} else {
|
||||
// Right side: use right range
|
||||
position = EYE_POSITION_CENTER + (uint16_t)(normalizedAngle * rangeRight);
|
||||
// Positive angle: use right range (300 units)
|
||||
positionFloat = (float)EYE_POSITION_CENTER + (normalized * rangeRight);
|
||||
}
|
||||
|
||||
// Clamp to valid range
|
||||
if (position < EYE_POSITION_MIN) position = EYE_POSITION_MIN;
|
||||
if (position > EYE_POSITION_MAX) position = EYE_POSITION_MAX;
|
||||
// Convert to int16_t first to handle negative values, then clamp
|
||||
int16_t position = (int16_t)positionFloat;
|
||||
|
||||
return position;
|
||||
// Clamp to valid range (1700 to 2500)
|
||||
if (position < (int16_t)EYE_POSITION_MIN) position = (int16_t)EYE_POSITION_MIN;
|
||||
if (position > (int16_t)EYE_POSITION_MAX) position = (int16_t)EYE_POSITION_MAX;
|
||||
|
||||
return (uint16_t)position;
|
||||
}
|
||||
|
||||
uint16_t FocusBehavior::calculateNeckPositionFromRadarAngle(float radarAngle) {
|
||||
|
|
|
|||
|
|
@ -80,8 +80,8 @@ private:
|
|||
static constexpr uint8_t NECK_MOTOR = 27; // Neck motor
|
||||
|
||||
// Eye motor position range (motors 14 and 15)
|
||||
static constexpr uint16_t EYE_POSITION_CENTER = 2047;
|
||||
static constexpr uint16_t EYE_POSITION_MIN = 1600;
|
||||
static constexpr uint16_t EYE_POSITION_CENTER = 2200;
|
||||
static constexpr uint16_t EYE_POSITION_MIN = 1700;
|
||||
static constexpr uint16_t EYE_POSITION_MAX = 2500;
|
||||
|
||||
// Neck motor position range (motor 27)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,368 @@
|
|||
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()
|
||||
Loading…
Reference in New Issue