added neck motion following eye focus

PROTOCOL_MIGRATION.md

@@ -104,6 +104,8 @@ ushort Crc16Ccitt(byte[] data)
 |-----|-------------|
 | `IMU0` | IMU data (accel x,y,z) |
 | `RDAR` | Radar target data |
+| `BHVR` | Behavior control (enable/disable) |
+| `BLST` | Behavior list (list all behaviors and states) |
 | `STAT` | System state/heartbeat |
 | `ACK!` | Acknowledge (success) |
 | `NACK` | Negative acknowledge (failure) |
@@ -272,6 +274,33 @@ For each of 3 targets:
 
 ---
 
+### Behaviors
+
+#### `BHVR` - Behavior Control (host → device)
+**Request:**
+```
+[behaviorID: 1 byte]   // Behavior ID (1 = Focus)
+[enable: 1 byte]       // 0 = disable, non-zero = enable
+```
+**Response:** `ACK!` on success, `NACK` on failure
+
+**Behavior IDs:**
+- `1` = Focus (radar tracking with motors 14 & 15)
+
+#### `BLST` - Behavior List (host → device)
+**Request:** Empty payload
+
+**Response:**
+```
+[count: 1 byte]                    // Number of registered behaviors
+[behaviorID1: 1 byte][enabled1: 1 byte]  // First behavior
+[behaviorID2: 1 byte][enabled2: 1 byte]  // Second behavior
+...
+```
+- `enabled`: 1 = enabled, 0 = disabled
+
+---
+
 ### System
 
 #### `STAT` - System State/Heartbeat (device → host)

behaviors.cpp

