face_tracking_PC_Code.py

import cv2
import serial
import serial.tools.list_ports
import time
from collections import deque

# === Pico connection ===
pico_port = None
ports = serial.tools.list_ports.comports()
for port in ports:
    if "Pico" in port.description or "USB Serial" in port.description:
        pico_port = port.device
        break

if pico_port is None:
    print("❌ Raspberry Pi Pico not found.")
    exit()

print(f"✅ Connecting to Pico on {pico_port}")
try:
    pico = serial.Serial(pico_port, 115200, timeout=1)
    time.sleep(0.5)
    print("✅ Connected to Pico successfully")
except Exception as e:
    print("❌ Could not connect to Pico:", e)
    exit()

# === Face detection ===
print("🎥 Initializing webcam...")
face_cascade = cv2.CascadeClassifier(cv2.data.haarcascades + "haarcascade_frontalface_default.xml")
cap = cv2.VideoCapture(0)
if not cap.isOpened():
    print("❌ Webcam not found!")
    exit()
print("✅ Webcam started successfully")

frame_width = int(cap.get(3))
frame_height = int(cap.get(4))
center_x = frame_width // 2

angle = 0
target_angle = 0
smooth_factor = 0.02     # ultra smooth
deadzone = 25
update_interval = 0.05   # slower update for smoothness

last_send_time = time.time()
last_face_center = None

# Moving average filter for face position
face_positions = deque(maxlen=10)  # more samples = smoother

print("🤖 Ultra Smooth Single‑Face Tracker running. Press 'Q' to quit.")

while True:
    ret, frame = cap.read()
    if not ret:
        print("⚠️ Failed to grab frame.")
        break

    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    faces = face_cascade.detectMultiScale(gray, 1.3, 5)

    if len(faces) > 0:
        (x, y, w, h) = faces[0]  # Only first face
        face_center = x + w // 2
        face_positions.append(face_center)

        # Smooth face center with moving average
        avg_face_center = sum(face_positions) / len(face_positions)

        offset = avg_face_center - center_x

        # Predictive velocity control
        velocity = 0
        if last_face_center is not None:
            velocity = avg_face_center - last_face_center
        last_face_center = avg_face_center

        if abs(offset) > deadzone:
            target_angle = int((-offset / center_x) * 90 + velocity * 0.3)
            target_angle = max(-90, min(90, target_angle))
        else:
            target_angle = angle
    else:
        target_angle = 0  # No face → center

    # Ultra smooth easing
    angle += (target_angle - angle) * smooth_factor

    # Send command periodically
    if time.time() - last_send_time > update_interval:
        if abs(angle - target_angle) > 0.2:
            cmd = f"ANGLE {int(angle)}\n"
            pico.write(cmd.encode())
            last_send_time = time.time()

    # Visualization
    if len(faces) > 0:
        (x, y, w, h) = faces[0]
        cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2)
    cv2.line(frame, (center_x, 0), (center_x, frame_height), (255, 0, 0), 2)
    cv2.putText(frame, f"Angle: {int(angle)}", (10, 30),
                cv2.FONT_HERSHEY_SIMPLEX, 1, (0,255,0), 2)

    if len(faces) == 0:
        cv2.putText(frame, "No Face Detected", (10, 60),
                    cv2.FONT_HERSHEY_SIMPLEX, 1, (0,0,255), 2)

    cv2.imshow("🤖 Ultra Smooth Single Face Tracker", frame)

    if cv2.waitKey(1) & 0xFF == ord("q"):
        print("🛑 Quitting tracker...")
        break

cap.release()
pico.close()
cv2.destroyAllWindows()
print("✅ Tracker stopped.")