#!/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.")