diff --git a/HansonServo.ino b/HansonServo.ino index 5da5812..55e8d73 100644 --- a/HansonServo.ino +++ b/HansonServo.ino @@ -22,6 +22,7 @@ #include "nodegraph.h" #include "protocol.h" #include "sensors.h" +#include "behaviors.h" #include @@ -135,10 +136,19 @@ void runFrameAnimation() { std::vector speeds; for (const auto& motorPos : *frameData) { + uint16_t finalPosition = motorPos.position; + + // Check if a behavior wants to control this motor + uint16_t behaviorPosition; + if (behaviorManager.getMotorPosition(motorPos.motorID, behaviorPosition)) { + // Behavior is controlling this motor, use its position instead + finalPosition = behaviorPosition; + } + motorIDs.push_back(motorPos.motorID); - positions.push_back(motorPos.position); + positions.push_back(finalPosition); speeds.push_back(0); - config.setMotorPosition(motorPos.motorID, motorPos.position); + config.setMotorPosition(motorPos.motorID, finalPosition); config.setMotorEnabled(motorPos.motorID, true); } @@ -265,10 +275,19 @@ void runNodeAnimation() { for (const auto &[motorID, value] : outputs) { if (value != 65535) { + uint16_t finalPosition = value; + + // Check if a behavior wants to control this motor + uint16_t behaviorPosition; + if (behaviorManager.getMotorPosition(motorID, behaviorPosition)) { + // Behavior is controlling this motor, use its position instead + finalPosition = behaviorPosition; + } + motorIDs.push_back(motorID); - positions.push_back(value); + positions.push_back(finalPosition); speeds.push_back(0); - config.setMotorPosition(motorID, value); + config.setMotorPosition(motorID, finalPosition); config.setMotorEnabled(motorID, true); } else { // Only disable torque for motors that should be limp @@ -448,6 +467,11 @@ void setup() { Serial.println("[HansonServo] Config init failed"); } + // Initialize behaviors + static FocusBehavior focusBehavior; + behaviorManager.addBehavior(&focusBehavior); + Serial.println("[HansonServo] Behaviors initialized"); + Serial.println("[HansonServo] Ready"); Serial.println("[HansonServo] Protocol: 0xA5 0x5A tagged packets with CRC16"); @@ -486,6 +510,9 @@ void loop() { // Protocol handling handleProtocol(); + // Update behaviors + behaviorManager.update(); + // Animation playback (auto-selects v1 node or v2 frame based on version) runAnimation(); @@ -494,7 +521,7 @@ void loop() { handleMotorStreaming(); // Sensor updates and streaming - //sensors.update(); + sensors.update(); // Heartbeat sendHeartbeat(); diff --git a/behaviors.cpp b/behaviors.cpp new file mode 100644 index 0000000..28ee05d --- /dev/null +++ b/behaviors.cpp @@ -0,0 +1,167 @@ +#include "behaviors.h" +#include + +// ============================================================================ +// 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; + currentPosition = POSITION_CENTER; + + // Add motors 14 and 15 to controlled list + addMotor(FOCUS_MOTOR_1); + addMotor(FOCUS_MOTOR_2); +} + +bool FocusBehavior::update() { + // Check radar for valid targets + uint8_t targetCount = radar.getTargetCount(); + + if (targetCount == 0) { + // No target - return to center + isActive = false; + currentPosition = POSITION_CENTER; + return false; + } + + // Get first valid target + const RadarTarget& target = radar.getTarget(0); + + if (!target.valid) { + isActive = false; + currentPosition = POSITION_CENTER; + return false; + } + + // Active tracking - calculate position from radar x coordinate + isActive = true; + currentPosition = calculatePositionFromRadarX(target.x); + + 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; + } + + // Only provide position if behavior is active + if (!isActive) { + return false; + } + + position = currentPosition; + return true; +} + +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 + + // Normalize radar x to -1.0 to 1.0 range + float normalizedX = radarX / RADAR_X_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 + + uint16_t position; + if (normalizedX < 0.0f) { + // Left side: use left range + position = POSITION_CENTER + (uint16_t)(normalizedX * rangeLeft); + } else { + // Right side: use right range + position = POSITION_CENTER + (uint16_t)(normalizedX * rangeRight); + } + + // Clamp to valid range + if (position < POSITION_MIN) position = POSITION_MIN; + if (position > POSITION_MAX) position = POSITION_MAX; + + return position; +} + +// ============================================================================ +// Behavior Manager Implementation +// ============================================================================ + +BehaviorManager behaviorManager; + +BehaviorManager::BehaviorManager() { + behaviors.clear(); +} + +void BehaviorManager::addBehavior(Behavior* behavior) { + if (behavior == nullptr) return; + + // Check if already added + for (Behavior* b : behaviors) { + if (b == behavior) return; + } + + behaviors.push_back(behavior); +} + +void BehaviorManager::removeBehavior(Behavior* behavior) { + behaviors.erase( + std::remove(behaviors.begin(), behaviors.end(), behavior), + behaviors.end() + ); +} + +void BehaviorManager::update() { + // Update all behaviors + for (Behavior* behavior : behaviors) { + if (behavior) { + behavior->update(); + } + } +} + +bool BehaviorManager::getMotorPosition(uint8_t motorID, uint16_t& position) { + // Check all behaviors to see if any wants to control this motor + for (Behavior* behavior : behaviors) { + if (behavior && behavior->getMotorPosition(motorID, position)) { + return true; // Found a behavior controlling this motor + } + } + return false; // No behavior controlling this motor +} diff --git a/behaviors.h b/behaviors.h new file mode 100644 index 0000000..59233d0 --- /dev/null +++ b/behaviors.h @@ -0,0 +1,97 @@ +#pragma once +#include +#include +#include "sensors.h" +#include "robotconfig.h" + +// ============================================================================ +// Base Behavior Class +// ============================================================================ + +class Behavior { +public: + Behavior(); + virtual ~Behavior() = default; + + // Get list of motor IDs this behavior controls + const std::vector& getControlledMotors() const { return controlledMotors; } + + // Add a motor to the controlled list + void addMotor(uint8_t motorID); + + // Remove a motor from the controlled list + void removeMotor(uint8_t motorID); + + // Clear all controlled motors + void clearMotors(); + + // Virtual method to update the behavior (called each frame) + // Returns true if the behavior is active and wants to control motors + virtual bool update() = 0; + + // Virtual method to get the desired position for a motor + // Returns true if this behavior wants to control this motor, false otherwise + virtual bool getMotorPosition(uint8_t motorID, uint16_t& position) = 0; + +protected: + std::vector controlledMotors; +}; + +// ============================================================================ +// Focus Behavior - Tracks radar targets with eyes/neck +// ============================================================================ + +class FocusBehavior : public Behavior { +public: + FocusBehavior(); + + // Update behavior - check radar for targets + bool update() override; + + // Get motor position for a controlled motor + bool getMotorPosition(uint8_t motorID, uint16_t& position) override; + +private: + bool isActive; + uint16_t currentPosition; // Current position for motors 14 and 15 + + // 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; + + // Calculate motor position from radar x coordinate + uint16_t calculatePositionFromRadarX(float radarX); +}; + +// ============================================================================ +// Behavior Manager - Manages active behaviors and resolves motor conflicts +// ============================================================================ + +class BehaviorManager { +public: + BehaviorManager(); + + // Add a behavior to the manager + void addBehavior(Behavior* behavior); + + // Remove a behavior from the manager + void removeBehavior(Behavior* behavior); + + // Update all behaviors (call each frame) + void update(); + + // Check if a behavior wants to control a specific motor + // Returns true if a behavior provides a position, false otherwise + bool getMotorPosition(uint8_t motorID, uint16_t& position); + +private: + std::vector behaviors; +}; + +// Global behavior manager instance +extern BehaviorManager behaviorManager;