289 lines
8.6 KiB
C++
289 lines
8.6 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 eyes to center
|
|
eyePosition = lerp(eyePosition, EYE_POSITION_CENTER, INTERPOLATION_SPEED);
|
|
|
|
// Keep neck at center (no movement)
|
|
neckPosition = NECK_POSITION_CENTER;
|
|
|
|
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);
|
|
|
|
// Eyes track immediately
|
|
targetEyePosition = targetEyePos;
|
|
|
|
// Neck disabled for now - keep it centered
|
|
targetNeckPosition = NECK_POSITION_CENTER;
|
|
neckRotating = false;
|
|
|
|
// Smoothly interpolate eye position toward target
|
|
eyePosition = lerp(eyePosition, targetEyePosition, INTERPOLATION_SPEED);
|
|
|
|
// Keep neck at center (no movement)
|
|
neckPosition = NECK_POSITION_CENTER;
|
|
|
|
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: -50 to +50 degrees, mapped to full eye range (1700-2500, center 2200)
|
|
|
|
constexpr float ANGLE_MIN = -50.0f;
|
|
constexpr float ANGLE_MAX = 50.0f;
|
|
|
|
// Clamp angle to -50 to +50 range
|
|
if (radarAngle < ANGLE_MIN) radarAngle = ANGLE_MIN;
|
|
if (radarAngle > ANGLE_MAX) radarAngle = ANGLE_MAX;
|
|
|
|
// Normalize angle to -1.0 to 1.0 range
|
|
float normalized = radarAngle / 50.0f;
|
|
|
|
// Calculate range from center in each direction
|
|
// Left range: 2200 - 1700 = 500, Right range: 2500 - 2200 = 300
|
|
float rangeLeft = (float)(EYE_POSITION_CENTER - EYE_POSITION_MIN); // 500
|
|
float rangeRight = (float)(EYE_POSITION_MAX - EYE_POSITION_CENTER); // 300
|
|
|
|
// Use different ranges for left (negative) and right (positive) to use full range
|
|
float positionFloat;
|
|
if (normalized < 0.0f) {
|
|
// Negative angle: use left range (500 units)
|
|
positionFloat = (float)EYE_POSITION_CENTER + (normalized * rangeLeft);
|
|
} else {
|
|
// Positive angle: use right range (300 units)
|
|
positionFloat = (float)EYE_POSITION_CENTER + (normalized * rangeRight);
|
|
}
|
|
|
|
// Convert to int16_t first to handle negative values, then clamp
|
|
int16_t position = (int16_t)positionFloat;
|
|
|
|
// Clamp to valid range (1700 to 2500)
|
|
if (position < (int16_t)EYE_POSITION_MIN) position = (int16_t)EYE_POSITION_MIN;
|
|
if (position > (int16_t)EYE_POSITION_MAX) position = (int16_t)EYE_POSITION_MAX;
|
|
|
|
return (uint16_t)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
|
|
}
|