added behaviours to the serial protocol

HansonServo.ino

@@ -52,6 +52,67 @@ uint16_t getSineWaveValue(unsigned long centiseconds) {
   return (uint16_t)round(scaled);
 }
 
+// ============================================================================
+// Unified Motor Command Application
+// ============================================================================
+
+// Apply motor commands from behaviors and/or animations
+// This function works whether an animation is active or not
+// Behaviors have priority - they override animation positions
+void applyMotorCommands(const std::vector<uint8_t>& motorIDs, 
+                        const std::vector<uint16_t>& positions) {
+  if (motorIDs.empty()) return;
+  
+  std::vector<uint8_t> finalMotorIDs;
+  std::vector<uint16_t> finalPositions;
+  std::vector<uint16_t> speeds;
+  
+  for (size_t i = 0; i < motorIDs.size(); i++) {
+    uint8_t motorID = motorIDs[i];
+    uint16_t finalPosition = positions[i];
+    
+    // Check if a behavior wants to control this motor (behaviors have priority)
+    uint16_t behaviorPosition;
+    if (behaviorManager.getMotorPosition(motorID, behaviorPosition)) {
+      // Behavior is controlling this motor, use its position instead
+      finalPosition = behaviorPosition;
+    }
+    
+    finalMotorIDs.push_back(motorID);
+    finalPositions.push_back(finalPosition);
+    speeds.push_back(0);
+    config.setMotorPosition(motorID, finalPosition);
+    config.setMotorEnabled(motorID, true);
+  }
+  
+  // Send all positions in one sync write
+  if (!finalMotorIDs.empty()) {
+    servoManager.syncWritePositions(finalMotorIDs.data(), finalPositions.data(),
+                                    speeds.data(), finalMotorIDs.size(), config, 0);
+  }
+}
+
+// Apply motor commands from behaviors only (when no animation is active)
+void applyBehaviorMotorCommands() {
+  // Get all motors that behaviors want to control
+  std::vector<uint8_t> motorIDs;
+  std::vector<uint16_t> positions;
+  
+  // Check all configured motors to see if any behavior wants to control them
+  for (const Motor& motor : config.motors) {
+    uint16_t position;
+    if (behaviorManager.getMotorPosition(motor.motorID, position)) {
+      motorIDs.push_back(motor.motorID);
+      positions.push_back(position);
+    }
+  }
+  
+  // Apply the behavior-controlled motors
+  if (!motorIDs.empty()) {
+    applyMotorCommands(motorIDs, positions);
+  }
+}
+
 // ============================================================================
 // Protocol Handler
 // ============================================================================
@@ -133,30 +194,14 @@ void runFrameAnimation() {
     // Collect motor commands from frame data
     std::vector<uint8_t> motorIDs;
     std::vector<uint16_t> positions;
-    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(finalPosition);
+      positions.push_back(motorPos.position);
-      speeds.push_back(0);
-      config.setMotorPosition(motorPos.motorID, finalPosition);
-      config.setMotorEnabled(motorPos.motorID, true);
     }
 
-    // Send all positions in one sync write
+    // Apply motor commands (behaviors will override if needed)
-    if (!motorIDs.empty()) {
+    applyMotorCommands(motorIDs, positions);
-      servoManager.syncWritePositions(motorIDs.data(), positions.data(),
-                                      speeds.data(), motorIDs.size(), config, 0);
-    }
     
     // Debug: print frame and motor positions
     // Serial.print("Frame ");
@@ -224,13 +269,13 @@ void runFrameAnimation() {
           done[3] = 1; // complete
           sendPacket(Tag::FRAME, done, 4);
           animState.stop();
-        } else {
+  } else {
           currentTick = animState.startFrame;
         }
         break;
       default:
         break;
-    }
+      }
   }
 }
 
