From 2627d26a5b1c1ee420362fd82faf756a1d4f93db Mon Sep 17 00:00:00 2001 From: Jake Date: Wed, 21 Jan 2026 22:43:12 +0800 Subject: [PATCH] implemened idle behaviour (slight random movement on every motor) --- HansonServo.ino | 14 +- PROTOCOL_MIGRATION.md | 3 +- behaviors.cpp | 62 +++++++ behaviors.h | 34 +++- focus.py | 368 ------------------------------------------ 5 files changed, 110 insertions(+), 371 deletions(-) delete mode 100644 focus.py diff --git a/HansonServo.ino b/HansonServo.ino index 3f39392..c9b192b 100644 --- a/HansonServo.ino +++ b/HansonServo.ino @@ -499,7 +499,19 @@ void setup() { // Initialize behaviors static FocusBehavior focusBehavior; behaviorManager.addBehavior(BEHAVIOR_FOCUS, &focusBehavior); - Serial.println("[HansonServo] Behaviors initialized"); + behaviorManager.setBehaviorEnabled(BEHAVIOR_FOCUS, true); + + // Initialize idle behavior with all motor IDs from config + static IdleBehavior idleBehavior; + std::vector allMotorIDs; + for (const Motor& motor : config.motors) { + allMotorIDs.push_back(motor.motorID); + } + idleBehavior.initMotors(allMotorIDs); + behaviorManager.addBehavior(BEHAVIOR_IDLE, &idleBehavior); + behaviorManager.setBehaviorEnabled(BEHAVIOR_IDLE, true); + + Serial.println("[HansonServo] Behaviors initialized (focus + idle)"); Serial.println("[HansonServo] Ready"); Serial.println("[HansonServo] Protocol: 0xA5 0x5A tagged packets with CRC16"); diff --git a/PROTOCOL_MIGRATION.md b/PROTOCOL_MIGRATION.md index 9574a3d..81e4027 100644 --- a/PROTOCOL_MIGRATION.md +++ b/PROTOCOL_MIGRATION.md @@ -285,7 +285,8 @@ For each of 3 targets: **Response:** `ACK!` on success, `NACK` on failure **Behavior IDs:** -- `1` = Focus (radar tracking with motors 14 & 15) +- `1` = Focus (radar tracking with eye motors 14 & 15) +- `2` = Idle (perlin noise motion for all motors, ±500 range from center) #### `BLST` - Behavior List (host → device) **Request:** Empty payload diff --git a/behaviors.cpp b/behaviors.cpp index 15ad3e5..4e5d8ed 100644 --- a/behaviors.cpp +++ b/behaviors.cpp @@ -208,6 +208,68 @@ uint16_t FocusBehavior::lerp(uint16_t current, uint16_t target, float t) { return (uint16_t)(current + delta); } +// ============================================================================ +// Idle Behavior Implementation +// ============================================================================ + +IdleBehavior::IdleBehavior() { + startTime = millis(); + + // Initialize all motor positions to center + for (int i = 0; i < 256; i++) { + motorPositions[i] = POSITION_CENTER; + } +} + +void IdleBehavior::initMotors(const std::vector& motorIDs) { + clearMotors(); + for (uint8_t id : motorIDs) { + addMotor(id); + motorPositions[id] = POSITION_CENTER; + } +} + +bool IdleBehavior::update() { + unsigned long now = millis(); + float timeOffset = (float)(now - startTime) * NOISE_SPEED; + + // Update position for each controlled motor using perlin noise + for (uint8_t motorID : controlledMotors) { + // Use motor ID as seed offset for variety between motors + uint16_t seed = motorID * MOTOR_SEED_OFFSET; + + // Get perlin noise value (-1 to 1 range approximately) + float noiseValue = perlin1D_octave(seed, timeOffset, 3, 0.5f); + + // Map noise to position range: center ± NOISE_RANGE + // Perlin noise typically returns values in roughly -1 to 1 range + int16_t offset = (int16_t)(noiseValue * (float)NOISE_RANGE); + + // Calculate final position + int16_t position = (int16_t)POSITION_CENTER + offset; + + // Clamp to valid servo range + if (position < 1547) position = 1547; // center - 500 + if (position > 2547) position = 2547; // center + 500 + + motorPositions[motorID] = (uint16_t)position; + } + + // Idle behavior is always active (but low priority) + return true; +} + +bool IdleBehavior::getMotorPosition(uint8_t motorID, uint16_t& position) { + // Check if we control this motor + for (uint8_t id : controlledMotors) { + if (id == motorID) { + position = motorPositions[motorID]; + return true; + } + } + return false; +} + // ============================================================================ // Behavior Manager Implementation // ============================================================================ diff --git a/behaviors.h b/behaviors.h index dc4afca..b13df2d 100644 --- a/behaviors.h +++ b/behaviors.h @@ -3,6 +3,7 @@ #include #include "sensors.h" #include "robotconfig.h" +#include "noise.h" // ============================================================================ // Behavior IDs @@ -10,7 +11,7 @@ enum BehaviorID : uint8_t { BEHAVIOR_FOCUS = 1, // Focus behavior (radar tracking) - // Add more behavior IDs here as needed + BEHAVIOR_IDLE = 2, // Idle behavior (perlin noise for all motors) }; // ============================================================================ @@ -102,6 +103,37 @@ private: uint16_t lerp(uint16_t current, uint16_t target, float t); }; +// ============================================================================ +// Idle Behavior - Adds perlin noise to all motors for natural idle motion +// ============================================================================ + +class IdleBehavior : public Behavior { +public: + IdleBehavior(); + + // Initialize with list of motor IDs to control + void initMotors(const std::vector& motorIDs); + + // Update behavior - calculates new noise positions + bool update() override; + + // Get motor position for a controlled motor + bool getMotorPosition(uint8_t motorID, uint16_t& position) override; + +private: + // Store current positions for each motor (indexed by motor ID) + uint16_t motorPositions[256]; + + // Time offset for perlin noise animation + unsigned long startTime; + + // Configuration + static constexpr uint16_t POSITION_CENTER = 2047; + static constexpr uint16_t NOISE_RANGE = 100; // ±500 from center + static constexpr float NOISE_SPEED = 0.000125f; // How fast noise evolves (slower = smoother, 4x slower) + static constexpr uint16_t MOTOR_SEED_OFFSET = 100; // Seed offset between motors for variety +}; + // ============================================================================ // Behavior Manager - Manages active behaviors and resolves motor conflicts // ============================================================================ diff --git a/focus.py b/focus.py deleted file mode 100644 index 4c7abee..0000000 --- a/focus.py +++ /dev/null @@ -1,368 +0,0 @@ -import math -import tkinter as tk -from tkinter import ttk -import time - -# Try to import serial -try: - import serial - import serial.tools.list_ports - SERIAL_AVAILABLE = True -except ImportError: - SERIAL_AVAILABLE = False - serial = None - print("Warning: pyserial not found. Serial communication will be disabled.") - print("Install with: pip install pyserial") - -# Serial communication protocol functions -SYNC0 = 0xA5 -SYNC1 = 0x5A -BAUD_RATE = 1000000 # 1 Mbps -TAG_MSET = 'MSET' # Set motor positions - -# Motor IDs -LEFT_EYE_ID = 14 -RIGHT_EYE_ID = 15 - -# Eye position range -EYE_MIN_POS = 1800 -EYE_MAX_POS = 2500 -EYE_CENTER_POS = 2200 # Midpoint of 1000-3000 - -# Angle range for eyes (degrees) -EYE_ANGLE_MIN = -90.0 -EYE_ANGLE_MAX = 90.0 - - -def crc16_ccitt(data, init=0xFFFF): - """CRC16-CCITT calculation""" - crc = init - for byte in data: - crc ^= byte << 8 - for i in range(8): - if crc & 0x8000: - crc = ((crc << 1) ^ 0x1021) & 0xFFFF - else: - crc = (crc << 1) & 0xFFFF - return crc - - -def tag_to_bytes(tag): - """Convert 4-character tag string to bytes""" - return tag.encode('ascii')[:4].ljust(4, b' ')[:4] - - -def send_packet(ser, tag, payload=b''): - """Send a packet with the protocol format""" - payload_array = bytes(payload) if isinstance(payload, (list, tuple)) else payload - length = len(payload_array) - seq = 0 # Simple sequence number - - # Build packet - packet = bytearray() - packet.append(SYNC0) - packet.append(SYNC1) - packet.extend(tag_to_bytes(tag)) - packet.append(length & 0xFF) - packet.append((length >> 8) & 0xFF) - packet.append(seq & 0xFF) - packet.append((seq >> 8) & 0xFF) - packet.extend(payload_array) - - # Calculate CRC over tag + length + seq + payload - crc_data = packet[2:] # Everything after sync bytes - crc = crc16_ccitt(crc_data) - packet.append(crc & 0xFF) - packet.append((crc >> 8) & 0xFF) - - packet_bytes = bytes(packet) - ser.write(packet_bytes) - ser.flush() - - -def send_motor_positions(ser, motor_positions): - """ - Send multiple motor position updates in ONE MSET packet. - - Args: - ser: Serial connection object - motor_positions: List of (motor_id, position) tuples - - Format: For each motor: [motor_id: 1 byte][position: 2 bytes little-endian] - """ - if not motor_positions: - return - - # Build payload: [motor_id (1 byte), position_low (1 byte), position_high (1 byte)] for each motor - payload = bytearray() - for motor_id, position in motor_positions: - payload.append(motor_id) - payload.append(position & 0xFF) # Low byte - payload.append((position >> 8) & 0xFF) # High byte - - motor_str = ", ".join([f"{mid}:{pos}" for mid, pos in motor_positions]) - print(f"Sending motor positions: [{motor_str}]") - send_packet(ser, TAG_MSET, bytes(payload)) - - -def angle_to_eye_position(angle_degrees): - """ - Convert angle in degrees to eye motor position (1000-3000). - -90 degrees -> 1000 - 0 degrees -> 2000 (center) - +90 degrees -> 3000 - """ - # Normalize angle to -1.0 to 1.0 - normalized = max(-1.0, min(1.0, angle_degrees / 90.0)) - # Map to 1000-3000 range - position = EYE_CENTER_POS + (normalized * (EYE_MAX_POS - EYE_CENTER_POS)) - return int(position) - - -def lerp(start, end, t): - """Linear interpolation between start and end, t is 0.0 to 1.0""" - return start + (end - start) * t - - -class EyeControlGUI: - def __init__(self, root): - self.root = root - self.root.title("Eye Control") - self.root.geometry("500x350") - - # Serial connection - self.serial_connection = None - self.last_send_time = 0.0 - self.min_send_interval = 1.0 / 60.0 # 60 updates per second - - # Animation state - self.lerping = False - self.lerp_start_time = 0.0 - self.lerp_duration = 0.5 # Calculated from distance and max speed - self.eye_start_pos = EYE_CENTER_POS - self.eye_target_pos = EYE_CENTER_POS - self.max_speed = 500.0 # Max speed in position units per second - - # Current motor positions (for display and lerping) - self.current_left_eye_pos = EYE_CENTER_POS - self.current_right_eye_pos = EYE_CENTER_POS - - # Create main frame - main_frame = ttk.Frame(root, padding="10") - main_frame.grid(row=0, column=0, sticky=(tk.W, tk.E, tk.N, tk.S)) - main_frame.columnconfigure(1, weight=1) - root.columnconfigure(0, weight=1) - root.rowconfigure(0, weight=1) - - row = 0 - - # Angle control section - angle_frame = ttk.LabelFrame(main_frame, text="Target Angle", padding="10") - angle_frame.grid(row=row, column=0, columnspan=2, sticky=(tk.W, tk.E), pady=5) - angle_frame.columnconfigure(1, weight=1) - row += 1 - - ttk.Label(angle_frame, text="Angle (degrees):").grid(row=0, column=0, sticky=tk.W, pady=5) - self.angle_var = tk.DoubleVar(value=0.0) - self.angle_scale = ttk.Scale(angle_frame, from_=-90, to=90, variable=self.angle_var, - orient=tk.HORIZONTAL, command=self.on_angle_change) - self.angle_scale.grid(row=0, column=1, sticky=(tk.W, tk.E), pady=5, padx=5) - self.angle_label = ttk.Label(angle_frame, text="0.0°", width=8) - self.angle_label.grid(row=0, column=2, padx=5) - - # Speed control section - speed_frame = ttk.LabelFrame(main_frame, text="Movement Speed", padding="10") - speed_frame.grid(row=row, column=0, columnspan=2, sticky=(tk.W, tk.E), pady=5) - speed_frame.columnconfigure(1, weight=1) - row += 1 - - ttk.Label(speed_frame, text="Max Speed (units/sec):").grid(row=0, column=0, sticky=tk.W, pady=5) - self.max_speed_var = tk.DoubleVar(value=500.0) - self.speed_scale = ttk.Scale(speed_frame, from_=50, to=2000, variable=self.max_speed_var, - orient=tk.HORIZONTAL, command=self.on_max_speed_change) - self.speed_scale.grid(row=0, column=1, sticky=(tk.W, tk.E), pady=5, padx=5) - self.speed_label = ttk.Label(speed_frame, text="500", width=8) - self.speed_label.grid(row=0, column=2, padx=5) - - # Status section - status_frame = ttk.LabelFrame(main_frame, text="Status", padding="10") - status_frame.grid(row=row, column=0, columnspan=2, sticky=(tk.W, tk.E), pady=5) - row += 1 - - self.status_label = ttk.Label(status_frame, text="Disconnected", foreground="red") - self.status_label.grid(row=0, column=0, sticky=tk.W) - - self.animation_status_label = ttk.Label(status_frame, text="Ready", foreground="green") - self.animation_status_label.grid(row=1, column=0, sticky=tk.W, pady=5) - - # Motor positions display - positions_frame = ttk.LabelFrame(main_frame, text="Motor Positions", padding="10") - positions_frame.grid(row=row, column=0, columnspan=2, sticky=(tk.W, tk.E), pady=5) - row += 1 - - ttk.Label(positions_frame, text="Left Eye (ID 14):").grid(row=0, column=0, sticky=tk.W, pady=5) - self.left_eye_label = ttk.Label(positions_frame, text=str(EYE_CENTER_POS), font=("Arial", 12, "bold")) - self.left_eye_label.grid(row=0, column=1, sticky=tk.W, padx=10) - - ttk.Label(positions_frame, text="Right Eye (ID 15):").grid(row=1, column=0, sticky=tk.W, pady=5) - self.right_eye_label = ttk.Label(positions_frame, text=str(EYE_CENTER_POS), font=("Arial", 12, "bold")) - self.right_eye_label.grid(row=1, column=1, sticky=tk.W, padx=10) - - # Handle window close - self.root.protocol("WM_DELETE_WINDOW", self.on_closing) - - # Connect to serial port - self.connect_to_serial() - - # Start animation update loop - self.update_animation() - - def connect_to_serial(self): - """Find and connect to the only available COM port""" - if not SERIAL_AVAILABLE: - self.status_label.config(text="pyserial not installed", foreground="red") - print("pyserial not available") - return - - try: - ports = list(serial.tools.list_ports.comports()) - if not ports: - self.status_label.config(text="No COM port found", foreground="red") - print("No COM port found") - return - - if len(ports) > 1: - port_names = [p.device for p in ports] - self.status_label.config(text=f"Multiple ports found: {', '.join(port_names)}", foreground="orange") - print(f"Warning: Multiple ports found: {port_names}, using first: {port_names[0]}") - - port = ports[0].device - print(f"Connecting to {port}...") - - self.serial_connection = serial.Serial( - port=port, - baudrate=BAUD_RATE, - timeout=2.0, - write_timeout=2.0 - ) - - self.status_label.config(text=f"Connected to {port}", foreground="green") - print(f"Connected to {port}") - - except serial.SerialException as e: - self.status_label.config(text=f"Connection failed: {str(e)}", foreground="red") - print(f"Connection failed: {e}") - self.serial_connection = None - except Exception as e: - self.status_label.config(text=f"Error: {str(e)}", foreground="red") - print(f"Error connecting: {e}") - self.serial_connection = None - - def on_closing(self): - """Handle window close event""" - if SERIAL_AVAILABLE and self.serial_connection and self.serial_connection.is_open: - try: - self.serial_connection.close() - print("Serial connection closed") - except: - pass - self.root.destroy() - - def on_max_speed_change(self, *args): - """Called when max speed slider changes""" - speed = self.max_speed_var.get() - self.speed_label.config(text=f"{int(speed)}") - self.max_speed = speed - - def on_angle_change(self, *args): - """Called when angle slider changes""" - angle = self.angle_var.get() - self.angle_label.config(text=f"{angle:.1f}°") - - # Calculate target eye position from angle - target_pos = angle_to_eye_position(angle) - - # Start lerping from current position to target position - if target_pos != self.eye_target_pos: - self.eye_start_pos = self.current_left_eye_pos # Both eyes same position - self.eye_target_pos = target_pos - - # Calculate duration based on distance and max speed - # For smoothstep S-curve, peak velocity = 1.5 * distance / duration - # So: duration = 1.5 * distance / max_speed - distance = abs(self.eye_target_pos - self.eye_start_pos) - if distance > 0 and self.max_speed > 0: - self.lerp_duration = (1.5 * distance) / self.max_speed - # Minimum duration to prevent instant jumps - self.lerp_duration = max(0.016, self.lerp_duration) - else: - self.lerp_duration = 0.016 # Minimum 1 frame - - self.lerping = True - self.lerp_start_time = time.time() - self.animation_status_label.config(text=f"Moving... ({self.lerp_duration:.2f}s)", foreground="blue") - print(f"[DEBUG] New target: {target_pos}, distance: {distance}, duration: {self.lerp_duration:.3f}s") - - def update_animation(self): - """Update animation loop - called periodically""" - if self.lerping: - current_time = time.time() - elapsed = current_time - self.lerp_start_time - - if elapsed >= self.lerp_duration: - # Lerp complete - self.current_left_eye_pos = self.eye_target_pos - self.current_right_eye_pos = self.eye_target_pos - self.lerping = False - self.animation_status_label.config(text="Ready", foreground="green") - print(f"[DEBUG] Lerp complete at {self.eye_target_pos}") - else: - # Continue lerping - t = elapsed / self.lerp_duration - # Smooth interpolation (ease in-out) - S-curve - t_smooth = t * t * (3.0 - 2.0 * t) - - # Lerp eyes to target position - self.current_left_eye_pos = int(lerp(self.eye_start_pos, self.eye_target_pos, t_smooth)) - self.current_right_eye_pos = self.current_left_eye_pos - print(f"[DEBUG] Lerping: t={t:.3f}, pos={self.current_left_eye_pos} (from {self.eye_start_pos} to {self.eye_target_pos})") - - # Update display - self.left_eye_label.config(text=str(self.current_left_eye_pos)) - self.right_eye_label.config(text=str(self.current_right_eye_pos)) - - # Send motor updates - self.send_motor_updates() - - # Schedule next update (60 FPS) - self.root.after(16, self.update_animation) - - def send_motor_updates(self): - """Send motor position updates if connected and throttled""" - if not SERIAL_AVAILABLE or not self.serial_connection or not self.serial_connection.is_open: - print(f"[DEBUG] Not sending - connection: {SERIAL_AVAILABLE and self.serial_connection is not None}") - return - - current_time = time.time() - time_since_last_send = current_time - self.last_send_time - - if time_since_last_send >= self.min_send_interval: - try: - motor_positions = [ - (LEFT_EYE_ID, self.current_left_eye_pos), - (RIGHT_EYE_ID, self.current_right_eye_pos) - ] - print(f"[DEBUG] Sending: L={self.current_left_eye_pos}, R={self.current_right_eye_pos}, interval={time_since_last_send*1000:.1f}ms") - send_motor_positions(self.serial_connection, motor_positions) - self.last_send_time = current_time - except Exception as e: - print(f"Error sending motor positions: {e}") - self.status_label.config(text=f"Send error: {str(e)}", foreground="red") - else: - print(f"[DEBUG] Throttled - only {time_since_last_send*1000:.1f}ms since last send") - - -if __name__ == "__main__": - root = tk.Tk() - app = EyeControlGUI(root) - root.mainloop()