My CozControl (Updated 4/14)
python · 21 views · 4 unique · 2026-04-13 05:26:02 · raw
#!/usr/bin/env python3
"""
Cozmo Control Script with HoboStreamer Integration
Full custom faces, smooth/jumpy modes, machine gun, previews, battery monitor.
Viewer buttons work great.
"""
import pycozmo
import pygame
import cv2
import time
import numpy as np
import os
import random
from PIL import Image
import json
import threading
from queue import Queue
import websocket
# ========================== TUNABLE SETTINGS ==========================
JUMPY_BURST_DURATION = 0.07
JUMPY_PAUSE_TIME = 0.03
HEARTBEAT_INTERVAL = 7.0
FACE_COOLDOWN = 0.35
MODE_COOLDOWN = 0.25
GEAR_COOLDOWN = 0.4
THROTTLE_LOW_THRESHOLD = -0.85
MG_PULSES = 12
MG_INTERVAL = 0.06
MG_VIBRATE = 18
STICK_DEADZONE = 0.08
# =====================================================================
pygame.init()
pygame.joystick.init()
screen = pygame.display.set_mode((400, 280))
pygame.display.set_caption("Cozmo Control - ESC to quit")
print("Connecting to Cozmo...")
joystick = None
if pygame.joystick.get_count() > 0:
joystick = pygame.joystick.Joystick(0)
joystick.init()
print(f"✅ Joystick detected: {joystick.get_name()}")
else:
print("⚠️ No joystick found — keyboard only")
latest_image = None
ir_light_on = False
last_heartbeat = 0
last_battery_check = 0
battery_voltage = 0.0
battery_percent = 0
last_face_time = 0
last_mode_time = 0
last_gear_time = 0
mirror_mode = False
last_face_image = np.zeros((32, 128), dtype=np.uint8)
def on_camera_image(cli, image):
global latest_image
latest_image = image.copy()
def send_heartbeat(cli, head_angle):
global last_heartbeat
now = time.time()
if now - last_heartbeat > HEARTBEAT_INTERVAL:
cli.set_head_angle(head_angle + 0.01)
time.sleep(0.05)
cli.set_head_angle(head_angle)
last_heartbeat = now
driving_mode = "smooth"
# === CUSTOM FACES + OTTER + MECHAMG ===
custom_faces_dir = os.path.expanduser("~/Desktop/custom_faces")
static_faces = []
face_filenames = []
armcat_up = armcat_down = hit_left = hit_right = jL = jR = None
nflag_frames = []
otter_frames = []
# mechaMG
mechaMG_frames = []
mechaMG_delays = []
mechaMG_folder = os.path.expanduser("~/Desktop/mechaMG")
if os.path.exists(mechaMG_folder):
mg_files = sorted([f for f in os.listdir(mechaMG_folder) if f.lower().startswith("frame_") and f.lower().endswith(".png")])
for filename in mg_files:
path = os.path.join(mechaMG_folder, filename)
try:
im = Image.open(path).convert("1")
mg_resized = im.resize((64, 48), Image.NEAREST)
frame = Image.new("1", (128, 32), color=0)
offset_x = (128 - 64) // 2
offset_y = (32 - 48) // 2 + 4
frame.paste(mg_resized, (offset_x, offset_y))
mechaMG_frames.append(frame)
if "_delay-" in filename.lower():
try:
delay_str = filename.lower().split("_delay-")[1].split("s.")[0]
delay = float(delay_str)
except:
delay = 0.2
else:
delay = 0.2
mechaMG_delays.append(delay)
print(f"✅ Loaded mechaMG frame: {filename} (delay {delay}s)")
except Exception as e:
print(f"⚠️ Could not load mechaMG {filename}: {e}")
else:
print("⚠️ mechaMG folder not found on Desktop!")
# Otter GIF
otter_folder = os.path.expanduser("~/Desktop/otterGIF")
if os.path.exists(otter_folder):
for i in range(10):
filename = f"frame_{i:02d}_delay-0.04s.png"
path = os.path.join(otter_folder, filename)
try:
im = Image.open(path).convert("1")
frame = Image.new("1", (128, 32), color=0)
otter_resized = im.resize((64, 64), Image.NEAREST)
offset_x = (128 - 64) // 2
offset_y = max(0, (32 - 64) // 2)
frame.paste(otter_resized, (offset_x, offset_y))
otter_frames.append(frame)
except Exception as e:
print(f"⚠️ Could not load otter frame {i:02d}: {e}")
if os.path.exists(custom_faces_dir):
bmp_files = [f for f in os.listdir(custom_faces_dir) if f.lower().endswith('.bmp')]
for f in sorted(bmp_files):
path = os.path.join(custom_faces_dir, f)
try:
im = Image.open(path).resize((128, 32), Image.NEAREST).convert("1")
fname_lower = f.lower()
if fname_lower == "armcatup.bmp": armcat_up = im
elif fname_lower == "armcatdown.bmp": armcat_down = im
elif fname_lower in ("hitl.bmp", "hit1.bmp", "left.bmp"): hit_left = im
elif fname_lower in ("hitr.bmp", "hit2.bmp", "right.bmp"): hit_right = im
elif fname_lower == "jl.bmp": jL = im
elif fname_lower == "jr.bmp": jR = im
elif fname_lower == "nflag1.1.bmp":
for angle in range(0, 360, 45):
rotated = im.rotate(angle, expand=False, fillcolor=0)
nflag_frames.append(rotated)
else:
static_faces.append(im)
face_filenames.append(f)
except Exception as e:
print(f"⚠️ Could not load {f}: {e}")
current_face_idx = -1
animation_mode = None
last_frame_time = 0
current_armcat_frame = current_hit_frame = current_j_frame = current_nflag_frame = 0
current_otter_frame = current_dual_otter_frame = 0
current_mechaMG_frame = 0
prev_left = prev_right = 0
procedural_enabled = False
gears = [("FAST", 140), ("MEDIUM", 88), ("SLOW", 55)]
gear_index = 0
def set_procedural_face(cli, enabled: bool):
global procedural_enabled
cli.enable_procedural_face(enabled)
procedural_enabled = enabled
print("👀 Default procedural eyes " + ("ENABLED" if enabled else "DISABLED"))
# ========================== HOBOSTREAMER INTEGRATION ==========================
# STREAM_KEY = "YOUR_REAL_KEY_HERE" # Keep private when sharing
WS_URL = "wss://hobostreamer.com/ws/control?mode=hardware&stream_key=YOUR_REAL_KEY_HERE"
remote_command_queue = Queue(maxsize=30)
def on_hobo_message(ws, raw_msg):
try:
msg = json.loads(raw_msg)
t = msg.get("type")
if t == "connected":
print("✅ HoboStreamer hardware bridge connected!")
try:
ws.send(json.dumps({"type": "status", "status": "ready", "robot": "cozmo"}))
except:
pass
return
if t in ("command", "key_down", "key_up", "video_click"):
remote_command_queue.put(msg)
except Exception as e:
print(f"WS parse error: {e}")
def hobo_websocket_thread():
while True:
try:
ws = websocket.WebSocketApp(
WS_URL,
on_message=on_hobo_message,
on_error=lambda ws, err: print(f"WS error: {err}"),
on_close=lambda ws, *args: print("WS closed - reconnecting in 5s...")
)
ws.run_forever(ping_interval=25, ping_timeout=10)
except Exception as e:
print(f"WS thread crashed: {e}")
time.sleep(5)
ws_thread = threading.Thread(target=hobo_websocket_thread, daemon=True)
ws_thread.start()
print("🚀 HoboStreamer WebSocket thread started...")
with pycozmo.connect(enable_procedural_face=False) as cli:
cli.load_anims()
cli.enable_camera(enable=True, color=True)
cli.set_head_light(enable=ir_light_on)
cli.add_handler(pycozmo.event.EvtNewRawCameraImage, on_camera_image)
set_procedural_face(cli, False)
print("🎉 Cozmo is ready for maximum stream chaos!")
print("=== CONTROLS ===")
print("WASD/Arrows : Drive/Turn (local)")
print("Q/E : Head Up/Down (when throttle lowest)")
print("R/F : Gradual Lift")
print("U/I : Instant lift slam")
print("L : Toggle IR Light")
print("Z : Cycle gear")
print("X : Toggle Smooth ↔ Jumpy")
print("G/Y : Otter / Dual Otter")
print("H : Random glance")
print("J/K/N : J / ArmCat / NFlag")
print("M : Static faces")
print("O : Toggle eyes")
print("P : mechaMG")
print("Space : Emergency Stop")
print("ESC : Quit")
cv2.namedWindow("Cozmo Eye View", cv2.WINDOW_NORMAL)
cv2.namedWindow("Cozmo Face Preview", cv2.WINDOW_NORMAL)
cv2.namedWindow("Cozmo Battery", cv2.WINDOW_NORMAL)
clock = pygame.time.Clock()
drive_speed = gears[gear_index][1]
head_angle = 0.0
lift_height_mm = pycozmo.MIN_LIFT_HEIGHT.mm
running = True
# ====================== HOBOSTREAMER COMMAND HANDLER ======================
def handle_remote_command(msg):
global driving_mode, animation_mode, last_face_time, last_mode_time, lift_height_mm, head_angle
global current_mechaMG_frame, current_otter_frame, current_dual_otter_frame
global current_armcat_frame, current_j_frame, current_nflag_frame, current_hit_frame
cmd = msg.get("command")
msg_type = msg.get("type")
now = time.time()
if not cmd:
return
print(f"📡 Viewer command: {msg_type} → {cmd}")
# Face / Animation commands
if cmd in ("mechaMG", "p"):
if animation_mode == "mechaMG":
animation_mode = None
elif mechaMG_frames:
set_procedural_face(cli, False)
animation_mode = "mechaMG"
current_mechaMG_frame = 0
last_face_time = now
elif cmd in ("otter", "g"):
animation_mode = "otter" if animation_mode != "otter" else None
if animation_mode == "otter":
current_otter_frame = 0
last_frame_time = now
elif cmd in ("dual_otter", "y"):
animation_mode = "dual_otter" if animation_mode != "dual_otter" else None
if animation_mode == "dual_otter":
current_dual_otter_frame = 0
last_frame_time = now
elif cmd in ("armcat", "k"):
animation_mode = "armcat" if animation_mode != "armcat" else None
if animation_mode == "armcat":
current_armcat_frame = 0
last_frame_time = now
elif cmd in ("j_animation", "j"):
animation_mode = "j" if animation_mode != "j" else None
if animation_mode == "j":
current_j_frame = 0
last_frame_time = now
elif cmd in ("nflag", "n"):
animation_mode = "nflag" if animation_mode != "nflag" else None
if animation_mode == "nflag":
current_nflag_frame = 0
last_frame_time = now
elif cmd in ("random_glance", "h"):
animation_mode = "hit" if animation_mode != "hit" else None
if animation_mode == "hit":
current_hit_frame = random.choice([0, 1])
last_frame_time = now
elif cmd in ("toggle_eyes", "o"):
new_state = not procedural_enabled
set_procedural_face(cli, new_state)
if new_state:
animation_mode = None
elif cmd in ("machine_gun", "mg"):
if now - last_face_time > 0.5:
current_lift = lift_height_mm
for _ in range(MG_PULSES):
cli.set_lift_height(min(pycozmo.MAX_LIFT_HEIGHT.mm, current_lift + MG_VIBRATE), accel=3000, duration=0.04)
time.sleep(MG_INTERVAL)
cli.set_lift_height(max(pycozmo.MIN_LIFT_HEIGHT.mm, current_lift - MG_VIBRATE), accel=3000, duration=0.04)
time.sleep(MG_INTERVAL)
cli.set_lift_height(current_lift, accel=800, duration=0.12)
last_face_time = now
elif cmd in ("toggle_mode", "x"):
if now - last_mode_time > MODE_COOLDOWN:
driving_mode = "jumpy" if driving_mode == "smooth" else "smooth"
print(f"🔄 Viewer switched mode → {driving_mode.upper()}")
last_mode_time = now
elif cmd in ("emergency_stop", "space", "stop"):
cli.drive_wheels(0, 0)
# Drive commands - single controlled burst (works great in jumpy, short roll in smooth)
elif cmd == "forward":
speed = drive_speed
if driving_mode == "smooth":
cli.drive_wheels(lwheel_speed=speed, rwheel_speed=speed, duration=0.45)
else:
cli.drive_wheels(lwheel_speed=speed, rwheel_speed=speed, duration=JUMPY_BURST_DURATION)
time.sleep(JUMPY_PAUSE_TIME)
elif cmd == "backward":
speed = -drive_speed
if driving_mode == "smooth":
cli.drive_wheels(lwheel_speed=speed, rwheel_speed=speed, duration=0.45)
else:
cli.drive_wheels(lwheel_speed=speed, rwheel_speed=speed, duration=JUMPY_BURST_DURATION)
time.sleep(JUMPY_PAUSE_TIME)
elif cmd == "turn_left":
speed = int(drive_speed * 0.7)
if driving_mode == "smooth":
cli.drive_wheels(lwheel_speed=-speed, rwheel_speed=speed, duration=0.35)
else:
cli.drive_wheels(lwheel_speed=-speed, rwheel_speed=speed, duration=JUMPY_BURST_DURATION)
time.sleep(JUMPY_PAUSE_TIME)
elif cmd == "turn_right":
speed = int(drive_speed * 0.7)
if driving_mode == "smooth":
cli.drive_wheels(lwheel_speed=speed, rwheel_speed=-speed, duration=0.35)
else:
cli.drive_wheels(lwheel_speed=speed, rwheel_speed=-speed, duration=JUMPY_BURST_DURATION)
time.sleep(JUMPY_PAUSE_TIME)
last_face_time = max(last_face_time, now - FACE_COOLDOWN + 0.05)
# =========================================================================
while running:
for event in pygame.event.get():
if event.type == pygame.QUIT or (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE):
running = False
pygame.event.pump()
keys = pygame.key.get_pressed()
now = time.time()
# Process viewer commands from HoboStreamer
while not remote_command_queue.empty():
msg = remote_command_queue.get_nowait()
handle_remote_command(msg)
# Gear cycle (Z key)
if keys[pygame.K_z] and now - last_gear_time > GEAR_COOLDOWN:
gear_index = (gear_index + 1) % len(gears)
drive_speed = gears[gear_index][1]
print(f"🔧 Gear: {gears[gear_index][0]}")
last_gear_time = now
# ====================== HEAD CONTROL ======================
if joystick:
throttle = joystick.get_axis(3)
if throttle < THROTTLE_LOW_THRESHOLD:
if keys[pygame.K_q]:
head_angle = min(pycozmo.MAX_HEAD_ANGLE.radians, head_angle + 0.15)
if keys[pygame.K_e]:
head_angle = max(pycozmo.MIN_HEAD_ANGLE.radians, head_angle - 0.15)
else:
head_angle = pycozmo.MAX_HEAD_ANGLE.radians - (throttle + 1.0) * 0.5 * \
(pycozmo.MAX_HEAD_ANGLE.radians - pycozmo.MIN_HEAD_ANGLE.radians)
cli.set_head_angle(head_angle)
# ====================== DRIVE (local) ======================
left = right = 0
if joystick:
y = joystick.get_axis(1)
twist = joystick.get_axis(2)
if abs(y) < STICK_DEADZONE: y = 0
if abs(twist) < STICK_DEADZONE: twist = 0
forward = -y * drive_speed
turn = twist * drive_speed
if mirror_mode:
turn = -turn
left = forward - turn
right = forward + turn
if keys[pygame.K_w] or keys[pygame.K_UP]: left = right = drive_speed
elif keys[pygame.K_s] or keys[pygame.K_DOWN]: left = right = -drive_speed
if keys[pygame.K_a] or keys[pygame.K_LEFT]: left = -drive_speed; right = drive_speed
elif keys[pygame.K_d] or keys[pygame.K_RIGHT]: left = drive_speed; right = -drive_speed
if driving_mode == "smooth":
if abs(left) > 5 or abs(right) > 5:
cli.drive_wheels(lwheel_speed=left, rwheel_speed=right)
elif prev_left != 0 or prev_right != 0:
cli.drive_wheels(0, 0)
else:
if abs(left) > 5 or abs(right) > 5:
cli.drive_wheels(lwheel_speed=left, rwheel_speed=right, duration=JUMPY_BURST_DURATION)
time.sleep(JUMPY_PAUSE_TIME)
prev_left = left
prev_right = right
if keys[pygame.K_SPACE]:
cli.drive_wheels(0, 0)
# Mirror
if keys[pygame.K_b]:
mirror_mode = not mirror_mode
print(f"🪞 Mirror Mode: {'ON' if mirror_mode else 'OFF'}")
time.sleep(0.3)
# Lift
if joystick:
hat = joystick.get_hat(0)
if hat[1] == 1:
lift_height_mm = min(pycozmo.MAX_LIFT_HEIGHT.mm, lift_height_mm + 4)
cli.set_lift_height(lift_height_mm)
elif hat[1] == -1:
lift_height_mm = max(pycozmo.MIN_LIFT_HEIGHT.mm, lift_height_mm - 4)
cli.set_lift_height(lift_height_mm)
if keys[pygame.K_r]:
lift_height_mm = min(pycozmo.MAX_LIFT_HEIGHT.mm, lift_height_mm + 4)
cli.set_lift_height(lift_height_mm)
if keys[pygame.K_f]:
lift_height_mm = max(pycozmo.MIN_LIFT_HEIGHT.mm, lift_height_mm - 4)
cli.set_lift_height(lift_height_mm)
if keys[pygame.K_u]:
cli.set_lift_height(pycozmo.MAX_LIFT_HEIGHT.mm, accel=1200, duration=0.18)
print("🚀 Manual Slam UP!")
time.sleep(0.2)
if keys[pygame.K_i]:
cli.set_lift_height(pycozmo.MIN_LIFT_HEIGHT.mm, accel=1200, duration=0.18)
print("🔽 Manual Slam DOWN!")
time.sleep(0.2)
# Machine Gun (local joystick button)
if joystick and joystick.get_button(0) and now - last_face_time > 0.5:
print("🔫 MACHINE GUN FIRING!")
current_lift = lift_height_mm
for _ in range(MG_PULSES):
cli.set_lift_height(min(pycozmo.MAX_LIFT_HEIGHT.mm, current_lift + MG_VIBRATE), accel=3000, duration=0.04)
time.sleep(MG_INTERVAL)
cli.set_lift_height(max(pycozmo.MIN_LIFT_HEIGHT.mm, current_lift - MG_VIBRATE), accel=3000, duration=0.04)
time.sleep(MG_INTERVAL)
cli.set_lift_height(current_lift, accel=800, duration=0.12)
last_face_time = now
# IR Light
if keys[pygame.K_l]:
ir_light_on = not ir_light_on
cli.set_head_light(enable=ir_light_on)
print(f"💡 IR Light: {'ON' if ir_light_on else 'OFF'}")
time.sleep(0.2)
# X: Drive mode toggle
if keys[pygame.K_x]:
if now - last_mode_time > MODE_COOLDOWN:
driving_mode = "jumpy" if driving_mode == "smooth" else "smooth"
print(f"🔄 Driving mode → {driving_mode.upper()}")
last_mode_time = now
# Face commands (local keys)
if now - last_face_time > FACE_COOLDOWN:
if keys[pygame.K_m] and static_faces:
current_face_idx = (current_face_idx + 1) % len(static_faces)
set_procedural_face(cli, False)
current_face = static_faces[current_face_idx]
cli.display_image(current_face)
animation_mode = None
print(f"🖼️ Static Face: {face_filenames[current_face_idx]}")
last_face_time = now
last_face_image = np.array(current_face).astype(np.uint8) * 255
if keys[pygame.K_g]:
animation_mode = "otter" if animation_mode != "otter" else None
print("🦦 Single Otter " + ("ON" if animation_mode == "otter" else "OFF"))
if animation_mode == "otter":
current_otter_frame = 0
last_frame_time = time.time()
last_face_time = now
if keys[pygame.K_y]:
animation_mode = "dual_otter" if animation_mode != "dual_otter" else None
print("🦦🦦 Dual Otters " + ("ON" if animation_mode == "dual_otter" else "OFF"))
if animation_mode == "dual_otter":
current_dual_otter_frame = 0
last_frame_time = time.time()
last_face_time = now
if keys[pygame.K_h]:
animation_mode = "hit" if animation_mode != "hit" else None
print("👀 Random Glance " + ("ON" if animation_mode == "hit" else "OFF"))
if animation_mode == "hit":
current_hit_frame = random.choice([0, 1])
last_frame_time = time.time()
last_face_time = now
if keys[pygame.K_k]:
animation_mode = "armcat" if animation_mode != "armcat" else None
print("🐱 ArmCat " + ("ON" if animation_mode == "armcat" else "OFF"))
if animation_mode == "armcat":
current_armcat_frame = 0
last_frame_time = time.time()
last_face_time = now
if keys[pygame.K_j]:
animation_mode = "j" if animation_mode != "j" else None
print("🃏 J Animation " + ("ON" if animation_mode == "j" else "OFF"))
if animation_mode == "j":
current_j_frame = 0
last_frame_time = time.time()
last_face_time = now
if keys[pygame.K_n]:
animation_mode = "nflag" if animation_mode != "nflag" else None
print("🏳️ NFlag Spinning " + ("ON" if animation_mode == "nflag" else "OFF"))
if animation_mode == "nflag":
current_nflag_frame = 0
last_frame_time = time.time()
last_face_time = now
if keys[pygame.K_o]:
new_state = not procedural_enabled
set_procedural_face(cli, new_state)
if new_state:
animation_mode = None
last_face_time = now
if keys[pygame.K_p]:
if animation_mode == "mechaMG":
animation_mode = None
print("🤖 mechaMG stopped")
elif mechaMG_frames:
set_procedural_face(cli, False)
animation_mode = "mechaMG"
current_mechaMG_frame = 0
last_frame_time = time.time()
print("🤖 mechaMG activated!")
last_face_time = now
# Run Animations
if not procedural_enabled:
now_anim = time.time()
if animation_mode == "armcat" and armcat_up and armcat_down and now_anim - last_frame_time > 0.2:
current_armcat_frame = 1 - current_armcat_frame
current_face = armcat_up if current_armcat_frame == 0 else armcat_down
cli.display_image(current_face)
last_frame_time = now_anim
last_face_image = np.array(current_face).astype(np.uint8) * 255
elif animation_mode == "hit" and hit_left and hit_right and now_anim - last_frame_time > 0:
delay = random.uniform(0.2, 0.5) if random.random() < 0.05 else random.uniform(2.0, 4.0)
if now_anim - last_frame_time > delay:
current_hit_frame = 1 - current_hit_frame
current_face = hit_left if current_hit_frame == 0 else hit_right
cli.display_image(current_face)
last_frame_time = now_anim
last_face_image = np.array(current_face).astype(np.uint8) * 255
elif animation_mode == "j" and jL and jR and now_anim - last_frame_time > 1.0:
current_j_frame = 1 - current_j_frame
current_face = jL if current_j_frame == 0 else jR
cli.display_image(current_face)
last_frame_time = now_anim
last_face_image = np.array(current_face).astype(np.uint8) * 255
elif animation_mode == "nflag" and nflag_frames and now_anim - last_frame_time > 0.08:
current_nflag_frame = (current_nflag_frame + 1) % len(nflag_frames)
current_face = nflag_frames[current_nflag_frame]
cli.display_image(current_face)
last_frame_time = now_anim
last_face_image = np.array(current_face).astype(np.uint8) * 255
elif animation_mode == "otter" and otter_frames and now_anim - last_frame_time > 0.04:
current_otter_frame = (current_otter_frame + 1) % len(otter_frames)
current_face = otter_frames[current_otter_frame]
cli.display_image(current_face)
last_frame_time = now_anim
last_face_image = np.array(current_face).astype(np.uint8) * 255
elif animation_mode == "dual_otter" and otter_frames and now_anim - last_frame_time > 0.04:
current_dual_otter_frame = (current_dual_otter_frame + 1) % len(otter_frames)
combined = Image.new("1", (128, 32), color=0)
left_idx = current_dual_otter_frame % len(otter_frames)
right_idx = (current_dual_otter_frame + 3) % len(otter_frames)
combined.paste(otter_frames[left_idx].crop((32, 0, 96, 32)), (0, 0))
combined.paste(otter_frames[right_idx].crop((32, 0, 96, 32)), (64, 0))
current_face = combined
cli.display_image(current_face)
last_frame_time = now_anim
last_face_image = np.array(current_face).astype(np.uint8) * 255
elif animation_mode == "mechaMG" and mechaMG_frames:
if now_anim - last_frame_time > mechaMG_delays[current_mechaMG_frame]:
current_mechaMG_frame = (current_mechaMG_frame + 1) % len(mechaMG_frames)
current_face = mechaMG_frames[current_mechaMG_frame]
cli.display_image(current_face)
last_frame_time = now_anim
last_face_image = np.array(current_face).astype(np.uint8) * 255
# Face Preview
preview = cv2.resize(last_face_image, (256, 64), interpolation=cv2.INTER_NEAREST)
preview = cv2.cvtColor(preview, cv2.COLOR_GRAY2BGR)
cv2.imshow("Cozmo Face Preview", preview)
# Battery
if now - last_battery_check > 5.0:
try:
battery_voltage = cli.battery_voltage
battery_percent = max(0, min(100, int((battery_voltage - 3.0) * 100 / 1.2)))
except:
battery_voltage = 0.0
battery_percent = 0
last_battery_check = now
battery_img = np.zeros((170, 340, 3), dtype=np.uint8)
cv2.putText(battery_img, f"Battery: {battery_voltage:.2f} V", (15, 45),
cv2.FONT_HERSHEY_SIMPLEX, 0.8, (255, 255, 255), 2)
color = (0, 255, 0) if battery_percent > 40 else (0, 165, 255) if battery_percent > 20 else (0, 0, 255)
cv2.putText(battery_img, f"{battery_percent}%", (15, 95),
cv2.FONT_HERSHEY_SIMPLEX, 1.6, color, 3)
cv2.putText(battery_img, f"Gear: {gears[gear_index][0]}", (15, 135),
cv2.FONT_HERSHEY_SIMPLEX, 1.0, (255, 255, 100), 2)
cv2.imshow("Cozmo Battery", battery_img)
send_heartbeat(cli, head_angle)
if latest_image is not None:
img = cv2.cvtColor(np.array(latest_image), cv2.COLOR_RGB2BGR)
cv2.imshow("Cozmo Eye View", img)
cv2.waitKey(5)
time.sleep(0.01)
clock.tick(60)
print("\nStopping safely...")
cli.drive_wheels(0, 0)
cli.set_lift_height(pycozmo.MIN_LIFT_HEIGHT.mm)
cli.enable_camera(enable=False)
cli.set_head_light(enable=False)
set_procedural_face(cli, True)
cv2.destroyAllWindows()
pygame.quit()
print("Disconnected.")
Ideas for Smooth Drive on WASD (for Goosely / team)Since single button clicks work great in jumpy mode but in smooth mode the robot keeps rolling (because drive_wheels() without duration stays on), here are two clean approaches:Best short-term fix (easiest on site side)
Make the site send key_down when WASD is pressed and key_up when released.
In the Python script we can then:On key_down "forward": start driving forward continuously (loop or repeated calls)
On key_up "forward": send drive_wheels(0, 0)
This would make WASD feel exactly like holding a key locally.
Alternative (if key_down/key_up is harder right now)
On the site, make each WASD button a short burst with a cooldown (e.g. 400-600ms drive then auto-stop).
We can adjust the duration= values in the script to make single clicks feel snappier in smooth mode.
#!/usr/bin/env python3
"""
Cozmo Control Script with HoboStreamer Integration
- Full custom faces (mechaMG with per-frame delays, otter, dual otter, ArmCat, J, NFlag, random glance, static faces)
- OpenCV eye view + face preview + HUD
- Smooth vs Jumpy Spider driving modes
- Machine gun lift shake
- Local keyboard + Logitech joystick override
- HoboStreamer viewer control support
- Strong keep-alive to prevent sleeping/disconnecting
"""
import pycozmo
import pygame
import cv2
import time
import numpy as np
import os
import random
from PIL import Image
import json
import threading
from queue import Queue
import websocket
# ========================== TUNABLE SETTINGS ==========================
JUMPY_BURST_DURATION = 0.07
JUMPY_PAUSE_TIME = 0.03
HEARTBEAT_INTERVAL = 2.5 # Aggressive keep-alive
FACE_COOLDOWN = 0.35
MODE_COOLDOWN = 0.25
GEAR_COOLDOWN = 0.4
THROTTLE_LOW_THRESHOLD = -0.85
MG_PULSES = 12
MG_INTERVAL = 0.06
MG_VIBRATE = 18
STICK_DEADZONE = 0.08
# =====================================================================
pygame.init()
pygame.joystick.init()
screen = pygame.display.set_mode((400, 280))
pygame.display.set_caption("Cozmo Control - ESC to quit")
print("Connecting to Cozmo...")
joystick = None
if pygame.joystick.get_count() > 0:
joystick = pygame.joystick.Joystick(0)
joystick.init()
print(f"✅ Joystick detected: {joystick.get_name()}")
else:
print("⚠️ No joystick found — keyboard only")
latest_image = None
ir_light_on = False
last_face_time = 0
last_mode_time = 0
last_gear_time = 0
mirror_mode = False
viewer_control_enabled = True
last_face_image = np.zeros((32, 128), dtype=np.uint8) * 255
def on_camera_image(cli, image):
global latest_image
latest_image = image.copy()
# ====================== DEDICATED HEARTBEAT THREAD ======================
def heartbeat_thread(cli):
while True:
try:
head = 0.0
cli.set_head_angle(head + 0.015)
time.sleep(0.03)
cli.set_head_angle(head)
# Extra safety pulse to fight low-power mode
cli.drive_wheels(10, 10, duration=0.12)
except:
pass
time.sleep(2.5)
# =========================================================================
driving_mode = "smooth"
# === CUSTOM FACES + OTTER + MECHAMG ===
custom_faces_dir = os.path.expanduser("~/Desktop/custom_faces")
static_faces = []
face_filenames = []
armcat_up = armcat_down = hit_left = hit_right = jL = jR = None
nflag_frames = []
otter_frames = []
# mechaMG
mechaMG_frames = []
mechaMG_delays = []
mechaMG_folder = os.path.expanduser("~/Desktop/mechaMG")
if os.path.exists(mechaMG_folder):
mg_files = sorted([f for f in os.listdir(mechaMG_folder) if f.lower().startswith("frame_") and f.lower().endswith(".png")])
for filename in mg_files:
path = os.path.join(mechaMG_folder, filename)
try:
im = Image.open(path).convert("1")
mg_resized = im.resize((64, 48), Image.NEAREST)
frame = Image.new("1", (128, 32), color=0)
offset_x = (128 - 64) // 2
offset_y = (32 - 48) // 2 + 4
frame.paste(mg_resized, (offset_x, offset_y))
mechaMG_frames.append(frame)
if "_delay-" in filename.lower():
try:
delay_str = filename.lower().split("_delay-")[1].split("s.")[0]
delay = float(delay_str)
except:
delay = 0.2
else:
delay = 0.2
mechaMG_delays.append(delay)
except Exception as e:
print(f"⚠️ Could not load mechaMG {filename}: {e}")
else:
print("⚠️ mechaMG folder not found on Desktop!")
# Otter GIF
otter_folder = os.path.expanduser("~/Desktop/otterGIF")
if os.path.exists(otter_folder):
for i in range(10):
filename = f"frame_{i:02d}_delay-0.04s.png"
path = os.path.join(otter_folder, filename)
try:
im = Image.open(path).convert("1")
frame = Image.new("1", (128, 32), color=0)
otter_resized = im.resize((64, 64), Image.NEAREST)
offset_x = (128 - 64) // 2
offset_y = max(0, (32 - 64) // 2)
frame.paste(otter_resized, (offset_x, offset_y))
otter_frames.append(frame)
except Exception as e:
print(f"⚠️ Could not load otter frame {i:02d}: {e}")
if os.path.exists(custom_faces_dir):
bmp_files = [f for f in os.listdir(custom_faces_dir) if f.lower().endswith('.bmp')]
for f in sorted(bmp_files):
path = os.path.join(custom_faces_dir, f)
try:
im = Image.open(path).resize((128, 32), Image.NEAREST).convert("1")
fname_lower = f.lower()
if fname_lower == "armcatup.bmp": armcat_up = im
elif fname_lower == "armcatdown.bmp": armcat_down = im
elif fname_lower in ("hitl.bmp", "hit1.bmp", "left.bmp"): hit_left = im
elif fname_lower in ("hitr.bmp", "hit2.bmp", "right.bmp"): hit_right = im
elif fname_lower == "jl.bmp": jL = im
elif fname_lower == "jr.bmp": jR = im
elif fname_lower == "nflag1.1.bmp":
for angle in range(0, 360, 45):
rotated = im.rotate(angle, expand=False, fillcolor=0)
nflag_frames.append(rotated)
else:
static_faces.append(im)
face_filenames.append(f)
except Exception as e:
print(f"⚠️ Could not load {f}: {e}")
current_face_idx = -1
animation_mode = None
last_frame_time = 0
current_armcat_frame = current_hit_frame = current_j_frame = current_nflag_frame = 0
current_otter_frame = current_dual_otter_frame = 0
current_mechaMG_frame = 0
prev_left = prev_right = 0
procedural_enabled = False
gears = [("FAST", 140), ("MEDIUM", 88), ("SLOW", 55)]
gear_index = 0
def set_procedural_face(cli, enabled: bool):
global procedural_enabled
cli.enable_procedural_face(enabled)
procedural_enabled = enabled
print("👀 Default procedural eyes " + ("ENABLED" if enabled else "DISABLED"))
# ========================== HOBOSTREAMER INTEGRATION ==========================
# STREAM_KEY = "YOUR_REAL_KEY_HERE" # Keep private when sharing
WS_URL = "wss://hobostreamer.com/ws/control?mode=hardware&stream_key=YOUR_REAL_KEY_HERE"
remote_command_queue = Queue(maxsize=30)
def on_hobo_message(ws, raw_msg):
try:
msg = json.loads(raw_msg)
t = msg.get("type")
if t == "connected":
print("✅ HoboStreamer hardware bridge connected!")
try:
ws.send(json.dumps({"type": "status", "status": "ready", "robot": "cozmo"}))
except:
pass
return
if t in ("command", "key_down", "key_up", "video_click"):
remote_command_queue.put(msg)
except Exception as e:
print(f"WS parse error: {e}")
def hobo_websocket_thread():
while True:
try:
ws = websocket.WebSocketApp(
WS_URL,
on_message=on_hobo_message,
on_error=lambda ws, err: print(f"WS error: {err}"),
on_close=lambda ws, *args: print("WS closed - reconnecting in 5s...")
)
ws.run_forever(ping_interval=25, ping_timeout=10)
except Exception as e:
print(f"WS thread crashed: {e}")
time.sleep(5)
ws_thread = threading.Thread(target=hobo_websocket_thread, daemon=True)
ws_thread.start()
print("🚀 HoboStreamer WebSocket thread started...")
with pycozmo.connect(enable_procedural_face=False) as cli:
cli.load_anims()
cli.enable_camera(enable=True, color=True)
cli.set_head_light(enable=ir_light_on)
cli.add_handler(pycozmo.event.EvtNewRawCameraImage, on_camera_image)
set_procedural_face(cli, False)
# Start dedicated heartbeat thread
hb_thread = threading.Thread(target=heartbeat_thread, args=(cli,), daemon=True)
hb_thread.start()
print("❤️ Aggressive keep-alive thread started")
print("🎉 Cozmo is ready for maximum stream chaos!")
print("=== FULL CONTROLS ===")
print("WASD / Arrows : Drive / Turn")
print("Q / E : Head Up / Down (when throttle at bottom)")
print("R / F : Gradual Lift Up / Down")
print("U / I : Instant Lift Slam Up / Down")
print("Joystick Hat Up/Down : Gradual Lift")
print("Joystick Hat Left/Right : Slam Lift Down / Up")
print("Joystick Trigger (Button 0) : Machine Gun Lift Vibrate")
print("Joystick Button 1 : Cycle Gear")
print("Joystick Button 3 : Toggle Smooth ↔ Jumpy Mode")
print("Joystick Button 4 : Toggle Viewer Control On/Off")
print("L : Toggle IR Light")
print("Z : Cycle Gear")
print("X : Toggle Smooth (Hobo) ↔ Jumpy (Robo)")
print("C : Toggle Viewer Control On/Off")
print("G : Single Otter GIF")
print("Y : Dual Otter GIF")
print("H : Random Glance")
print("J : J Animation")
print("K : ArmCat Animation")
print("N : NFlag Spinning")
print("O : Toggle Default Eyes")
print("P : mechaMG Animation")
print("M : Cycle Static Faces")
print("Space : Emergency Stop")
print("ESC : Quit")
print("====================")
cv2.namedWindow("Cozmo Eye View", cv2.WINDOW_NORMAL)
cv2.namedWindow("Cozmo Face Preview", cv2.WINDOW_NORMAL)
cv2.namedWindow("Cozmo HUD", cv2.WINDOW_NORMAL)
clock = pygame.time.Clock()
drive_speed = gears[gear_index][1]
head_angle = 0.0
lift_height_mm = pycozmo.MIN_LIFT_HEIGHT.mm
running = True
# ====================== HOBOSTREAMER COMMAND HANDLER ======================
def handle_remote_command(msg):
global driving_mode, animation_mode, last_face_time, last_mode_time, lift_height_mm, head_angle
global current_mechaMG_frame, current_otter_frame, current_dual_otter_frame
global current_armcat_frame, current_j_frame, current_nflag_frame, current_hit_frame
if not viewer_control_enabled:
return
cmd = msg.get("command")
now = time.time()
if not cmd:
return
print(f"📡 Viewer command: {msg.get('type')} → {cmd}")
if cmd in ("mechaMG", "p"):
if animation_mode == "mechaMG":
animation_mode = None
elif mechaMG_frames:
set_procedural_face(cli, False)
animation_mode = "mechaMG"
current_mechaMG_frame = 0
last_face_time = now
elif cmd in ("otter", "g"):
animation_mode = "otter" if animation_mode != "otter" else None
if animation_mode == "otter":
current_otter_frame = 0
last_frame_time = now
elif cmd in ("dual_otter", "y"):
animation_mode = "dual_otter" if animation_mode != "dual_otter" else None
if animation_mode == "dual_otter":
current_dual_otter_frame = 0
last_frame_time = now
elif cmd in ("armcat", "k"):
animation_mode = "armcat" if animation_mode != "armcat" else None
if animation_mode == "armcat":
current_armcat_frame = 0
last_frame_time = now
elif cmd in ("j_animation", "j"):
animation_mode = "j" if animation_mode != "j" else None
if animation_mode == "j":
current_j_frame = 0
last_frame_time = now
elif cmd in ("nflag", "n"):
animation_mode = "nflag" if animation_mode != "nflag" else None
if animation_mode == "nflag":
current_nflag_frame = 0
last_frame_time = now
elif cmd in ("random_glance", "h"):
animation_mode = "hit" if animation_mode != "hit" else None
if animation_mode == "hit":
current_hit_frame = random.choice([0, 1])
last_frame_time = now
elif cmd in ("toggle_eyes", "o"):
new_state = not procedural_enabled
set_procedural_face(cli, new_state)
if new_state:
animation_mode = None
elif cmd in ("machine_gun", "mg"):
if now - last_face_time > 0.5:
current_lift = lift_height_mm
for _ in range(MG_PULSES):
cli.set_lift_height(min(pycozmo.MAX_LIFT_HEIGHT.mm, current_lift + MG_VIBRATE), accel=3000, duration=0.04)
time.sleep(MG_INTERVAL)
cli.set_lift_height(max(pycozmo.MIN_LIFT_HEIGHT.mm, current_lift - MG_VIBRATE), accel=3000, duration=0.04)
time.sleep(MG_INTERVAL)
cli.set_lift_height(current_lift, accel=800, duration=0.12)
last_face_time = now
elif cmd in ("toggle_mode", "x"):
if now - last_mode_time > MODE_COOLDOWN:
driving_mode = "jumpy" if driving_mode == "smooth" else "smooth"
print(f"🔄 Viewer switched mode → {driving_mode.upper()}")
last_mode_time = now
elif cmd in ("emergency_stop", "space", "stop"):
cli.drive_wheels(0, 0)
elif cmd == "lift_up":
lift_height_mm = min(pycozmo.MAX_LIFT_HEIGHT.mm, lift_height_mm + 35)
cli.set_lift_height(lift_height_mm)
elif cmd == "lift_down":
lift_height_mm = max(pycozmo.MIN_LIFT_HEIGHT.mm, lift_height_mm - 35)
cli.set_lift_height(lift_height_mm)
elif cmd == "head_up":
head_angle = min(pycozmo.MAX_HEAD_ANGLE.radians, head_angle + 0.25)
cli.set_head_angle(head_angle)
elif cmd == "head_down":
head_angle = max(pycozmo.MIN_HEAD_ANGLE.radians, head_angle - 0.25)
cli.set_head_angle(head_angle)
# Drive commands
elif cmd == "forward":
speed = drive_speed
if driving_mode == "smooth":
cli.drive_wheels(lwheel_speed=speed, rwheel_speed=speed, duration=0.45)
else:
cli.drive_wheels(lwheel_speed=speed, rwheel_speed=speed, duration=JUMPY_BURST_DURATION)
time.sleep(JUMPY_PAUSE_TIME)
elif cmd == "backward":
speed = -drive_speed
if driving_mode == "smooth":
cli.drive_wheels(lwheel_speed=speed, rwheel_speed=speed, duration=0.45)
else:
cli.drive_wheels(lwheel_speed=speed, rwheel_speed=speed, duration=JUMPY_BURST_DURATION)
time.sleep(JUMPY_PAUSE_TIME)
elif cmd == "turn_left":
speed = int(drive_speed * 0.7)
if driving_mode == "smooth":
cli.drive_wheels(lwheel_speed=-speed, rwheel_speed=speed, duration=0.35)
else:
cli.drive_wheels(lwheel_speed=-speed, rwheel_speed=speed, duration=JUMPY_BURST_DURATION)
time.sleep(JUMPY_PAUSE_TIME)
elif cmd == "turn_right":
speed = int(drive_speed * 0.7)
if driving_mode == "smooth":
cli.drive_wheels(lwheel_speed=speed, rwheel_speed=-speed, duration=0.35)
else:
cli.drive_wheels(lwheel_speed=speed, rwheel_speed=-speed, duration=JUMPY_BURST_DURATION)
time.sleep(JUMPY_PAUSE_TIME)
last_face_time = max(last_face_time, now - FACE_COOLDOWN + 0.05)
# =========================================================================
while running:
for event in pygame.event.get():
if event.type == pygame.QUIT or (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE):
running = False
pygame.event.pump()
keys = pygame.key.get_pressed()
now = time.time()
# Viewer control toggle (C key + button 4)
if (keys[pygame.K_c] or (joystick and joystick.get_button(4))) and now - last_face_time > 0.3:
viewer_control_enabled = not viewer_control_enabled
print(f"🔌 Viewer control {'ENABLED' if viewer_control_enabled else 'DISABLED'}")
last_face_time = now
# Mode toggle (X key + button 3)
if (keys[pygame.K_x] or (joystick and joystick.get_button(3))) and now - last_mode_time > MODE_COOLDOWN:
driving_mode = "jumpy" if driving_mode == "smooth" else "smooth"
print(f"🔄 Driving mode → {driving_mode.upper()}")
last_mode_time = now
# Process viewer commands
if viewer_control_enabled:
while not remote_command_queue.empty():
msg = remote_command_queue.get_nowait()
handle_remote_command(msg)
# Gear (Z key + thumb button 1)
if (keys[pygame.K_z] or (joystick and joystick.get_button(1))) and now - last_gear_time > GEAR_COOLDOWN:
gear_index = (gear_index + 1) % len(gears)
drive_speed = gears[gear_index][1]
print(f"🔧 Gear: {gears[gear_index][0]}")
last_gear_time = now
# Head control
if joystick:
throttle = joystick.get_axis(3)
if throttle < THROTTLE_LOW_THRESHOLD:
if keys[pygame.K_q]:
head_angle = min(pycozmo.MAX_HEAD_ANGLE.radians, head_angle + 0.15)
if keys[pygame.K_e]:
head_angle = max(pycozmo.MIN_HEAD_ANGLE.radians, head_angle - 0.15)
else:
head_angle = pycozmo.MAX_HEAD_ANGLE.radians - (throttle + 1.0) * 0.5 * \
(pycozmo.MAX_HEAD_ANGLE.radians - pycozmo.MIN_HEAD_ANGLE.radians)
cli.set_head_angle(head_angle)
# Drive
left = right = 0
if joystick:
y = joystick.get_axis(1)
twist = joystick.get_axis(2)
if abs(y) < STICK_DEADZONE: y = 0
if abs(twist) < STICK_DEADZONE: twist = 0
forward = -y * drive_speed
turn = twist * drive_speed
if mirror_mode:
turn = -turn
left = forward - turn
right = forward + turn
if keys[pygame.K_w] or keys[pygame.K_UP]: left = right = drive_speed
elif keys[pygame.K_s] or keys[pygame.K_DOWN]: left = right = -drive_speed
if keys[pygame.K_a] or keys[pygame.K_LEFT]: left = -drive_speed; right = drive_speed
elif keys[pygame.K_d] or keys[pygame.K_RIGHT]: left = drive_speed; right = -drive_speed
if driving_mode == "smooth":
if abs(left) > 5 or abs(right) > 5:
cli.drive_wheels(lwheel_speed=left, rwheel_speed=right)
else:
cli.drive_wheels(0, 0)
else:
if abs(left) > 5 or abs(right) > 5:
cli.drive_wheels(lwheel_speed=left, rwheel_speed=right, duration=JUMPY_BURST_DURATION)
time.sleep(JUMPY_PAUSE_TIME)
else:
cli.drive_wheels(0, 0)
prev_left = left
prev_right = right
if keys[pygame.K_SPACE]:
cli.drive_wheels(0, 0)
# Mirror
if keys[pygame.K_b]:
mirror_mode = not mirror_mode
print(f"🪞 Mirror Mode: {'ON' if mirror_mode else 'OFF'}")
time.sleep(0.3)
# Lift + Hat slam
if joystick:
hat = joystick.get_hat(0)
if hat[1] == 1:
lift_height_mm = min(pycozmo.MAX_LIFT_HEIGHT.mm, lift_height_mm + 4)
cli.set_lift_height(lift_height_mm)
elif hat[1] == -1:
lift_height_mm = max(pycozmo.MIN_LIFT_HEIGHT.mm, lift_height_mm - 4)
cli.set_lift_height(lift_height_mm)
if hat[0] == 1 and now - last_face_time > 0.25:
cli.set_lift_height(pycozmo.MAX_LIFT_HEIGHT.mm, accel=1200, duration=0.18)
print("🚀 Hat Slam UP!")
last_face_time = now
elif hat[0] == -1 and now - last_face_time > 0.25:
cli.set_lift_height(pycozmo.MIN_LIFT_HEIGHT.mm, accel=1200, duration=0.18)
print("🔽 Hat Slam DOWN!")
last_face_time = now
if keys[pygame.K_r]:
lift_height_mm = min(pycozmo.MAX_LIFT_HEIGHT.mm, lift_height_mm + 4)
cli.set_lift_height(lift_height_mm)
if keys[pygame.K_f]:
lift_height_mm = max(pycozmo.MIN_LIFT_HEIGHT.mm, lift_height_mm - 4)
cli.set_lift_height(lift_height_mm)
if keys[pygame.K_u]:
cli.set_lift_height(pycozmo.MAX_LIFT_HEIGHT.mm, accel=1200, duration=0.18)
print("🚀 Manual Slam UP!")
time.sleep(0.2)
if keys[pygame.K_i]:
cli.set_lift_height(pycozmo.MIN_LIFT_HEIGHT.mm, accel=1200, duration=0.18)
print("🔽 Manual Slam DOWN!")
time.sleep(0.2)
# Machine Gun
if joystick and joystick.get_button(0) and now - last_face_time > 0.5:
print("🔫 MACHINE GUN FIRING!")
current_lift = lift_height_mm
for _ in range(MG_PULSES):
cli.set_lift_height(min(pycozmo.MAX_LIFT_HEIGHT.mm, current_lift + MG_VIBRATE), accel=3000, duration=0.04)
time.sleep(MG_INTERVAL)
cli.set_lift_height(max(pycozmo.MIN_LIFT_HEIGHT.mm, current_lift - MG_VIBRATE), accel=3000, duration=0.04)
time.sleep(MG_INTERVAL)
cli.set_lift_height(current_lift, accel=800, duration=0.12)
last_face_time = now
# IR Light
if keys[pygame.K_l]:
ir_light_on = not ir_light_on
cli.set_head_light(enable=ir_light_on)
print(f"💡 IR Light: {'ON' if ir_light_on else 'OFF'}")
time.sleep(0.2)
# Face commands
if now - last_face_time > FACE_COOLDOWN:
if keys[pygame.K_m] and static_faces:
current_face_idx = (current_face_idx + 1) % len(static_faces)
set_procedural_face(cli, False)
current_face = static_faces[current_face_idx]
cli.display_image(current_face)
animation_mode = None
print(f"🖼️ Static Face: {face_filenames[current_face_idx]}")
last_face_time = now
last_face_image = np.array(current_face).astype(np.uint8) * 255
if keys[pygame.K_g]:
animation_mode = "otter" if animation_mode != "otter" else None
print("🦦 Single Otter " + ("ON" if animation_mode == "otter" else "OFF"))
if animation_mode == "otter":
current_otter_frame = 0
last_frame_time = time.time()
last_face_time = now
if keys[pygame.K_y]:
animation_mode = "dual_otter" if animation_mode != "dual_otter" else None
print("🦦🦦 Dual Otters " + ("ON" if animation_mode == "dual_otter" else "OFF"))
if animation_mode == "dual_otter":
current_dual_otter_frame = 0
last_frame_time = time.time()
last_face_time = now
if keys[pygame.K_h]:
animation_mode = "hit" if animation_mode != "hit" else None
print("👀 Random Glance " + ("ON" if animation_mode == "hit" else "OFF"))
if animation_mode == "hit":
current_hit_frame = random.choice([0, 1])
last_frame_time = time.time()
last_face_time = now
if keys[pygame.K_k]:
animation_mode = "armcat" if animation_mode != "armcat" else None
print("🐱 ArmCat " + ("ON" if animation_mode == "armcat" else "OFF"))
if animation_mode == "armcat":
current_armcat_frame = 0
last_frame_time = time.time()
last_face_time = now
if keys[pygame.K_j]:
animation_mode = "j" if animation_mode != "j" else None
print("🃏 J Animation " + ("ON" if animation_mode == "j" else "OFF"))
if animation_mode == "j":
current_j_frame = 0
last_frame_time = time.time()
last_face_time = now
if keys[pygame.K_n]:
animation_mode = "nflag" if animation_mode != "nflag" else None
print("🏳️ NFlag Spinning " + ("ON" if animation_mode == "nflag" else "OFF"))
if animation_mode == "nflag":
current_nflag_frame = 0
last_frame_time = time.time()
last_face_time = now
if keys[pygame.K_o]:
new_state = not procedural_enabled
set_procedural_face(cli, new_state)
if new_state:
animation_mode = None
last_face_time = now
if keys[pygame.K_p]:
if animation_mode == "mechaMG":
animation_mode = None
print("🤖 mechaMG stopped")
elif mechaMG_frames:
set_procedural_face(cli, False)
animation_mode = "mechaMG"
current_mechaMG_frame = 0
last_frame_time = time.time()
print("🤖 mechaMG activated!")
last_face_time = now
# Run Animations
if not procedural_enabled:
now_anim = time.time()
if animation_mode == "armcat" and armcat_up and armcat_down and now_anim - last_frame_time > 0.2:
current_armcat_frame = 1 - current_armcat_frame
current_face = armcat_up if current_armcat_frame == 0 else armcat_down
cli.display_image(current_face)
last_frame_time = now_anim
last_face_image = np.array(current_face).astype(np.uint8) * 255
elif animation_mode == "hit" and hit_left and hit_right and now_anim - last_frame_time > 0:
delay = random.uniform(0.2, 0.5) if random.random() < 0.05 else random.uniform(2.0, 4.0)
if now_anim - last_frame_time > delay:
current_hit_frame = 1 - current_hit_frame
current_face = hit_left if current_hit_frame == 0 else hit_right
cli.display_image(current_face)
last_frame_time = now_anim
last_face_image = np.array(current_face).astype(np.uint8) * 255
elif animation_mode == "j" and jL and jR and now_anim - last_frame_time > 1.0:
current_j_frame = 1 - current_j_frame
current_face = jL if current_j_frame == 0 else jR
cli.display_image(current_face)
last_frame_time = now_anim
last_face_image = np.array(current_face).astype(np.uint8) * 255
elif animation_mode == "nflag" and nflag_frames and now_anim - last_frame_time > 0.08:
current_nflag_frame = (current_nflag_frame + 1) % len(nflag_frames)
current_face = nflag_frames[current_nflag_frame]
cli.display_image(current_face)
last_frame_time = now_anim
last_face_image = np.array(current_face).astype(np.uint8) * 255
elif animation_mode == "otter" and otter_frames and now_anim - last_frame_time > 0.04:
current_otter_frame = (current_otter_frame + 1) % len(otter_frames)
current_face = otter_frames[current_otter_frame]
cli.display_image(current_face)
last_frame_time = now_anim
last_face_image = np.array(current_face).astype(np.uint8) * 255
elif animation_mode == "dual_otter" and otter_frames and now_anim - last_frame_time > 0.04:
current_dual_otter_frame = (current_dual_otter_frame + 1) % len(otter_frames)
combined = Image.new("1", (128, 32), color=0)
left_idx = current_dual_otter_frame % len(otter_frames)
right_idx = (current_dual_otter_frame + 3) % len(otter_frames)
combined.paste(otter_frames[left_idx].crop((32, 0, 96, 32)), (0, 0))
combined.paste(otter_frames[right_idx].crop((32, 0, 96, 32)), (64, 0))
current_face = combined
cli.display_image(current_face)
last_frame_time = now_anim
last_face_image = np.array(current_face).astype(np.uint8) * 255
elif animation_mode == "mechaMG" and mechaMG_frames:
if now_anim - last_frame_time > mechaMG_delays[current_mechaMG_frame]:
current_mechaMG_frame = (current_mechaMG_frame + 1) % len(mechaMG_frames)
current_face = mechaMG_frames[current_mechaMG_frame]
cli.display_image(current_face)
last_frame_time = now_anim
last_face_image = np.array(current_face).astype(np.uint8) * 255
# Face Preview
preview = cv2.resize(last_face_image, (256, 64), interpolation=cv2.INTER_NEAREST)
preview = cv2.cvtColor(preview, cv2.COLOR_GRAY2BGR)
cv2.imshow("Cozmo Face Preview", preview)
# HUD
if now - last_battery_check > 5.0:
try:
battery_voltage = cli.battery_voltage
battery_percent = max(0, min(100, int((battery_voltage - 3.0) * 100 / 1.2)))
except:
battery_voltage = 0.0
battery_percent = 0
last_battery_check = now
hud = np.zeros((200, 340, 3), dtype=np.uint8)
cv2.putText(hud, f"Battery: {battery_voltage:.2f} V", (15, 35), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255,255,255), 2)
color = (0, 255, 0) if battery_percent > 40 else (0, 165, 255) if battery_percent > 20 else (0, 0, 255)
cv2.putText(hud, f"{battery_percent}%", (15, 70), cv2.FONT_HERSHEY_SIMPLEX, 1.4, color, 3)
cv2.putText(hud, f"Gear: {gears[gear_index][0]}", (15, 110), cv2.FONT_HERSHEY_SIMPLEX, 0.85, (255,255,100), 2)
mode_text = "Mode: Hobo" if driving_mode == "smooth" else "Mode: Robo"
cv2.putText(hud, mode_text, (15, 140), cv2.FONT_HERSHEY_SIMPLEX, 0.85, (0,255,255), 2)
viewer_text = "Viewer: ON" if viewer_control_enabled else "Viewer: OFF"
cv2.putText(hud, viewer_text, (15, 170), cv2.FONT_HERSHEY_SIMPLEX, 0.85, (0,255,0) if viewer_control_enabled else (0,0,255), 2)
cv2.imshow("Cozmo HUD", hud)
if latest_image is not None:
img = cv2.cvtColor(np.array(latest_image), cv2.COLOR_RGB2BGR)
cv2.imshow("Cozmo Eye View", img)
cv2.waitKey(1)
time.sleep(0.008)
clock.tick(60)
print("\nStopping safely...")
cli.drive_wheels(0, 0)
cli.set_lift_height(pycozmo.MIN_LIFT_HEIGHT.mm)
cli.enable_camera(enable=False)
cli.set_head_light(enable=False)
set_procedural_face(cli, True)
cv2.destroyAllWindows()
pygame.quit()
print("Disconnected.")