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

Compare commits

base: realrobotshk:main
Branches Tags
realrobotshk:websocket
realrobotshk:main
realrobotshk:protocolv2
realrobotshk:master
..
compare: realrobotshk:protocolv2
Branches Tags
realrobotshk:websocket
realrobotshk:main
realrobotshk:protocolv2
realrobotshk:master

No commits in common. "main" and "protocolv2" have entirely different histories.
main ... protocolv2

9 changed files with 44 additions and 1065 deletions
Show Stats Expand all files Collapse all files

199
ls_firmware.ino → HansonServo.ino
View File

@ -19,7 +19,6 @@
#include "animation.h"
#include "commands.h"
#include "motorcontrol.h"
#include "motoraid.h"
#include "nodegraph.h"
#include "protocol.h"
#include "sensors.h"
@ -399,57 +398,11 @@ void runNodeAnimation() {
}
}
// ============================================================================
// Test Functions
// ============================================================================
void testSweepMotor40() {
static unsigned long lastUpdate = 0;
static uint16_t position = 500;
static int16_t direction = 1;
const unsigned long SWEEP_INTERVAL_MS = 20; // Update every 20ms
const uint16_t MIN_POS = 500;
const uint16_t MAX_POS = 2500;
const uint16_t STEP = 10;
unsigned long now = millis();
if (now - lastUpdate < SWEEP_INTERVAL_MS) {
return;
}
lastUpdate = now;
// Update position
position += (direction * STEP);
// Reverse direction at limits
if (position >= MAX_POS) {
position = MAX_POS;
direction = -1;
} else if (position <= MIN_POS) {
position = MIN_POS;
direction = 1;
}
// Send position to motor 40
uint8_t motorID = 40;
uint16_t positions[1] = {position};
uint16_t speeds[1] = {0};
uint8_t ids[1] = {motorID};
servoManager.syncWritePositions(ids, positions, speeds, 1, config, 0);
}
// ============================================================================
// Motor Position Updates
// ============================================================================
void updateMotorPositions() {
// Only read positions when motor streaming is active
// This prevents blocking the main loop when positions aren't needed
if (!motorStream.active) {
return;
}
static unsigned long lastUpdate = 0;
if (millis() - lastUpdate < MOTOR_UPDATE_INTERVAL_MS)
@ -460,46 +413,15 @@ void updateMotorPositions() {
servoManager[0]->setFeetechMode(motor.servoModel.major);
uint16_t position = servoManager[0]->getPosition(motor.motorID);
config.setMotorPosition(motor.motorID, position);
uint16_t current = servoManager[0]->getCurrent(motor.motorID);
config.setMotorCurrent(motor.motorID, current);
}
}
void printMotorStats() {
Serial.println("--- Motor stats ---");
for (const Motor& motor : config.motors) {
Serial.print("ID:");
Serial.print(motor.motorID);
Serial.print(" pos:");
Serial.print(motor.position);
Serial.print(" cur:");
Serial.println(motor.current);
}
Serial.println("-------------------");
}
void handleMotorStreaming() {
if (motorStream.shouldStream()) {
sendMotorPositions();
}
}
void printLoopRate() {
static unsigned long lastPrint = 0;
static uint32_t loopCount = 0;
loopCount++;
unsigned long now = millis();
if (now - lastPrint >= 1000) {
Serial.print("[Loop] ");
Serial.print(loopCount);
Serial.println(" Hz");
loopCount = 0;
lastPrint = now;
}
}
// ============================================================================
// Heartbeat
// ============================================================================
@ -547,9 +469,6 @@ void setup() {
Serial.setRxBufferSize(8192);
Serial.begin(1000000);
pinMode(6, OUTPUT);
digitalWrite(6, 1);
// Startup delay
delay(500);
Serial.println("\n[HansonServo] Starting...");
@ -570,99 +489,48 @@ void setup() {
// Load or create robot config
if (config.loadOrCreateDefault()) {
Serial.println("[HansonServo] Config loaded: " + config.deviceName);
} else {
Serial.println("[HansonServo] Config init failed");
}
// Initialize behaviors (order determines priority: first added = highest priority)
// Priority: Focus > Viseme > Idle
// NOTE: Don't set enabled state here - let config load restore it, or set defaults after
// 1. Focus behavior (highest priority - radar tracking)
static FocusBehavior focusBehavior;
behaviorManager.addBehavior(BEHAVIOR_FOCUS, &focusBehavior);
behaviorManager.setBehaviorEnabled(BEHAVIOR_FOCUS, true);
// 2. Viseme behavior (medium priority - mouth animation for speech)
// Viseme positions: (id, motor40, motor43, motor44)
visemeBehavior.addViseme(0, 2047, 2047, 2047); // Neutral/rest (sil)
visemeBehavior.addViseme(1, 2200, 1900, 2100); // AA (as in "father")
visemeBehavior.addViseme(2, 2100, 2000, 2000); // AE (as in "cat")
visemeBehavior.addViseme(3, 2150, 1950, 2050); // AH (as in "but")
visemeBehavior.addViseme(4, 2000, 2100, 1950); // AO (as in "bought")
visemeBehavior.addViseme(5, 1900, 2200, 1900); // EH (as in "bed")
visemeBehavior.addViseme(6, 1850, 2250, 1850); // IH (as in "bit")
visemeBehavior.addViseme(7, 1800, 2300, 1800); // IY (as in "beat")
visemeBehavior.addViseme(8, 2300, 1800, 2200); // OW (as in "boat")
visemeBehavior.addViseme(9, 2250, 1850, 2150); // UH (as in "book")
visemeBehavior.addViseme(10, 2200, 1900, 2100); // UW (as in "boot")
behaviorManager.addBehavior(BEHAVIOR_VISEME, &visemeBehavior);
behaviorManager.setBehaviorEnabled(BEHAVIOR_VISEME, true);
// 3. Idle behavior (lowest priority - perlin noise for all motors)
static IdleBehavior idleBehavior;
behaviorManager.addBehavior(BEHAVIOR_IDLE, &idleBehavior);
Serial.println("[HansonServo] Behaviors initialized (focus > viseme > idle)");
// Check if config file exists before loading
bool configExisted = FFat.exists("/robot_config.bin");
// Load full config with behaviors and visemes (will restore their state)
// This must happen BEFORE setting defaults, so saved states aren't overwritten
bool configLoaded = config.loadOrCreateDefault("/robot_config.bin", &behaviorManager, &visemeBehavior);
if (configLoaded) {
Serial.println("[HansonServo] Config loaded: " + config.deviceName);
// If config file existed before, behavior states should have been loaded
// If it's a new file, we need to set defaults
if (!configExisted) {
Serial.println("[HansonServo] New config file, setting default behavior states...");
behaviorManager.setBehaviorEnabled(BEHAVIOR_FOCUS, true);
behaviorManager.setBehaviorEnabled(BEHAVIOR_VISEME, true);
behaviorManager.setBehaviorEnabled(BEHAVIOR_IDLE, true);
// Save the defaults
config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
} else {
Serial.println("[HansonServo] Behavior states loaded from config");
}
// Check if visemes were loaded from config
if (visemeBehavior.getVisemes().empty()) {
Serial.println("[HansonServo] No visemes in config, creating defaults...");
// Only create defaults if config had no visemes
visemeBehavior.addViseme(0, "SIL", 2047, 2047, 2047); // Neutral/rest (sil)
visemeBehavior.addViseme(1, "AA ", 2200, 1900, 2100); // AA (as in "father")
visemeBehavior.addViseme(2, "AE ", 2100, 2000, 2000); // AE (as in "cat")
visemeBehavior.addViseme(3, "AH ", 2150, 1950, 2050); // AH (as in "but")
visemeBehavior.addViseme(4, "AO ", 2000, 2100, 1950); // AO (as in "bought")
visemeBehavior.addViseme(5, "EH ", 1900, 2200, 1900); // EH (as in "bed")
visemeBehavior.addViseme(6, "IH ", 1850, 2250, 1850); // IH (as in "bit")
visemeBehavior.addViseme(7, "IY ", 1800, 2300, 1800); // IY (as in "beat")
visemeBehavior.addViseme(8, "OW ", 2300, 1800, 2200); // OW (as in "boat")
visemeBehavior.addViseme(9, "UH ", 2250, 1850, 2150); // UH (as in "book")
visemeBehavior.addViseme(10, "UW ", 2200, 1900, 2100); // UW (as in "boot")
// Save the defaults
config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
} else {
Serial.println("[HansonServo] Visemes loaded from config: " + String(visemeBehavior.getVisemes().size()));
}
} else {
Serial.println("[HansonServo] Config init failed");
// Set defaults on failure
behaviorManager.setBehaviorEnabled(BEHAVIOR_FOCUS, true);
behaviorManager.setBehaviorEnabled(BEHAVIOR_VISEME, true);
behaviorManager.setBehaviorEnabled(BEHAVIOR_IDLE, true);
}
// Initialize idle behavior motors (needs config.motors to be loaded)
std::vector<uint8_t> allMotorIDs;
for (const Motor& motor : config.motors) {
for (const Motor& motor : config.motors) {
allMotorIDs.push_back(motor.motorID);
}
idleBehavior.initMotors(allMotorIDs);
behaviorManager.addBehavior(BEHAVIOR_IDLE, &idleBehavior);
behaviorManager.setBehaviorEnabled(BEHAVIOR_IDLE, true);
motorStream.start();
// Initialize motor-aided movement
// Motor 25 - default settings
motorAid.addMotor(25);
// Motors 30, 31, 35, 36 - high gear ratio, maximum sensitivity
AidedMotorSettings highGearSettings;
highGearSettings.velocityThreshold = 8; // Extremely sensitive trigger
highGearSettings.resetThreshold = 4; // Minimal hysteresis
highGearSettings.leadOffset = 100; // Smaller lead for fine control
highGearSettings.assistSpeed = 600; // Moderate speed
highGearSettings.assistDuration = 400; // Shorter burst
motorAid.addMotor(30, highGearSettings);
motorAid.addMotor(31, highGearSettings);
motorAid.addMotor(35, highGearSettings);
motorAid.addMotor(36, highGearSettings);
Serial.println("[HansonServo] Behaviors initialized (focus > viseme > idle)");
Serial.println("[HansonServo] Ready");
Serial.println("[HansonServo] Protocol: 0xA5 0x5A tagged packets with CRC16");
@ -717,22 +585,9 @@ void loop() {
updateMotorPositions();
handleMotorStreaming();
// Motor-aided movement (when motorStream is active)
if (motorStream.active) {
motorAid.update();
}
//printMotorStats();
// Sensor updates and streaming
//sensors.update();
sensors.update();
// Heartbeat
//sendHeartbeat();
// Debug: print loop rate
printLoopRate();
// ============================================================================
// TEST: Uncomment to enable motor 40 sweep test
// ============================================================================
//testSweepMotor40();
sendHeartbeat();
}

155
behaviors.cpp
View File

@ -366,66 +366,6 @@ void VisemeBehavior::addViseme(uint8_t id, uint16_t pos40, uint16_t pos43, uint1
addMotor(44);
}
// Overload to add viseme with explicit label
void VisemeBehavior::addViseme(uint8_t id, const char* label, uint16_t pos40, uint16_t pos43, uint16_t pos44) {
Viseme* existing = findViseme(id);
if (existing) {
// Update existing viseme
if (label) {
existing->label[0] = label[0];
existing->label[1] = label[1];
existing->label[2] = label[2];
existing->label[3] = '\0';
}
existing->motorPositions.clear();
existing->motorPositions.push_back({40, pos40});
existing->motorPositions.push_back({43, pos43});
existing->motorPositions.push_back({44, pos44});
} else {
// Add new viseme
Viseme newViseme;
newViseme.id = id;
// Set label
if (label) {
newViseme.label[0] = label[0];
newViseme.label[1] = label[1];
newViseme.label[2] = label[2];
newViseme.label[3] = '\0';
} else {
// Default label
newViseme.label[0] = 'V';
if (id < 10) {
newViseme.label[1] = '0' + id;
newViseme.label[2] = ' ';
} else if (id < 100) {
newViseme.label[1] = '0' + (id / 10);
newViseme.label[2] = '0' + (id % 10);
} else {
newViseme.label[1] = 'X';
newViseme.label[2] = 'X';
}
newViseme.label[3] = '\0';
}
newViseme.motorPositions.push_back({40, pos40});
newViseme.motorPositions.push_back({43, pos43});
newViseme.motorPositions.push_back({44, pos44});
visemes.push_back(newViseme);
// Update nextVisemeID if needed
if (id >= nextVisemeID) {
nextVisemeID = id + 1;
}
}
// Update controlled motors list
addMotor(40);
addMotor(43);
addMotor(44);
}
bool VisemeBehavior::deleteViseme(uint8_t visemeID) {
for (auto it = visemes.begin(); it != visemes.end(); ++it) {
if (it->id == visemeID) {
@ -466,101 +406,6 @@ bool VisemeBehavior::setVisemeMotors(uint8_t visemeID, const std::vector<VisemeM
return true;
}
bool VisemeBehavior::setVisemeMotorsAndLabel(uint8_t visemeID, const char* label, const std::vector<VisemeMotorPosition>& positions) {
Viseme* viseme = findViseme(visemeID);
if (!viseme) {
Serial.print("[Viseme] setVisemeMotorsAndLabel failed - unknown viseme ID ");
Serial.println(visemeID);
return false;
}
// Update label (3 bytes)
if (label) {
viseme->label[0] = label[0];
viseme->label[1] = label[1];
viseme->label[2] = label[2];
viseme->label[3] = '\0';
}
// Update motor positions
viseme->motorPositions = positions;
// Update controlled motors list
for (const auto& pos : positions) {
addMotor(pos.motorID);
}
Serial.print("[Viseme] Updated viseme ID ");
Serial.print(visemeID);
Serial.print(" label '");
Serial.print(viseme->label);
Serial.print("' with ");
Serial.print(positions.size());
Serial.println(" motors");
return true;
}
bool VisemeBehavior::createOrUpdateViseme(uint8_t visemeID, const char* label, const std::vector<VisemeMotorPosition>& positions) {
Viseme* viseme = findViseme(visemeID);
if (viseme) {
// Update existing
return setVisemeMotorsAndLabel(visemeID, label, positions);
} else {
// Create new
Viseme newViseme;
newViseme.id = visemeID;
// Set label
if (label) {
newViseme.label[0] = label[0];
newViseme.label[1] = label[1];
newViseme.label[2] = label[2];
newViseme.label[3] = '\0';
} else {
// Default label
newViseme.label[0] = 'V';
if (visemeID < 10) {
newViseme.label[1] = '0' + visemeID;
newViseme.label[2] = ' ';
} else if (visemeID < 100) {
newViseme.label[1] = '0' + (visemeID / 10);
newViseme.label[2] = '0' + (visemeID % 10);
} else {
newViseme.label[1] = 'X';
newViseme.label[2] = 'X';
}
newViseme.label[3] = '\0';
}
// Set motor positions
newViseme.motorPositions = positions;
visemes.push_back(newViseme);
// Update controlled motors list
for (const auto& pos : positions) {
addMotor(pos.motorID);
}
// Update nextVisemeID if needed
if (visemeID >= nextVisemeID) {
nextVisemeID = visemeID + 1;
}
Serial.print("[Viseme] Created viseme ID ");
Serial.print(visemeID);
Serial.print(" label '");
Serial.print(newViseme.label);
Serial.print("' with ");
Serial.print(positions.size());
Serial.println(" motors");
return true;
}
}
bool VisemeBehavior::triggerViseme(uint8_t visemeID) {
Viseme* viseme = findViseme(visemeID);
if (!viseme) {

11
behaviors.h
View File

@ -163,9 +163,6 @@ public:
// Legacy: Add a viseme with specific ID and motor positions (for backwards compatibility)
void addViseme(uint8_t id, uint16_t pos40, uint16_t pos43, uint16_t pos44);
// Add a viseme with specific ID, label, and motor positions
void addViseme(uint8_t id, const char* label, uint16_t pos40, uint16_t pos43, uint16_t pos44);
// Delete a viseme by ID
// Returns true if deleted, false if not found
bool deleteViseme(uint8_t visemeID);
@ -174,14 +171,6 @@ public:
// Returns true if viseme found and updated, false otherwise
bool setVisemeMotors(uint8_t visemeID, const std::vector<VisemeMotorPosition>& positions);
// Set motor positions and label for a viseme
// Returns true if viseme found and updated, false otherwise
bool setVisemeMotorsAndLabel(uint8_t visemeID, const char* label, const std::vector<VisemeMotorPosition>& positions);
// Create or update a viseme with specific ID, label, and motor positions
// Returns true on success
bool createOrUpdateViseme(uint8_t visemeID, const char* label, const std::vector<VisemeMotorPosition>& positions);
// Get all visemes (for VLST command)
const std::vector<Viseme>& getVisemes() const { return visemes; }

36
commands.cpp
View File

@ -466,9 +466,6 @@ void handleBehavior(const uint8_t* payload, uint16_t len) {
bool enabled = (enable != 0);
behaviorManager.setBehaviorEnabled(static_cast<BehaviorID>(behaviorID), enabled);
// Save config to persist the behavior state change
config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
// Send acknowledgment
sendAck(Tag::BHVR);
@ -577,9 +574,6 @@ void handleVisemeAdd(const uint8_t* payload, uint16_t len) {
// Add the viseme
uint8_t newID = visemeBehavior.addViseme(label);
// Save config to persist the new viseme
config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
// Send ACK with the new ID
uint8_t ackPayload[1] = { newID };
sendPacket(Tag::ACK, ackPayload, 1);
@ -595,8 +589,6 @@ void handleVisemeDelete(const uint8_t* payload, uint16_t len) {
uint8_t visemeID = payload[0];
if (visemeBehavior.deleteViseme(visemeID)) {
// Save config to persist the deletion
config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
sendAck(Tag::VDEL);
} else {
sendNack(Tag::VDEL, "Viseme not found");
@ -604,25 +596,16 @@ void handleVisemeDelete(const uint8_t* payload, uint16_t len) {
}
void handleVisemeSet(const uint8_t* payload, uint16_t len) {
// VSET payload: [viseme_id: 1][label: 3 bytes][motor_count: 1][motor_id: 1][pos_low: 1][pos_high: 1]...
if (len < 5) { // Minimum: viseme_id(1) + label(3) + motor_count(1)
// VSET payload: [viseme_id: 1][motor_count: 1][motor_id: 1][pos_low: 1][pos_high: 1]...
if (len < 2) {
sendNack(Tag::VSET, "Invalid payload length");
return;
}
uint8_t visemeID = payload[0];
uint8_t motorCount = payload[1];
// Extract label (3 bytes)
char label[4];
label[0] = payload[1];
label[1] = payload[2];
label[2] = payload[3];
label[3] = '\0';
uint8_t motorCount = payload[4];
// Calculate expected length: viseme_id(1) + label(3) + motor_count(1) + motors(motorCount * 3)
if (len < 5 + motorCount * 3) {
if (len < 2 + motorCount * 3) {
sendNack(Tag::VSET, "Motor data truncated");
return;
}
@ -630,20 +613,17 @@ void handleVisemeSet(const uint8_t* payload, uint16_t len) {
// Parse motor positions
std::vector<VisemeMotorPosition> positions;
for (uint8_t i = 0; i < motorCount; i++) {
uint16_t offset = 5 + i * 3; // Start after viseme_id(1) + label(3) + motor_count(1)
uint16_t offset = 2 + i * 3;
VisemeMotorPosition mp;
mp.motorID = payload[offset];
mp.position = payload[offset + 1] | (payload[offset + 2] << 8); // Little-endian
mp.position = payload[offset + 1] | (payload[offset + 2] << 8);
positions.push_back(mp);
}
// Use createOrUpdateViseme so VSET can create new visemes or update existing ones
if (visemeBehavior.createOrUpdateViseme(visemeID, label, positions)) {
// Save config to persist the changes
config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
if (visemeBehavior.setVisemeMotors(visemeID, positions)) {
sendAck(Tag::VSET);
} else {
sendNack(Tag::VSET, "Failed to update viseme");
sendNack(Tag::VSET, "Viseme not found");
}
}

13
feetech.cpp
View File

@ -377,16 +377,11 @@ uint8_t Feetech::getMoving(uint8_t id) {
return waitOnData1Byte(10);
}
// STS/SMS: actual current at 0x45, SCS: use load at 0x3C as proxy
// Multiplier could be wrong
uint16_t Feetech::getCurrent(uint8_t id) {
if (feetechMode == MODE_STS || feetechMode == MODE_SMSA || feetechMode == MODE_SMSB) {
sendRequest(id, REQUEST_CURRENT_CURRENT, 2);
return waitOnData2Bytes(10);
} else {
// SCS doesn't have current register, use load as proxy
sendRequest(id, REQUEST_CURRENT_LOAD, 2);
return flipBytes(waitOnData2Bytes(10));
}
sendRequest(id, REQUEST_CURRENT_CURRENT, 2);
//return waitOnData2Bytes(10) * 0.01; FLOAT
return waitOnData2Bytes(10);
}
void Feetech::sendRequest(uint8_t id, byte instruction, uint8_t byteCount) {

221
motoraid.cpp
View File

@ -1,221 +0,0 @@
#include "motoraid.h"
#include "motorcontrol.h"
#include "robotconfig.h"
// Global instance
MotorAid motorAid;
extern RobotConfig config;
void MotorAid::addMotor(uint8_t motorID) {
// Use default settings
AidedMotorSettings defaultSettings;
addMotor(motorID, defaultSettings);
}
void MotorAid::addMotor(uint8_t motorID, const AidedMotorSettings& settings) {
// Don't add duplicates
if (findMotor(motorID) != nullptr) return;
AidedMotor motor;
motor.motorID = motorID;
motor.settings = settings;
// Read actual position directly from servo (not config which may be stale)
uint8_t model = config.getMotorModel(motorID);
servoManager[0]->setFeetechMode(model);
motor.lastPosition = servoManager[0]->getPosition(motorID);
motor.stablePosition = motor.lastPosition; // Initialize stable position
motor.lastUpdateTime = millis();
// Initialize velocity samples to zero
for (uint8_t i = 0; i < VELOCITY_SAMPLES; i++) {
motor.velocitySamples[i] = 0;
}
aidedMotors.push_back(motor);
// Disable torque on this motor so it can be moved freely
servoManager[0]->disableTorque(motorID);
Serial.print("[MotorAid] Added motor ");
Serial.print(motorID);
Serial.print(" at pos ");
Serial.print(motor.lastPosition);
Serial.print(" (thresh=");
Serial.print(settings.velocityThreshold);
Serial.print(", lead=");
Serial.print(settings.leadOffset);
Serial.println(")");
}
void MotorAid::removeMotor(uint8_t motorID) {
for (auto it = aidedMotors.begin(); it != aidedMotors.end(); ++it) {
if (it->motorID == motorID) {
aidedMotors.erase(it);
Serial.print("[MotorAid] Removed motor ");
Serial.println(motorID);
return;
}
}
}
bool MotorAid::isMotorAided(uint8_t motorID) const {
return findMotor(motorID) != nullptr;
}
AidedMotorSettings* MotorAid::getMotorSettings(uint8_t motorID) {
AidedMotor* motor = findMotor(motorID);
if (motor) return &motor->settings;
return nullptr;
}
AidedMotor* MotorAid::findMotor(uint8_t motorID) {
for (auto& motor : aidedMotors) {
if (motor.motorID == motorID) return &motor;
}
return nullptr;
}
const AidedMotor* MotorAid::findMotor(uint8_t motorID) const {
for (const auto& motor : aidedMotors) {
if (motor.motorID == motorID) return &motor;
}
return nullptr;
}
int16_t MotorAid::calculateSmoothedVelocity(AidedMotor& motor) {
// Calculate average of velocity samples
int32_t sum = 0;
for (uint8_t i = 0; i < VELOCITY_SAMPLES; i++) {
sum += motor.velocitySamples[i];
}
return (int16_t)(sum / VELOCITY_SAMPLES);
}
void MotorAid::update() {
unsigned long now = millis();
for (AidedMotor& motor : aidedMotors) {
const AidedMotorSettings& s = motor.settings; // Shorthand
// Calculate time delta
unsigned long deltaTime = now - motor.lastUpdateTime;
if (deltaTime < updateInterval) continue;
// Get current position from config (already updated by updateMotorPositions)
uint16_t currentPosition = config.getMotorPosition(motor.motorID);
// Calculate position delta from stable position (filters noise)
int16_t positionDelta = (int16_t)currentPosition - (int16_t)motor.stablePosition;
// Only register movement if it exceeds minimum delta (noise filter)
int16_t instantVelocity = 0;
if (abs(positionDelta) >= MIN_POSITION_DELTA) {
// Significant movement - calculate velocity and update stable position
instantVelocity = (positionDelta * 1000) / (int16_t)deltaTime;
motor.stablePosition = currentPosition;
}
// else: tiny movement, treat as zero velocity (noise)
// Add to velocity samples (circular buffer)
motor.velocitySamples[motor.sampleIndex] = instantVelocity;
motor.sampleIndex = (motor.sampleIndex + 1) % VELOCITY_SAMPLES;
// Track samples collected for warmup
if (motor.samplesCollected < VELOCITY_SAMPLES * 2) {
motor.samplesCollected++;
}
// Don't calculate or trigger until we have enough samples (2x buffer = ~300ms warmup)
if (motor.samplesCollected < VELOCITY_SAMPLES * 2) {
continue; // Still warming up
}
motor.warmedUp = true;
// Calculate smoothed velocity
motor.smoothedVelocity = calculateSmoothedVelocity(motor);
// Update tracking
motor.lastUpdateTime = now;
// Check if currently assisting
if (motor.assisting) {
// Calculate target position AHEAD of current position in direction of movement
int16_t leadOffset = (motor.smoothedVelocity > 0) ? s.leadOffset : -s.leadOffset;
motor.assistTargetPosition = constrain((int16_t)currentPosition + leadOffset, 0, 4095);
// Send updated position command to lead the movement
uint8_t model = config.getMotorModel(motor.motorID);
servoManager[0]->setFeetechMode(model);
uint8_t ids[1] = { motor.motorID };
uint16_t positions[1] = { motor.assistTargetPosition };
uint16_t speeds[1] = { s.assistSpeed };
servoManager.syncWritePositions(ids, positions, speeds, 1, config, 0);
// Check if assist duration has elapsed (fixed duration, always ends)
if (now - motor.assistStartTime >= s.assistDuration) {
motor.assisting = false;
// Disable torque again
servoManager[0]->disableTorque(motor.motorID);
Serial.print("[MotorAid] Assist ended, motor ");
Serial.print(motor.motorID);
Serial.print(" at pos ");
Serial.println(currentPosition);
}
continue;
}
// Hysteresis: if we were triggered, wait for velocity to drop before re-triggering
if (motor.wasTriggered) {
if (abs(motor.smoothedVelocity) < s.resetThreshold) {
motor.wasTriggered = false; // Reset, can trigger again
motor.consecutiveHighCount = 0; // Reset consecutive counter too
}
continue; // Don't trigger while in hysteresis zone
}
// Check if smoothed velocity exceeds threshold (in either direction)
if (abs(motor.smoothedVelocity) > s.velocityThreshold) {
motor.consecutiveHighCount++;
} else {
motor.consecutiveHighCount = 0; // Reset on low reading
}
// Only trigger after CONSECUTIVE_REQUIRED high readings (filters noise spikes)
if (motor.consecutiveHighCount >= CONSECUTIVE_REQUIRED) {
// Sustained movement detected - assist!
motor.assisting = true;
motor.assistStartTime = now;
motor.wasTriggered = true;
// Calculate target position AHEAD of current position in direction of movement
int16_t leadOffset = (motor.smoothedVelocity > 0) ? s.leadOffset : -s.leadOffset;
motor.assistTargetPosition = constrain((int16_t)currentPosition + leadOffset, 0, 4095);
// Set servo mode based on motor model
uint8_t model = config.getMotorModel(motor.motorID);
servoManager[0]->setFeetechMode(model);
// Enable torque and move AHEAD to assist
servoManager[0]->enableTorque(motor.motorID);
uint8_t ids[1] = { motor.motorID };
uint16_t positions[1] = { motor.assistTargetPosition };
uint16_t speeds[1] = { s.assistSpeed };
servoManager.syncWritePositions(ids, positions, speeds, 1, config, 0);
// Debug output
Serial.print("[MotorAid] Assisting motor ");
Serial.print(motor.motorID);
Serial.print(" vel=");
Serial.print(motor.smoothedVelocity);
Serial.print(" pos=");
Serial.print(currentPosition);
Serial.print(" -> ");
Serial.println(motor.assistTargetPosition);
}
}
}

82
motoraid.h
View File

@ -1,82 +0,0 @@
#pragma once
#include <Arduino.h>
#include <vector>
// Motor-aided movement: detects external manipulation and assists to reduce gear stress
// Works by monitoring position velocity - when someone moves the motor manually,
// it briefly enables torque and moves to the current position to assist.
constexpr uint8_t VELOCITY_SAMPLES = 4; // Number of samples for velocity smoothing (fewer = faster response)
constexpr int16_t MIN_POSITION_DELTA = 1; // Ignore position changes smaller than this (minimum for high gear ratio)
constexpr uint8_t CONSECUTIVE_REQUIRED = 3; // Require this many consecutive high readings to trigger
struct AidedMotorSettings {
int16_t velocityThreshold = 80; // Position units/sec to trigger assist (low for high gear ratio)
int16_t resetThreshold = 30; // Velocity must drop below this to re-trigger
int16_t leadOffset = 200; // How far ahead to command (position units)
uint16_t assistSpeed = 1000; // Speed to use when assisting
unsigned long assistDuration = 500; // ms to assist for
};
struct AidedMotor {
uint8_t motorID;
uint16_t lastPosition = 0;
unsigned long lastUpdateTime = 0;
// Per-motor settings
AidedMotorSettings settings;
// Velocity smoothing
int16_t velocitySamples[VELOCITY_SAMPLES] = {0};
uint8_t sampleIndex = 0;
uint8_t samplesCollected = 0; // Track how many samples we've collected
int16_t smoothedVelocity = 0;
uint16_t stablePosition = 0; // Position used for delta calculation (filters noise)
// Assist state
bool assisting = false;
unsigned long assistStartTime = 0;
uint16_t assistTargetPosition = 0;
// Hysteresis - prevents re-triggering until velocity drops
bool wasTriggered = false;
// Warmup - don't trigger until we have enough samples
bool warmedUp = false;
// Consecutive high readings counter (filters noise spikes)
uint8_t consecutiveHighCount = 0;
};
class MotorAid {
public:
// Add a motor with default settings
void addMotor(uint8_t motorID);
// Add a motor with custom settings
void addMotor(uint8_t motorID, const AidedMotorSettings& settings);
// Remove a motor from aided list
void removeMotor(uint8_t motorID);
// Check if a motor is being aided
bool isMotorAided(uint8_t motorID) const;
// Get settings for a motor (to modify)
AidedMotorSettings* getMotorSettings(uint8_t motorID);
// Main update function - call from loop() when motorStream is active
void update();
private:
std::vector<AidedMotor> aidedMotors;
unsigned long updateInterval = 30; // ms between position checks
AidedMotor* findMotor(uint8_t motorID);
const AidedMotor* findMotor(uint8_t motorID) const;
// Calculate smoothed velocity from samples
int16_t calculateSmoothedVelocity(AidedMotor& motor);
};
extern MotorAid motorAid;

325
robotconfig.cpp
View File

@ -1,5 +1,4 @@
#include "robotconfig.h"
#include "behaviors.h"
#include <FFat.h>
uint16_t RobotConfig::getMotorPosition(uint8_t motorID) const {
@ -12,15 +11,6 @@ uint16_t RobotConfig::getMotorPosition(uint8_t motorID) const {
return 2047;
}
uint16_t RobotConfig::getMotorCurrent(uint8_t motorID) const {
for (const Motor& motor : motors) {
if (motor.motorID == motorID) {
return motor.current;
}
}
return 0;
}
uint8_t RobotConfig::getMotorModel(uint8_t motorID) {
for (const Motor& motor : motors) {
if (motor.motorID == motorID) {
@ -42,16 +32,6 @@ bool RobotConfig::setMotorPosition(uint8_t motorID, uint16_t newPosition) {
return false;
}
bool RobotConfig::setMotorCurrent(uint8_t motorID, uint16_t newCurrent) {
for (Motor& motor : motors) {
if (motor.motorID == motorID) {
motor.current = newCurrent;
return true;
}
}
return false;
}
bool RobotConfig::setMotorEnabled(uint8_t motorID, bool enable) {
for (Motor& motor : motors) {
if (motor.motorID == motorID) {
@ -218,310 +198,10 @@ bool RobotConfig::loadFromFFat(const char* path) {
return true;
}
// Legacy method - calls new version with null pointers
bool RobotConfig::loadOrCreateDefault(const char* path) {
return loadOrCreateDefault(path, nullptr, nullptr);
}
// ============================================================================
// Key-Value Format V2 (Compact, Extensible)
// ============================================================================
bool RobotConfig::saveToFFatV2(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior) const {
File file = FFat.open(path, FILE_WRITE);
if (!file) return false;
// Write version header
file.write((uint8_t)0x02); // Version 2
// Count settings (we'll write this after we know the count)
uint16_t settingCount = 0;
size_t countPos = file.position();
file.write((uint8_t)0); // Placeholder for count low byte
file.write((uint8_t)0); // Placeholder for count high byte
// Setting 1: Device Name
if (deviceName.length() > 0) {
file.write((uint8_t)(KEY_DEVICE_NAME & 0xFF));
file.write((uint8_t)((KEY_DEVICE_NAME >> 8) & 0xFF));
file.write(TYPE_STRING);
uint8_t nameLen = deviceName.length();
file.write(nameLen);
file.write((const uint8_t*)deviceName.c_str(), nameLen);
settingCount++;
}
// Setting 2: Firmware Major
file.write((uint8_t)(KEY_FIRMWARE_MAJOR & 0xFF));
file.write((uint8_t)((KEY_FIRMWARE_MAJOR >> 8) & 0xFF));
file.write(TYPE_UINT8);
file.write(firmwareVersion.major);
settingCount++;
// Setting 3: Firmware Minor
file.write((uint8_t)(KEY_FIRMWARE_MINOR & 0xFF));
file.write((uint8_t)((KEY_FIRMWARE_MINOR >> 8) & 0xFF));
file.write(TYPE_UINT8);
file.write(firmwareVersion.minor);
settingCount++;
// Setting 4: Motor Array
if (!motors.empty()) {
file.write((uint8_t)(KEY_MOTOR_ARRAY & 0xFF));
file.write((uint8_t)((KEY_MOTOR_ARRAY >> 8) & 0xFF));
file.write(TYPE_MOTOR_ARRAY);
uint8_t motorCount = motors.size();
file.write(motorCount);
for (const Motor& m : motors) {
file.write(m.motorID);
file.write(m.servoModel.major);
file.write(m.servoModel.minor);
uint8_t nameLen = m.name.length();
file.write(nameLen);
if (nameLen > 0) {
file.write((const uint8_t*)m.name.c_str(), nameLen);
}
}
settingCount++;
}
// Setting 5: Behavior States
if (behaviorManager) {
file.write((uint8_t)(KEY_BEHAVIOR_STATES & 0xFF));
file.write((uint8_t)((KEY_BEHAVIOR_STATES >> 8) & 0xFF));
file.write(TYPE_BEHAVIOR_STATES);
// Count enabled behaviors
uint8_t behaviorCount = behaviorManager->getBehaviorCount();
file.write(behaviorCount);
// Write behaviorID + enabled state pairs
for (uint8_t i = 0; i < behaviorCount; i++) {
BehaviorID id;
bool enabled;
if (behaviorManager->getBehaviorInfo(i, id, enabled)) {
file.write((uint8_t)id);
file.write(enabled ? 1 : 0);
}
}
settingCount++;
}
// Setting 6: Viseme Array
if (visemeBehavior) {
const std::vector<Viseme>& visemes = visemeBehavior->getVisemes();
if (!visemes.empty()) {
file.write((uint8_t)(KEY_VISEME_ARRAY & 0xFF));
file.write((uint8_t)((KEY_VISEME_ARRAY >> 8) & 0xFF));
file.write(TYPE_VISEME_ARRAY);
uint8_t visemeCount = visemes.size();
file.write(visemeCount);
for (const Viseme& v : visemes) {
file.write(v.id);
// Label (3 bytes)
file.write((uint8_t)v.label[0]);
file.write((uint8_t)v.label[1]);
file.write((uint8_t)v.label[2]);
// Motor positions
uint8_t motorCount = v.motorPositions.size();
file.write(motorCount);
for (const VisemeMotorPosition& mp : v.motorPositions) {
file.write(mp.motorID);
file.write((uint8_t)(mp.position & 0xFF)); // position low
file.write((uint8_t)((mp.position >> 8) & 0xFF)); // position high
}
}
settingCount++;
}
}
// Write setting count at the beginning
size_t endPos = file.position();
file.seek(countPos);
file.write((uint8_t)(settingCount & 0xFF));
file.write((uint8_t)((settingCount >> 8) & 0xFF));
file.seek(endPos);
file.close();
return true;
}
bool RobotConfig::loadFromFFatV2(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior) {
File file = FFat.open(path, FILE_READ);
if (!file) return false;
// Read version
uint8_t version = file.read();
if (version != 0x02) {
file.close();
return false; // Wrong version
}
// Read setting count
uint16_t settingCount = file.read();
settingCount |= (file.read() << 8);
motors.clear();
// Parse each setting
for (uint16_t i = 0; i < settingCount; i++) {
// Read key
uint16_t key = file.read();
key |= (file.read() << 8);
// Read type
uint8_t type = file.read();
switch (key) {
case KEY_DEVICE_NAME: {
if (type == TYPE_STRING) {
uint8_t nameLen = file.read();
char nameBuf[64] = {0};
if (nameLen > 0 && nameLen < sizeof(nameBuf)) {
file.readBytes(nameBuf, nameLen);
deviceName = String(nameBuf);
}
}
break;
}
case KEY_FIRMWARE_MAJOR: {
if (type == TYPE_UINT8) {
firmwareVersion.major = file.read();
}
break;
}
case KEY_FIRMWARE_MINOR: {
if (type == TYPE_UINT8) {
firmwareVersion.minor = file.read();
}
break;
}
case KEY_MOTOR_ARRAY: {
if (type == TYPE_MOTOR_ARRAY) {
uint8_t motorCount = file.read();
for (uint8_t j = 0; j < motorCount; j++) {
Motor m;
m.motorID = file.read();
m.servoModel.major = file.read();
m.servoModel.minor = file.read();
uint8_t nameLen = file.read();
char nameBuf[64] = {0};
if (nameLen > 0 && nameLen < sizeof(nameBuf)) {
file.readBytes(nameBuf, nameLen);
m.name = String(nameBuf);
}
m.position = 0;
m.isEnabled = true;
motors.push_back(m);
}
}
break;
}
case KEY_BEHAVIOR_STATES: {
if (type == TYPE_BEHAVIOR_STATES && behaviorManager) {
uint8_t behaviorCount = file.read();
for (uint8_t j = 0; j < behaviorCount; j++) {
BehaviorID id = (BehaviorID)file.read();
bool enabled = file.read() != 0;
behaviorManager->setBehaviorEnabled(id, enabled);
}
}
break;
}
case KEY_VISEME_ARRAY: {
if (type == TYPE_VISEME_ARRAY && visemeBehavior) {
uint8_t visemeCount = file.read();
for (uint8_t j = 0; j < visemeCount; j++) {
uint8_t visemeID = file.read();
// Read label (3 bytes)
char label[4];
label[0] = file.read();
label[1] = file.read();
label[2] = file.read();
label[3] = '\0';
// Read motor positions
uint8_t motorCount = file.read();
std::vector<VisemeMotorPosition> positions;
for (uint8_t k = 0; k < motorCount; k++) {
VisemeMotorPosition mp;
mp.motorID = file.read();
uint16_t posLow = file.read();
uint16_t posHigh = file.read();
mp.position = posLow | (posHigh << 8);
positions.push_back(mp);
}
// Create or update viseme
visemeBehavior->createOrUpdateViseme(visemeID, label, positions);
}
}
break;
}
default:
// Unknown key - skip based on type
switch (type) {
case TYPE_UINT8: file.read(); break;
case TYPE_UINT16: file.read(); file.read(); break;
case TYPE_UINT32: file.read(); file.read(); file.read(); file.read(); break;
case TYPE_BOOL: file.read(); break;
case TYPE_INT8: file.read(); break;
case TYPE_INT16: file.read(); file.read(); break;
case TYPE_INT32: file.read(); file.read(); file.read(); file.read(); break;
case TYPE_FLOAT: file.read(); file.read(); file.read(); file.read(); break;
case TYPE_STRING: {
uint8_t len = file.read();
for (uint8_t k = 0; k < len; k++) file.read();
break;
}
default:
file.close();
return false; // Unknown type
}
break;
}
}
file.close();
return true;
}
bool RobotConfig::loadOrCreateDefault(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior) {
if (FFat.exists(path)) {
Serial.println("Loading robot config from FFat...");
// Try V2 format first
if (loadFromFFatV2(path, behaviorManager, visemeBehavior)) {
Serial.println("Loaded V2 format");
return true;
}
// Fall back to V1 format
if (loadFromFFat(path)) {
Serial.println("Loaded V1 format (legacy)");
// Upgrade to V2 format
saveToFFatV2(path, behaviorManager, visemeBehavior);
return true;
}
return loadFromFFat(path);
}
Serial.println("No config found. Creating default config...");
@ -529,7 +209,8 @@ bool RobotConfig::loadOrCreateDefault(const char* path, BehaviorManager* behavio
// 🔧 Define your default config here
deviceName = "DefaultBot";
firmwareVersion = { 1, 0 };
motors.clear();
return saveToFFatV2(path, behaviorManager, visemeBehavior);
return saveToFFat(path);
}

65
robotconfig.h
View File

@ -2,50 +2,6 @@
#include <Arduino.h>
#include <vector>
// Forward declarations
class BehaviorManager;
class VisemeBehavior;
// ============================================================================
// Config Key-Value System
// ============================================================================
enum ConfigKey : uint16_t {
// Basic settings
KEY_DEVICE_NAME = 0x0001,
KEY_FIRMWARE_MAJOR = 0x0002,
KEY_FIRMWARE_MINOR = 0x0003,
// Motor array (single entry containing all motors)
KEY_MOTOR_ARRAY = 0x0100,
// Behavior states (array of behaviorID + enabled pairs)
KEY_BEHAVIOR_STATES = 0x0200,
// Viseme array (single entry containing all visemes)
KEY_VISEME_ARRAY = 0x0300,
// Future extensible settings
KEY_SERIAL_BAUD = 0x0400,
KEY_MOTOR_UPDATE_INTERVAL = 0x0401,
// ... add more as needed
};
enum ConfigType : uint8_t {
TYPE_UINT8 = 0x01,
TYPE_UINT16 = 0x02,
TYPE_UINT32 = 0x03,
TYPE_BOOL = 0x04,
TYPE_INT8 = 0x05,
TYPE_INT16 = 0x06,
TYPE_INT32 = 0x07,
TYPE_FLOAT = 0x08,
TYPE_STRING = 0x09,
TYPE_MOTOR_ARRAY = 0x0A, // Special type for motor array
TYPE_BEHAVIOR_STATES = 0x0B, // Special type for behavior state array
TYPE_VISEME_ARRAY = 0x0C, // Special type for viseme array
};
struct FirmwareVersion {
uint8_t major;
uint8_t minor;
@ -61,7 +17,6 @@ struct Motor {
uint8_t motorID;
ServoModel servoModel;
uint16_t position;
uint16_t current = 0;
bool isEnabled = true;
};
@ -71,34 +26,16 @@ struct RobotConfig {
std::vector<Motor> motors;
uint16_t getMotorPosition(uint8_t motorID) const;
uint16_t getMotorCurrent(uint8_t motorID) const;
uint8_t getMotorModel(uint8_t motorID);
bool setMotorPosition(uint8_t motorID, uint16_t newPosition);
bool setMotorCurrent(uint8_t motorID, uint16_t newCurrent);
bool setMotorEnabled(uint8_t motorID, bool enable);
bool isMotorEnabled(uint8_t motorID);
void enableAllMotors();
String serializeJSON() const;
std::vector<uint8_t> serializeToBytes() const;
// Legacy format (v1)
bool saveToFFat(const char* path = "/robot_config.bin") const;
bool loadFromFFat(const char* path = "/robot_config.bin");
// New key-value format (v2)
bool saveToFFatV2(const char* path = "/robot_config.bin",
BehaviorManager* behaviorManager = nullptr,
VisemeBehavior* visemeBehavior = nullptr) const;
bool loadFromFFatV2(const char* path = "/robot_config.bin",
BehaviorManager* behaviorManager = nullptr,
VisemeBehavior* visemeBehavior = nullptr);
// New version with behavior/viseme support
bool loadOrCreateDefault(const char* path = "/robot_config.bin",
BehaviorManager* behaviorManager = nullptr,
VisemeBehavior* visemeBehavior = nullptr);
// Legacy version (for backward compatibility)
bool loadOrCreateDefault(const char* path);
bool loadOrCreateDefault(const char* path = "/robot_config.bin");
};