From 7e3218afb64e566ae70f970175bf0e350f1ec3c9 Mon Sep 17 00:00:00 2001 From: Jake Date: Wed, 21 Jan 2026 15:36:07 +0800 Subject: [PATCH] added neck motion following eye focus --- PROTOCOL_MIGRATION.md | 29 +++++++ behaviors.cpp | 185 ++++++++++++++++++++++++++++++++++-------- behaviors.h | 47 ++++++++--- sensors.cpp | 15 +++- sensors.h | 3 +- 5 files changed, 231 insertions(+), 48 deletions(-) diff --git a/PROTOCOL_MIGRATION.md b/PROTOCOL_MIGRATION.md index ebdea12..9574a3d 100644 --- a/PROTOCOL_MIGRATION.md +++ b/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) diff --git a/behaviors.cpp b/behaviors.cpp index 7601f3a..652cddf 100644 --- a/behaviors.cpp +++ b/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 - if (motorID != FOCUS_MOTOR_1 && motorID != FOCUS_MOTOR_2) { - return false; + // Provide position for eyes (motors 14 and 15) + if (motorID == FOCUS_MOTOR_1 || motorID == FOCUS_MOTOR_2) { + position = eyePosition; + return true; } - - // Always provide position for controlled motors (active by default) - // When not tracking a target, currentPosition is set to center in update() - position = currentPosition; - return true; + + // Provide position for neck (motor 27) + if (motorID == NECK_MOTOR) { + position = neckPosition; + return true; + } + + return false; } -uint16_t FocusBehavior::calculatePositionFromRadarX(float radarX) { - // Clamp radar x to valid range - if (radarX < RADAR_X_MIN) radarX = RADAR_X_MIN; - if (radarX > RADAR_X_MAX) radarX = RADAR_X_MAX; - - // Linear mapping from radar x (-80 to 80) to motor position (1700 to 2300) - // Center (2047) corresponds to radar x = 0 - // Left (-80) maps to 1700, Right (80) maps to 2300 +uint16_t FocusBehavior::calculateEyePositionFromRadarAngle(float radarAngle) { + // Calculate eye motor position from radar angle (in degrees) + // Angle range: approximately -45 to +45 degrees (typical radar FOV) + // Map to eye motor position range: 1700 to 2300 (center 2047) - // Normalize radar x to -1.0 to 1.0 range - float normalizedX = radarX / RADAR_X_MAX; + // 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 + float normalizedAngle = radarAngle / ANGLE_MAX; // Calculate range from center - // Left side: center - 347 = 1700 - // Right side: center + 253 = 2300 - float rangeLeft = POSITION_CENTER - POSITION_MIN; // 347 - float rangeRight = POSITION_MAX - POSITION_CENTER; // 253 + float rangeLeft = EYE_POSITION_CENTER - EYE_POSITION_MIN; // 347 + float rangeRight = EYE_POSITION_MAX - EYE_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 > POSITION_MAX) position = POSITION_MAX; + if (position < EYE_POSITION_MIN) position = EYE_POSITION_MIN; + 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 // ============================================================================ diff --git a/behaviors.h b/behaviors.h index 7863a05..c5ea6a4 100644 --- a/behaviors.h +++ b/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_2 = 15; - static constexpr uint16_t POSITION_CENTER = 2047; - 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; + static constexpr uint8_t FOCUS_MOTOR_1 = 14; // Eye motor 1 + static constexpr uint8_t FOCUS_MOTOR_2 = 15; // Eye motor 2 + static constexpr uint8_t NECK_MOTOR = 27; // Neck motor + + // Eye motor position range (motors 14 and 15) + 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 x coordinate - uint16_t calculatePositionFromRadarX(float radarX); + // 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); }; // ============================================================================ diff --git a/sensors.cpp b/sensors.cpp index 677e04f..42b8040 100644 --- a/sensors.cpp +++ b/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); } diff --git a/sensors.h b/sensors.h index 50232c5..3d343d1 100644 --- a/sensors.h +++ b/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;