@@ -36,21 +36,37 @@ void Behavior::clearMotors() {
 
 FocusBehavior::FocusBehavior() {
   isActive = false;
-  currentPosition = POSITION_CENTER;
+  eyePosition = EYE_POSITION_CENTER;
+  neckPosition = NECK_POSITION_CENTER;
+  targetEyePosition = EYE_POSITION_CENTER;
+  targetNeckPosition = NECK_POSITION_CENTER;
+  targetDetectedTime = 0;
+  neckStartTime = 0;
+  neckRotating = false;
   
-  // Add motors 14 and 15 to controlled list
+  // Add motors 14, 15, and 27 to controlled list
   addMotor(FOCUS_MOTOR_1);
   addMotor(FOCUS_MOTOR_2);
+  addMotor(NECK_MOTOR);
 }
 
 bool FocusBehavior::update() {
   // Check radar for valid targets
   uint8_t targetCount = radar.getTargetCount();
+  unsigned long now = millis();
   
-  if (targetCount == 0) {
+  if (targetCount == 0 || !radar.getTarget(0).valid) {
     // No target - return to center
     isActive = false;
-    currentPosition = POSITION_CENTER;
+    targetEyePosition = EYE_POSITION_CENTER;
+    targetNeckPosition = NECK_POSITION_CENTER;
+    targetDetectedTime = 0;
+    neckRotating = false;
+    
+    // Smoothly interpolate to center
+    eyePosition = lerp(eyePosition, EYE_POSITION_CENTER, INTERPOLATION_SPEED);
+    neckPosition = lerp(neckPosition, NECK_POSITION_CENTER, INTERPOLATION_SPEED);
+    
     return false;
   }
 
@@ -59,63 +75,164 @@ bool FocusBehavior::update() {
   
   if (!target.valid) {
     isActive = false;
-    currentPosition = POSITION_CENTER;
+    targetEyePosition = EYE_POSITION_CENTER;
+    targetNeckPosition = NECK_POSITION_CENTER;
+    targetDetectedTime = 0;
+    neckRotating = false;
     return false;
   }
 
-  // Active tracking - calculate position from radar x coordinate
+  // Active tracking - calculate target positions from radar angle
   isActive = true;
-  currentPosition = calculatePositionFromRadarX(target.x);
+  uint16_t targetEyePos = calculateEyePositionFromRadarAngle(target.angle);
+  uint16_t targetNeckPos = calculateNeckPositionFromRadarAngle(target.angle);
+  
+  // Eyes track immediately
+  targetEyePosition = targetEyePos;
+  
+  // Check if this is a new target (angle changed significantly or first detection)
+  bool newTarget = (targetDetectedTime == 0) || 
+                   (abs((int16_t)targetEyePosition - (int16_t)eyePosition) > 50);
+  
+  if (newTarget) {
+    // Reset timing for new target
+    targetDetectedTime = now;
+    neckStartTime = now + NECK_DELAY_MS;
+    neckRotating = false;
+  }
+  
+  // Check if neck should start rotating
+  if (!neckRotating && now >= neckStartTime && targetDetectedTime > 0) {
+    neckRotating = true;
+    targetNeckPosition = targetNeckPos;
+  }
+  
+  // If neck is rotating, gradually center the eyes as neck approaches target
+  if (neckRotating) {
+    // Neck tracks the target
+    targetNeckPosition = targetNeckPos;
+    
+    // Calculate how far the neck has moved toward its target (0 = just started, 1 = at target)
+    int16_t neckDistanceToTarget = abs((int16_t)targetNeckPosition - (int16_t)neckPosition);
+    int16_t totalNeckRange = NECK_POSITION_MAX - NECK_POSITION_MIN;
+    float neckProgress = 1.0f - ((float)neckDistanceToTarget / (float)totalNeckRange);
+    neckProgress = (neckProgress < 0.0f) ? 0.0f : (neckProgress > 1.0f) ? 1.0f : neckProgress;
+    
+    // As neck gets closer to target, eyes gradually center
+    // Interpolate eye target between current target position and center based on neck progress
+    float eyeTargetBlend = 1.0f - neckProgress;  // 1.0 = at target angle, 0.0 = centered
+    int16_t eyeTargetOffset = (int16_t)targetEyePos - (int16_t)EYE_POSITION_CENTER;
+    targetEyePosition = EYE_POSITION_CENTER + (uint16_t)(eyeTargetOffset * eyeTargetBlend);
+  }
+  
+  // Smoothly interpolate current positions toward targets
+  // Use different speeds for eyes and neck (neck is slower/smoother)
+  eyePosition = lerp(eyePosition, targetEyePosition, INTERPOLATION_SPEED);
+  neckPosition = lerp(neckPosition, targetNeckPosition, NECK_INTERPOLATION_SPEED);
   
   return true;
 }
 
 bool FocusBehavior::getMotorPosition(uint8_t motorID, uint16_t& position) {
-  // Only provide position for motors we control
+  // Provide position for eyes (motors 14 and 15)
-  if (motorID != FOCUS_MOTOR_1 && motorID != FOCUS_MOTOR_2) {
+  if (motorID == FOCUS_MOTOR_1 || motorID == FOCUS_MOTOR_2) {
-    return false;
+    position = eyePosition;
+    return true;
   }
   
-  // Always provide position for controlled motors (active by default)
+  // Provide position for neck (motor 27)
-  // When not tracking a target, currentPosition is set to center in update()
+  if (motorID == NECK_MOTOR) {
-  position = currentPosition;
+    position = neckPosition;
     return true;
+  }
+  
+  return false;
 }
 
-uint16_t FocusBehavior::calculatePositionFromRadarX(float radarX) {
+uint16_t FocusBehavior::calculateEyePositionFromRadarAngle(float radarAngle) {
-  // Clamp radar x to valid range
+  // Calculate eye motor position from radar angle (in degrees)
-  if (radarX < RADAR_X_MIN) radarX = RADAR_X_MIN;
+  // Angle range: approximately -45 to +45 degrees (typical radar FOV)
-  if (radarX > RADAR_X_MAX) radarX = RADAR_X_MAX;
+  // Map to eye motor position range: 1700 to 2300 (center 2047)
   
-  // Linear mapping from radar x (-80 to 80) to motor position (1700 to 2300)
+  // Clamp angle to reasonable range (can extend later if needed)
-  // Center (2047) corresponds to radar x = 0
+  constexpr float ANGLE_MIN = -45.0f;
-  // Left (-80) maps to 1700, Right (80) maps to 2300
+  constexpr float ANGLE_MAX = 45.0f;
   
-  // Normalize radar x to -1.0 to 1.0 range
+  if (radarAngle < ANGLE_MIN) radarAngle = ANGLE_MIN;
-  float normalizedX = radarX / RADAR_X_MAX;
+  if (radarAngle > ANGLE_MAX) radarAngle = ANGLE_MAX;
+  
+  // Normalize angle to -1.0 to 1.0 range
+  float normalizedAngle = radarAngle / ANGLE_MAX;
   
   // Calculate range from center
-  // Left side: center - 347 = 1700
+  float rangeLeft = EYE_POSITION_CENTER - EYE_POSITION_MIN;   // 347
-  // Right side: center + 253 = 2300
+  float rangeRight = EYE_POSITION_MAX - EYE_POSITION_CENTER;  // 253
-  float rangeLeft = POSITION_CENTER - POSITION_MIN;   // 347
-  float rangeRight = POSITION_MAX - POSITION_CENTER;  // 253
   
   uint16_t position;
-  if (normalizedX < 0.0f) {
+  if (normalizedAngle < 0.0f) {
     // Left side: use left range
-    position = POSITION_CENTER + (uint16_t)(normalizedX * rangeLeft);
+    position = EYE_POSITION_CENTER + (uint16_t)(normalizedAngle * rangeLeft);
   } else {
     // Right side: use right range
-    position = POSITION_CENTER + (uint16_t)(normalizedX * rangeRight);
+    position = EYE_POSITION_CENTER + (uint16_t)(normalizedAngle * rangeRight);
   }
 
   // Clamp to valid range
-  if (position < POSITION_MIN) position = POSITION_MIN;
+  if (position < EYE_POSITION_MIN) position = EYE_POSITION_MIN;
-  if (position > POSITION_MAX) position = POSITION_MAX;
+  if (position > EYE_POSITION_MAX) position = EYE_POSITION_MAX;
 
   return position;
 }
 
+uint16_t FocusBehavior::calculateNeckPositionFromRadarAngle(float radarAngle) {
+  // Calculate neck motor position from radar angle (in degrees)
+  // Angle range: approximately -45 to +45 degrees (typical radar FOV)
+  // Map to neck motor position range: 1000 to 3000 (center 2000)
+  // NOTE: Rotation is inverted for neck motor
+  
+  // Clamp angle to reasonable range (can extend later if needed)
+  constexpr float ANGLE_MIN = -45.0f;
+  constexpr float ANGLE_MAX = 45.0f;
+  
+  if (radarAngle < ANGLE_MIN) radarAngle = ANGLE_MIN;
+  if (radarAngle > ANGLE_MAX) radarAngle = ANGLE_MAX;
+  
+  // Normalize angle to -1.0 to 1.0 range, then invert for neck motor
+  float normalizedAngle = -(radarAngle / ANGLE_MAX);
+  
+  // Calculate range from center
+  float rangeLeft = NECK_POSITION_CENTER - NECK_POSITION_MIN;   // 1000
+  float rangeRight = NECK_POSITION_MAX - NECK_POSITION_CENTER;  // 1000
+  
+  uint16_t position;
+  if (normalizedAngle < 0.0f) {
+    // Left side: use left range
+    position = NECK_POSITION_CENTER + (uint16_t)(normalizedAngle * rangeLeft);
+  } else {
+    // Right side: use right range
+    position = NECK_POSITION_CENTER + (uint16_t)(normalizedAngle * rangeRight);
+  }
+
+  // Clamp to valid range
+  if (position < NECK_POSITION_MIN) position = NECK_POSITION_MIN;
+  if (position > NECK_POSITION_MAX) position = NECK_POSITION_MAX;
+
+  return position;
+}
+
+uint16_t FocusBehavior::lerp(uint16_t current, uint16_t target, float t) {
+  // Linear interpolation with clamping to prevent overshoot
+  int16_t diff = (int16_t)target - (int16_t)current;
+  int16_t delta = (int16_t)(diff * t);
+  
+  // If difference is very small, snap to target
+  if (abs(diff) < 2) {
+    return target;
+  }
+  
+  return (uint16_t)(current + delta);
+}
+
 // ============================================================================
 // Behavior Manager Implementation
 // ============================================================================

behaviors.h

@@ -62,19 +62,44 @@ public:
 
 private:
   bool isActive;
-  uint16_t currentPosition;  // Current position for motors 14 and 15
+  uint16_t eyePosition;      // Current interpolated position for motors 14 and 15
+  uint16_t neckPosition;     // Current interpolated position for motor 27
+  
+  // Target positions
+  uint16_t targetEyePosition;
+  uint16_t targetNeckPosition;
+  
+  // Timing
+  unsigned long targetDetectedTime;  // When target was first detected
+  unsigned long neckStartTime;       // When neck rotation should start
+  bool neckRotating;                 // Whether neck is actively rotating
   
   // Configuration
-  static constexpr uint8_t FOCUS_MOTOR_1 = 14;
+  static constexpr uint8_t FOCUS_MOTOR_1 = 14;  // Eye motor 1
-  static constexpr uint8_t FOCUS_MOTOR_2 = 15;
+  static constexpr uint8_t FOCUS_MOTOR_2 = 15;  // Eye motor 2
-  static constexpr uint16_t POSITION_CENTER = 2047;
+  static constexpr uint8_t NECK_MOTOR = 27;     // Neck motor
-  static constexpr uint16_t POSITION_MIN = 1700;
-  static constexpr uint16_t POSITION_MAX = 2300;
-  static constexpr float RADAR_X_MIN = -80.0f;
-  static constexpr float RADAR_X_MAX = 80.0f;
   
-  // Calculate motor position from radar x coordinate
+  // Eye motor position range (motors 14 and 15)
-  uint16_t calculatePositionFromRadarX(float radarX);
+  static constexpr uint16_t EYE_POSITION_CENTER = 2047;
+  static constexpr uint16_t EYE_POSITION_MIN = 1600;
+  static constexpr uint16_t EYE_POSITION_MAX = 2500;
+  
+  // Neck motor position range (motor 27)
+  static constexpr uint16_t NECK_POSITION_CENTER = 2000;
+  static constexpr uint16_t NECK_POSITION_MIN = 1000;
+  static constexpr uint16_t NECK_POSITION_MAX = 3000;
+  
+  static constexpr unsigned long NECK_DELAY_MS = 1000;  // 1 second delay before neck moves
+  static constexpr float INTERPOLATION_SPEED = 0.05f;   // Smooth interpolation factor for eyes (0-1, higher = faster)
+  static constexpr float NECK_INTERPOLATION_SPEED = 0.03f;  // Slower interpolation for neck (smoother motion)
+  static constexpr float EYE_CENTERING_SPEED = 0.03f;   // Speed at which eyes center when neck rotates
+
+  // Calculate motor position from radar angle (in degrees)
+  uint16_t calculateEyePositionFromRadarAngle(float radarAngle);
+  uint16_t calculateNeckPositionFromRadarAngle(float radarAngle);
+  
+  // Smooth interpolation helper (linear interpolation)
+  uint16_t lerp(uint16_t current, uint16_t target, float t);
 };
 
 // ============================================================================

sensors.cpp

@@ -88,6 +88,14 @@ void Radar::parseFrame() {
     targets[i].speed = decodeSignMag(spd_raw) * RADAR_DISTANCE_SCALE;
     
     targets[i].valid = (y_raw != 0) && (y_raw != 0x8000) && (targets[i].y >= RADAR_MIN_VALID_DIST);
+    
+    // Calculate angle from x and y (x is mm from center line, y is distance)
+    // atan2(x, y) gives angle in radians, convert to degrees
+    if (targets[i].valid && targets[i].y > 0) {
+      targets[i].angle = atan2(targets[i].x, targets[i].y) * 180.0f / PI;
+    } else {
+      targets[i].angle = 0.0f;
+    }
   }
 }
 
@@ -105,7 +113,7 @@ uint8_t Radar::getTargetCount() const {
 }
 
 uint16_t Radar::packPayload(uint8_t* buffer) const {
-  // Format: count(1) + [valid(1), x(2), y(2), speed(2)] * 3 = 22 bytes
+  // Format: count(1) + [valid(1), x(2), y(2), speed(2), angle(2)] * 3 = 28 bytes
   buffer[0] = getTargetCount();
   uint16_t offset = 1;
   
@@ -115,6 +123,7 @@ uint16_t Radar::packPayload(uint8_t* buffer) const {
     int16_t x = (int16_t)(targets[i].x * 10);  // cm * 10 for precision
     int16_t y = (int16_t)(targets[i].y * 10);
     int16_t spd = (int16_t)(targets[i].speed * 10);
+    int16_t angle = (int16_t)(targets[i].angle * 10);  // degrees * 10 for precision
     
     buffer[offset++] = x & 0xFF;
     buffer[offset++] = (x >> 8) & 0xFF;
@@ -122,6 +131,8 @@ uint16_t Radar::packPayload(uint8_t* buffer) const {
     buffer[offset++] = (y >> 8) & 0xFF;
     buffer[offset++] = spd & 0xFF;
     buffer[offset++] = (spd >> 8) & 0xFF;
+    buffer[offset++] = angle & 0xFF;
+    buffer[offset++] = (angle >> 8) & 0xFF;
   }
   
   return offset;
@@ -322,7 +333,7 @@ void SensorManager::sendADXLPacket() {
 }
 
 void SensorManager::sendRadarPacket() {
-  uint8_t payload[32];
+  uint8_t payload[32];  // Updated size for angle field
   uint16_t len = radar.packPayload(payload);
   sendPacket(Tag::RADAR, payload, len);
 }

sensors.h

@@ -27,6 +27,7 @@ struct RadarTarget {
   float x;        // X position in cm (negative=left, positive=right)
   float y;        // Y distance in cm (forward)
   float speed;    // Speed in cm/s
+  float angle;    // Angle in degrees (calculated from x,y using atan2)
   bool valid;     // Target is valid
 };
 
@@ -116,7 +117,7 @@ public:
 private:
   bool adxlStreamEnabled = true;
   bool radarStreamEnabled = true;
-  uint16_t adxlInterval = 10;
+  uint16_t adxlInterval = 500;  // 500ms = 2 packets per second
   uint16_t radarInterval = 10;
   unsigned long lastADXLSend = 0;
   unsigned long lastRadarSend = 0;