@@ -262,7 +307,7 @@ void runNodeAnimation() {
   uint32_t now = millis();
   if (now - lastTickTime < frameIntervalMs)
     return;
-  lastTickTime = now;
+    lastTickTime = now;
 
   // Tick the node graph
   animState.current->nodeGraph.tick(currentTick, *animState.current);
@@ -271,24 +316,11 @@ void runNodeAnimation() {
   // Collect motor commands
     std::vector<uint8_t> motorIDs;
     std::vector<uint16_t> positions;
-    std::vector<uint16_t> speeds;
 
   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(finalPosition);
+        positions.push_back(value);
-        speeds.push_back(0);
-        config.setMotorPosition(motorID, finalPosition);
-      config.setMotorEnabled(motorID, true);
       } else {
       // Only disable torque for motors that should be limp
         if (config.setMotorEnabled(motorID, false)) {
@@ -297,11 +329,8 @@ void runNodeAnimation() {
     }
   }
 
-  // Send all positions in one sync write - motors move together!
+  // Apply motor commands (behaviors will override if needed)
-  if (!motorIDs.empty()) {
+  applyMotorCommands(motorIDs, positions);
-    servoManager.syncWritePositions(motorIDs.data(), positions.data(),
-                                    speeds.data(), motorIDs.size(), config, 0);
-  }
 
   // Emit per-frame event: [frameLo, frameHi, playMode, status=0]
   // Send actual frame number (currentTick), not relative frame
@@ -360,8 +389,8 @@ void runNodeAnimation() {
       } else {
         // Reset to start frame for next repeat
         currentTick = animState.startFrame;
-      }
+          }
-      break;
+          break;
     default:
       break;
     }
