diff --git a/HansonServo.ino b/HansonServo.ino index 55e8d73..3f39392 100644 --- a/HansonServo.ino +++ b/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& motorIDs, + const std::vector& positions) { + if (motorIDs.empty()) return; + + std::vector finalMotorIDs; + std::vector finalPositions; + std::vector 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 motorIDs; + std::vector 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 // ============================================================================ @@ -89,7 +150,7 @@ void runAnimation() { if (!animState.current || !animState.current->isActive()) { return; } - + if (animState.current->header.version == 2) { runFrameAnimation(); } else { @@ -133,30 +194,14 @@ void runFrameAnimation() { // Collect motor commands from frame data std::vector motorIDs; std::vector positions; - 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(finalPosition); - speeds.push_back(0); - config.setMotorPosition(motorPos.motorID, finalPosition); - config.setMotorEnabled(motorPos.motorID, true); + positions.push_back(motorPos.position); } - // Send all positions in one sync write - if (!motorIDs.empty()) { - servoManager.syncWritePositions(motorIDs.data(), positions.data(), - speeds.data(), motorIDs.size(), config, 0); - } + // Apply motor commands (behaviors will override if needed) + applyMotorCommands(motorIDs, positions); // 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 motorIDs; std::vector positions; - std::vector 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); - speeds.push_back(0); - config.setMotorPosition(motorID, finalPosition); - config.setMotorEnabled(motorID, true); + positions.push_back(value); } 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! - if (!motorIDs.empty()) { - servoManager.syncWritePositions(motorIDs.data(), positions.data(), - speeds.data(), motorIDs.size(), config, 0); - } + // Apply motor commands (behaviors will override if needed) + applyMotorCommands(motorIDs, positions); // 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"); @@ -515,6 +544,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(); diff --git a/behaviors.cpp b/behaviors.cpp index 28ee05d..7601f3a 100644 --- a/behaviors.cpp +++ b/behaviors.cpp @@ -76,11 +76,8 @@ bool FocusBehavior::getMotorPosition(uint8_t motorID, uint16_t& position) { return false; } - // Only provide position if behavior is active - if (!isActive) { - return false; - } - + // Always provide position for controlled motors (active by default) + // When not tracking a target, currentPosition is set to center in update() 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) { - if (b == behavior) return; + for (const auto& entry : behaviors) { + 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 - for (Behavior* behavior : behaviors) { - if (behavior) { - behavior->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 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 + // 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 behavior controlling this motor + return false; // No enabled behavior controlling this motor } diff --git a/behaviors.h b/behaviors.h index 59233d0..7863a05 100644 --- a/behaviors.h +++ b/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 - void addBehavior(Behavior* behavior); + // Add a behavior to the manager with an ID + 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 behaviors; + struct BehaviorEntry { + BehaviorID id; + Behavior* behavior; + }; + + std::vector behaviors; + bool enabledStates[256] = {false}; // Track enabled state by ID }; // Global behavior manager instance diff --git a/commands.cpp b/commands.cpp index b185d7a..f01ecab 100644 --- a/commands.cpp +++ b/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 @@ -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), 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 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(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(id)) + ")"; + break; + } + msg += name + "(" + String(static_cast(id)) + ")="; + msg += enabled ? "ON" : "OFF"; + } + } + sendMessage(msg); +} + // ============================================================================ // Motor Control Handlers // ============================================================================ diff --git a/commands.h b/commands.h index 460b104..3c468d2 100644 --- a/commands.h +++ b/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); diff --git a/protocol.h b/protocol.h index c31b3e1..d5d35e0 100644 --- a/protocol.h +++ b/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