added behaviour module which can override animation for chosen motors, eg for eyes focusing on radar target

HansonServo.ino

@@ -22,6 +22,7 @@
 #include "nodegraph.h"
 #include "protocol.h"
 #include "sensors.h"
+#include "behaviors.h"
 #include <FFat.h>
 
 
@@ -135,10 +136,19 @@ void runFrameAnimation() {
     std::vector<uint16_t> 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();

behaviors.cpp

@@ -0,0 +1,167 @@
+#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;
+  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
+}

behaviors.h

@@ -0,0 +1,97 @@
+#pragma once
+#include <Arduino.h>
+#include <vector>
+#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<uint8_t>& 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<uint8_t> 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<Behavior*> behaviors;
+};
+
+// Global behavior manager instance
+extern BehaviorManager behaviorManager;