@@ -469,7 +498,7 @@ void setup() {
 
   // Initialize behaviors
   static FocusBehavior focusBehavior;
-  behaviorManager.addBehavior(&focusBehavior);
+  behaviorManager.addBehavior(BEHAVIOR_FOCUS, &focusBehavior);
   Serial.println("[HansonServo] Behaviors initialized");
 
   Serial.println("[HansonServo] Ready");
@@ -516,6 +545,11 @@ void loop() {
   // Animation playback (auto-selects v1 node or v2 frame based on version)
   runAnimation();
   
+  // If no animation is active, still apply behavior motor commands
+  if (!animState.current || !animState.current->isActive()) {
+    applyBehaviorMotorCommands();
+  }
+
   // Motor position updates
   updateMotorPositions();
   handleMotorStreaming();

behaviors.cpp

@@ -76,11 +76,8 @@ bool FocusBehavior::getMotorPosition(uint8_t motorID, uint16_t& position) {
     return false;
   }
 
-  // Only provide position if behavior is active
+  // Always provide position for controlled motors (active by default)
-  if (!isActive) {
+  // When not tracking a target, currentPosition is set to center in update()
-    return false;
-  }
-
   position = currentPosition;
   return true;
 }
@@ -127,41 +124,73 @@ 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(Behavior* behavior) {
+void BehaviorManager::addBehavior(BehaviorID id, Behavior* behavior) {
   if (behavior == nullptr) return;
   
   // Check if already added
-  for (Behavior* b : behaviors) {
+  for (const auto& entry : behaviors) {
-    if (b == behavior) return;
+    if (entry.behavior == behavior || entry.id == id) return;
   }
   
-  behaviors.push_back(behavior);
+  behaviors.push_back({id, behavior});
+  // New behaviors are enabled by default
+  enabledStates[id] = true;
 }
 
 void BehaviorManager::removeBehavior(Behavior* behavior) {
   behaviors.erase(
-    std::remove(behaviors.begin(), behaviors.end(), behavior),
+    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 behaviors
+  // Update all enabled behaviors
-  for (Behavior* behavior : behaviors) {
+  for (const auto& entry : behaviors) {
-    if (behavior) {
+    if (entry.behavior && enabledStates[entry.id]) {
-      behavior->update();
+      entry.behavior->update();
     }
   }
 }
 
 bool BehaviorManager::getMotorPosition(uint8_t motorID, uint16_t& position) {
-  // Check all behaviors to see if any wants to control this motor
+  // Check all enabled behaviors to see if any wants to control this motor
-  for (Behavior* behavior : behaviors) {
+  for (const auto& entry : behaviors) {
-    if (behavior && behavior->getMotorPosition(motorID, position)) {
+    if (entry.behavior && enabledStates[entry.id] && 
-      return true;  // Found a behavior controlling this motor
+        entry.behavior->getMotorPosition(motorID, position)) {
+      return true;  // Found an enabled behavior controlling this motor
     }
   }
-  return false;  // No behavior controlling this motor
+  return false;  // No enabled behavior controlling this motor
 }

behaviors.h

@@ -4,6 +4,15 @@
 #include "sensors.h"
 #include "robotconfig.h"
 
+// ============================================================================
+// Behavior IDs
+// ============================================================================
+
+enum BehaviorID : uint8_t {
+  BEHAVIOR_FOCUS = 1,  // Focus behavior (radar tracking)
+  // Add more behavior IDs here as needed
+};
+
 // ============================================================================
 // Base Behavior Class
 // ============================================================================
@@ -76,13 +85,26 @@ class BehaviorManager {
 public:
   BehaviorManager();
   
-  // Add a behavior to the manager
+  // Add a behavior to the manager with an ID
-  void addBehavior(Behavior* behavior);
+  void addBehavior(BehaviorID id, Behavior* behavior);
   
   // Remove a behavior from the manager
   void removeBehavior(Behavior* behavior);
   
-  // Update all behaviors (call each frame)
+  // Enable/disable a behavior by ID
+  void setBehaviorEnabled(BehaviorID id, bool enabled);
+  
+  // Check if a behavior is enabled
+  bool isBehaviorEnabled(BehaviorID id) const;
+  
+  // Get count of registered behaviors
+  uint8_t getBehaviorCount() const;
+  
+  // Get behavior info at index (for iteration)
+  // Returns true if index is valid and fills out id and enabled
+  bool getBehaviorInfo(uint8_t index, BehaviorID& id, bool& enabled) const;
+  
+  // Update all enabled behaviors (call each frame)
   void update();
   
   // Check if a behavior wants to control a specific motor
@@ -90,7 +112,13 @@ public:
   bool getMotorPosition(uint8_t motorID, uint16_t& position);
 
 private:
-  std::vector<Behavior*> behaviors;
+  struct BehaviorEntry {
+    BehaviorID id;
+    Behavior* behavior;
+  };
+  
+  std::vector<BehaviorEntry> behaviors;
+  bool enabledStates[256] = {false};  // Track enabled state by ID
 };
 
 // Global behavior manager instance

commands.cpp

@@ -1,6 +1,7 @@
 #include "commands.h"
 #include "nodegraph.h"
 #include "sensors.h"
+#include "behaviors.h"
 #include "esp_system.h"
 #include "soc/rtc_cntl_reg.h"
 #include <vector>
@@ -128,6 +129,13 @@ void dispatchCommand() {
   else if (tagMatches(tag, Tag::MSTRM)) {
     handleMotorStream(payload, len);
   }
+  // Behaviors
+  else if (tagMatches(tag, Tag::BHVR)) {
+    handleBehavior(payload, len);
+  }
+  else if (tagMatches(tag, Tag::BLST)) {
+    handleBehaviorList(payload, len);
+  }
   // System
   else if (tagMatches(tag, Tag::BOOT)) {
     handleBootloader(payload, len);
@@ -418,6 +426,83 @@ void handleFileStop(const uint8_t* payload, uint16_t len) {
   sendMessage("Animation stopped");
 }
 
+// ============================================================================
+// Behavior Handler
+// ============================================================================
+
+void handleBehavior(const uint8_t* payload, uint16_t len) {
+  // BHVR payload: [behaviorID (1 byte)][enable (1 byte)]
+  if (len < 2) {
+    sendNack(Tag::BHVR, "Invalid payload length");
+    return;
+  }
+  
+  uint8_t behaviorID = payload[0];
+  uint8_t enable = payload[1];
+  
+  // Validate behavior ID
+  if (behaviorID == 0 || behaviorID > 255) {
+    sendNack(Tag::BHVR, "Invalid behavior ID");
+    return;
+  }
+  
+  // Enable/disable the behavior
+  bool enabled = (enable != 0);
+  behaviorManager.setBehaviorEnabled(static_cast<BehaviorID>(behaviorID), enabled);
+  
+  // Send acknowledgment
+  sendAck(Tag::BHVR);
+  
+  // Send status message
+  String status = enabled ? "enabled" : "disabled";
+  sendMessage("Behavior " + String(behaviorID) + " " + status);
+}
+
+void handleBehaviorList(const uint8_t* payload, uint16_t len) {
+  // BLST payload: none (can be empty)
+  // Response: [count (1 byte)][behaviorID1 (1 byte)][enabled1 (1 byte)]...
+  
+  uint8_t count = behaviorManager.getBehaviorCount();
+  
+  // Build response payload: [count][id1][enabled1][id2][enabled2]...
+  std::vector<uint8_t> response;
+  response.push_back(count);
+  
+  for (uint8_t i = 0; i < count; i++) {
+    BehaviorID id;
+    bool enabled;
+    if (behaviorManager.getBehaviorInfo(i, id, enabled)) {
+      response.push_back(static_cast<uint8_t>(id));
+      response.push_back(enabled ? 1 : 0);
+    }
+  }
+  
+  // Send response packet
+  sendPacket(Tag::BLST, response.data(), response.size());
+  
+  // Also send debug message with behavior names
+  String msg = "Behaviors: ";
+  for (uint8_t i = 0; i < count; i++) {
+    BehaviorID id;
+    bool enabled;
+    if (behaviorManager.getBehaviorInfo(i, id, enabled)) {
+      if (i > 0) msg += ", ";
+      String name;
+      switch (id) {
+        case BEHAVIOR_FOCUS:
+          name = "Focus";
+          break;
+        default:
+          name = "Unknown(" + String(static_cast<uint8_t>(id)) + ")";
+          break;
+      }
+      msg += name + "(" + String(static_cast<uint8_t>(id)) + ")=";
+      msg += enabled ? "ON" : "OFF";
+    }
+  }
+  sendMessage(msg);
+}
+
 // ============================================================================
 // Motor Control Handlers
 // ============================================================================

commands.h

@@ -69,6 +69,10 @@ void handleMotorScan(const uint8_t* payload, uint16_t len);
 void handleMotorWrite(const uint8_t* payload, uint16_t len);
 void handleMotorStream(const uint8_t* payload, uint16_t len);
 
+// Behaviors
+void handleBehavior(const uint8_t* payload, uint16_t len);
+void handleBehaviorList(const uint8_t* payload, uint16_t len);
+
 // System
 void handleBootloader(const uint8_t* payload, uint16_t len);
 

protocol.h

@@ -46,6 +46,10 @@ namespace Tag {
   constexpr char RADAR[4] = {'R','D','A','R'};  // Radar targets
   constexpr char FRAME[4] = {'F','R','M','E'};  // Animation frame events (frame, mode, status)
   
+  // Behaviors
+  constexpr char BHVR[4]  = {'B','H','V','R'};  // Behavior control (activate/deactivate)
+  constexpr char BLST[4]  = {'B','L','S','T'};  // Behavior list (list all behaviors and their states)
+  
   // System
   constexpr char STATE[4] = {'S','T','A','T'};  // System state/heartbeat
   constexpr char MSGE[4]  = {'M','S','G','E'};  // Log/debug message