realrobotshk
/
HansonServo
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added behaviours to the serial protocol

protocolv2
Jake 2026-01-21 14:49:48 +08:00
parent 7db6f2ab26
commit d7158ea1d0
6 changed files with 252 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

122
HansonServo.ino
View File

@ -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);
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<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);
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");
@ -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();

67
behaviors.cpp
View File

@ -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
}

36
behaviors.h
View File

@ -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<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

85
commands.cpp
View File

@ -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
// ============================================================================

4
commands.h
View File

@ -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);

4
protocol.h
View File

@ -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