diff --git a/behaviors.cpp b/behaviors.cpp index 652cddf..15ad3e5 100644 --- a/behaviors.cpp +++ b/behaviors.cpp @@ -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); } + + // Convert to int16_t first to handle negative values, then clamp + int16_t position = (int16_t)positionFloat; - // Clamp to valid range - if (position < EYE_POSITION_MIN) position = EYE_POSITION_MIN; - if (position > EYE_POSITION_MAX) position = EYE_POSITION_MAX; + // 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 position; + return (uint16_t)position; } uint16_t FocusBehavior::calculateNeckPositionFromRadarAngle(float radarAngle) { diff --git a/behaviors.h b/behaviors.h index c5ea6a4..dc4afca 100644 --- a/behaviors.h +++ b/behaviors.h @@ -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) diff --git a/focus.py b/focus.py new file mode 100644 index 0000000..4c7abee --- /dev/null +++ b/focus.py @@ -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()