314 lines
9.9 KiB
C++
314 lines
9.9 KiB
C++
#include "behaviors.h"
|
|
#include <algorithm>
|
|
|
|
// ============================================================================
|
|
// Base Behavior Implementation
|
|
// ============================================================================
|
|
|
|
Behavior::Behavior() {
|
|
controlledMotors.clear();
|
|
}
|
|
|
|
void Behavior::addMotor(uint8_t motorID) {
|
|
// Check if motor already in list
|
|
for (uint8_t id : controlledMotors) {
|
|
if (id == motorID) {
|
|
return; // Already added
|
|
}
|
|
}
|
|
controlledMotors.push_back(motorID);
|
|
}
|
|
|
|
void Behavior::removeMotor(uint8_t motorID) {
|
|
controlledMotors.erase(
|
|
std::remove(controlledMotors.begin(), controlledMotors.end(), motorID),
|
|
controlledMotors.end()
|
|
);
|
|
}
|
|
|
|
void Behavior::clearMotors() {
|
|
controlledMotors.clear();
|
|
}
|
|
|
|
// ============================================================================
|
|
// Focus Behavior Implementation
|
|
// ============================================================================
|
|
|
|
FocusBehavior::FocusBehavior() {
|
|
isActive = false;
|
|
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, 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 || !radar.getTarget(0).valid) {
|
|
// No target - return to center
|
|
isActive = false;
|
|
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;
|
|
}
|
|
|
|
// Get first valid target
|
|
const RadarTarget& target = radar.getTarget(0);
|
|
|
|
if (!target.valid) {
|
|
isActive = false;
|
|
targetEyePosition = EYE_POSITION_CENTER;
|
|
targetNeckPosition = NECK_POSITION_CENTER;
|
|
targetDetectedTime = 0;
|
|
neckRotating = false;
|
|
return false;
|
|
}
|
|
|
|
// Active tracking - calculate target positions from radar angle
|
|
isActive = true;
|
|
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) {
|
|
// Provide position for eyes (motors 14 and 15)
|
|
if (motorID == FOCUS_MOTOR_1 || motorID == FOCUS_MOTOR_2) {
|
|
position = eyePosition;
|
|
return true;
|
|
}
|
|
|
|
// Provide position for neck (motor 27)
|
|
if (motorID == NECK_MOTOR) {
|
|
position = neckPosition;
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
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)
|
|
|
|
// 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
|
|
float rangeLeft = EYE_POSITION_CENTER - EYE_POSITION_MIN; // 347
|
|
float rangeRight = EYE_POSITION_MAX - EYE_POSITION_CENTER; // 253
|
|
|
|
uint16_t position;
|
|
if (normalizedAngle < 0.0f) {
|
|
// Left side: use left range
|
|
position = EYE_POSITION_CENTER + (uint16_t)(normalizedAngle * rangeLeft);
|
|
} else {
|
|
// Right side: use right range
|
|
position = EYE_POSITION_CENTER + (uint16_t)(normalizedAngle * rangeRight);
|
|
}
|
|
|
|
// Clamp to valid range
|
|
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
|
|
// ============================================================================
|
|
|
|
BehaviorManager behaviorManager;
|
|
|
|
BehaviorManager::BehaviorManager() {
|
|
behaviors.clear();
|
|
// Initialize all enabled states to false
|
|
for (int i = 0; i < 256; i++) {
|
|
enabledStates[i] = false;
|
|
}
|
|
}
|
|
|
|
void BehaviorManager::addBehavior(BehaviorID id, Behavior* behavior) {
|
|
if (behavior == nullptr) return;
|
|
|
|
// Check if already added
|
|
for (const auto& entry : behaviors) {
|
|
if (entry.behavior == behavior || entry.id == id) return;
|
|
}
|
|
|
|
behaviors.push_back({id, behavior});
|
|
// New behaviors are enabled by default
|
|
enabledStates[id] = true;
|
|
}
|
|
|
|
void BehaviorManager::removeBehavior(Behavior* behavior) {
|
|
behaviors.erase(
|
|
std::remove_if(behaviors.begin(), behaviors.end(),
|
|
[behavior](const BehaviorEntry& entry) {
|
|
return entry.behavior == behavior;
|
|
}),
|
|
behaviors.end()
|
|
);
|
|
}
|
|
|
|
void BehaviorManager::setBehaviorEnabled(BehaviorID id, bool enabled) {
|
|
enabledStates[id] = enabled;
|
|
}
|
|
|
|
bool BehaviorManager::isBehaviorEnabled(BehaviorID id) const {
|
|
return enabledStates[id];
|
|
}
|
|
|
|
uint8_t BehaviorManager::getBehaviorCount() const {
|
|
return behaviors.size();
|
|
}
|
|
|
|
bool BehaviorManager::getBehaviorInfo(uint8_t index, BehaviorID& id, bool& enabled) const {
|
|
if (index >= behaviors.size()) {
|
|
return false;
|
|
}
|
|
|
|
id = behaviors[index].id;
|
|
enabled = enabledStates[id];
|
|
return true;
|
|
}
|
|
|
|
void BehaviorManager::update() {
|
|
// Update all enabled behaviors
|
|
for (const auto& entry : behaviors) {
|
|
if (entry.behavior && enabledStates[entry.id]) {
|
|
entry.behavior->update();
|
|
}
|
|
}
|
|
}
|
|
|
|
bool BehaviorManager::getMotorPosition(uint8_t motorID, uint16_t& position) {
|
|
// Check all enabled behaviors to see if any wants to control this motor
|
|
for (const auto& entry : behaviors) {
|
|
if (entry.behavior && enabledStates[entry.id] &&
|
|
entry.behavior->getMotorPosition(motorID, position)) {
|
|
return true; // Found an enabled behavior controlling this motor
|
|
}
|
|
}
|
|
return false; // No enabled behavior controlling this motor
|
|
}
|