implemened idle behaviour (slight random movement on every motor)

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<uint8_t> 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");

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

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<uint8_t>& 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
 // ============================================================================

behaviors.h

@@ -3,6 +3,7 @@
 #include <vector>
 #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<uint8_t>& 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
 // ============================================================================

focus.py

@@ -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()