Compare commits
No commits in common. "main" and "protocolv2" have entirely different histories.
main
...
protocolv2
|
|
@ -19,7 +19,6 @@
|
||||||
#include "animation.h"
|
#include "animation.h"
|
||||||
#include "commands.h"
|
#include "commands.h"
|
||||||
#include "motorcontrol.h"
|
#include "motorcontrol.h"
|
||||||
#include "motoraid.h"
|
|
||||||
#include "nodegraph.h"
|
#include "nodegraph.h"
|
||||||
#include "protocol.h"
|
#include "protocol.h"
|
||||||
#include "sensors.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
|
// Motor Position Updates
|
||||||
// ============================================================================
|
// ============================================================================
|
||||||
|
|
||||||
void updateMotorPositions() {
|
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;
|
static unsigned long lastUpdate = 0;
|
||||||
|
|
||||||
if (millis() - lastUpdate < MOTOR_UPDATE_INTERVAL_MS)
|
if (millis() - lastUpdate < MOTOR_UPDATE_INTERVAL_MS)
|
||||||
|
|
@ -460,46 +413,15 @@ void updateMotorPositions() {
|
||||||
servoManager[0]->setFeetechMode(motor.servoModel.major);
|
servoManager[0]->setFeetechMode(motor.servoModel.major);
|
||||||
uint16_t position = servoManager[0]->getPosition(motor.motorID);
|
uint16_t position = servoManager[0]->getPosition(motor.motorID);
|
||||||
config.setMotorPosition(motor.motorID, position);
|
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() {
|
void handleMotorStreaming() {
|
||||||
if (motorStream.shouldStream()) {
|
if (motorStream.shouldStream()) {
|
||||||
sendMotorPositions();
|
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
|
// Heartbeat
|
||||||
// ============================================================================
|
// ============================================================================
|
||||||
|
|
@ -547,9 +469,6 @@ void setup() {
|
||||||
Serial.setRxBufferSize(8192);
|
Serial.setRxBufferSize(8192);
|
||||||
Serial.begin(1000000);
|
Serial.begin(1000000);
|
||||||
|
|
||||||
pinMode(6, OUTPUT);
|
|
||||||
digitalWrite(6, 1);
|
|
||||||
|
|
||||||
// Startup delay
|
// Startup delay
|
||||||
delay(500);
|
delay(500);
|
||||||
Serial.println("\n[HansonServo] Starting...");
|
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)
|
// Initialize behaviors (order determines priority: first added = highest priority)
|
||||||
// Priority: Focus > Viseme > Idle
|
// 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)
|
// 1. Focus behavior (highest priority - radar tracking)
|
||||||
static FocusBehavior focusBehavior;
|
static FocusBehavior focusBehavior;
|
||||||
behaviorManager.addBehavior(BEHAVIOR_FOCUS, &focusBehavior);
|
behaviorManager.addBehavior(BEHAVIOR_FOCUS, &focusBehavior);
|
||||||
|
behaviorManager.setBehaviorEnabled(BEHAVIOR_FOCUS, true);
|
||||||
|
|
||||||
// 2. Viseme behavior (medium priority - mouth animation for speech)
|
// 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.addBehavior(BEHAVIOR_VISEME, &visemeBehavior);
|
||||||
|
behaviorManager.setBehaviorEnabled(BEHAVIOR_VISEME, true);
|
||||||
|
|
||||||
// 3. Idle behavior (lowest priority - perlin noise for all motors)
|
// 3. Idle behavior (lowest priority - perlin noise for all motors)
|
||||||
static IdleBehavior idleBehavior;
|
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;
|
std::vector<uint8_t> allMotorIDs;
|
||||||
for (const Motor& motor : config.motors) {
|
for (const Motor& motor : config.motors) {
|
||||||
allMotorIDs.push_back(motor.motorID);
|
allMotorIDs.push_back(motor.motorID);
|
||||||
}
|
}
|
||||||
idleBehavior.initMotors(allMotorIDs);
|
idleBehavior.initMotors(allMotorIDs);
|
||||||
|
behaviorManager.addBehavior(BEHAVIOR_IDLE, &idleBehavior);
|
||||||
motorStream.start();
|
behaviorManager.setBehaviorEnabled(BEHAVIOR_IDLE, true);
|
||||||
|
|
||||||
// Initialize motor-aided movement
|
Serial.println("[HansonServo] Behaviors initialized (focus > viseme > idle)");
|
||||||
// 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] Ready");
|
Serial.println("[HansonServo] Ready");
|
||||||
Serial.println("[HansonServo] Protocol: 0xA5 0x5A tagged packets with CRC16");
|
Serial.println("[HansonServo] Protocol: 0xA5 0x5A tagged packets with CRC16");
|
||||||
|
|
@ -716,23 +584,10 @@ void loop() {
|
||||||
// Motor position updates
|
// Motor position updates
|
||||||
updateMotorPositions();
|
updateMotorPositions();
|
||||||
handleMotorStreaming();
|
handleMotorStreaming();
|
||||||
|
|
||||||
// Motor-aided movement (when motorStream is active)
|
|
||||||
if (motorStream.active) {
|
|
||||||
motorAid.update();
|
|
||||||
}
|
|
||||||
//printMotorStats();
|
|
||||||
// Sensor updates and streaming
|
// Sensor updates and streaming
|
||||||
//sensors.update();
|
sensors.update();
|
||||||
|
|
||||||
// Heartbeat
|
// Heartbeat
|
||||||
//sendHeartbeat();
|
sendHeartbeat();
|
||||||
|
|
||||||
// Debug: print loop rate
|
|
||||||
printLoopRate();
|
|
||||||
|
|
||||||
// ============================================================================
|
|
||||||
// TEST: Uncomment to enable motor 40 sweep test
|
|
||||||
// ============================================================================
|
|
||||||
//testSweepMotor40();
|
|
||||||
}
|
}
|
||||||
155
behaviors.cpp
155
behaviors.cpp
|
|
@ -366,66 +366,6 @@ void VisemeBehavior::addViseme(uint8_t id, uint16_t pos40, uint16_t pos43, uint1
|
||||||
addMotor(44);
|
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) {
|
bool VisemeBehavior::deleteViseme(uint8_t visemeID) {
|
||||||
for (auto it = visemes.begin(); it != visemes.end(); ++it) {
|
for (auto it = visemes.begin(); it != visemes.end(); ++it) {
|
||||||
if (it->id == visemeID) {
|
if (it->id == visemeID) {
|
||||||
|
|
@ -466,101 +406,6 @@ bool VisemeBehavior::setVisemeMotors(uint8_t visemeID, const std::vector<VisemeM
|
||||||
return true;
|
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) {
|
bool VisemeBehavior::triggerViseme(uint8_t visemeID) {
|
||||||
Viseme* viseme = findViseme(visemeID);
|
Viseme* viseme = findViseme(visemeID);
|
||||||
if (!viseme) {
|
if (!viseme) {
|
||||||
|
|
|
||||||
11
behaviors.h
11
behaviors.h
|
|
@ -163,9 +163,6 @@ public:
|
||||||
// Legacy: Add a viseme with specific ID and motor positions (for backwards compatibility)
|
// 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);
|
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
|
// Delete a viseme by ID
|
||||||
// Returns true if deleted, false if not found
|
// Returns true if deleted, false if not found
|
||||||
bool deleteViseme(uint8_t visemeID);
|
bool deleteViseme(uint8_t visemeID);
|
||||||
|
|
@ -174,14 +171,6 @@ public:
|
||||||
// Returns true if viseme found and updated, false otherwise
|
// Returns true if viseme found and updated, false otherwise
|
||||||
bool setVisemeMotors(uint8_t visemeID, const std::vector<VisemeMotorPosition>& positions);
|
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)
|
// Get all visemes (for VLST command)
|
||||||
const std::vector<Viseme>& getVisemes() const { return visemes; }
|
const std::vector<Viseme>& getVisemes() const { return visemes; }
|
||||||
|
|
||||||
|
|
|
||||||
36
commands.cpp
36
commands.cpp
|
|
@ -466,9 +466,6 @@ void handleBehavior(const uint8_t* payload, uint16_t len) {
|
||||||
bool enabled = (enable != 0);
|
bool enabled = (enable != 0);
|
||||||
behaviorManager.setBehaviorEnabled(static_cast<BehaviorID>(behaviorID), enabled);
|
behaviorManager.setBehaviorEnabled(static_cast<BehaviorID>(behaviorID), enabled);
|
||||||
|
|
||||||
// Save config to persist the behavior state change
|
|
||||||
config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
|
|
||||||
|
|
||||||
// Send acknowledgment
|
// Send acknowledgment
|
||||||
sendAck(Tag::BHVR);
|
sendAck(Tag::BHVR);
|
||||||
|
|
||||||
|
|
@ -577,9 +574,6 @@ void handleVisemeAdd(const uint8_t* payload, uint16_t len) {
|
||||||
// Add the viseme
|
// Add the viseme
|
||||||
uint8_t newID = visemeBehavior.addViseme(label);
|
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
|
// Send ACK with the new ID
|
||||||
uint8_t ackPayload[1] = { newID };
|
uint8_t ackPayload[1] = { newID };
|
||||||
sendPacket(Tag::ACK, ackPayload, 1);
|
sendPacket(Tag::ACK, ackPayload, 1);
|
||||||
|
|
@ -595,8 +589,6 @@ void handleVisemeDelete(const uint8_t* payload, uint16_t len) {
|
||||||
uint8_t visemeID = payload[0];
|
uint8_t visemeID = payload[0];
|
||||||
|
|
||||||
if (visemeBehavior.deleteViseme(visemeID)) {
|
if (visemeBehavior.deleteViseme(visemeID)) {
|
||||||
// Save config to persist the deletion
|
|
||||||
config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
|
|
||||||
sendAck(Tag::VDEL);
|
sendAck(Tag::VDEL);
|
||||||
} else {
|
} else {
|
||||||
sendNack(Tag::VDEL, "Viseme not found");
|
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) {
|
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]...
|
// VSET payload: [viseme_id: 1][motor_count: 1][motor_id: 1][pos_low: 1][pos_high: 1]...
|
||||||
if (len < 5) { // Minimum: viseme_id(1) + label(3) + motor_count(1)
|
if (len < 2) {
|
||||||
sendNack(Tag::VSET, "Invalid payload length");
|
sendNack(Tag::VSET, "Invalid payload length");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t visemeID = payload[0];
|
uint8_t visemeID = payload[0];
|
||||||
|
uint8_t motorCount = payload[1];
|
||||||
|
|
||||||
// Extract label (3 bytes)
|
if (len < 2 + motorCount * 3) {
|
||||||
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) {
|
|
||||||
sendNack(Tag::VSET, "Motor data truncated");
|
sendNack(Tag::VSET, "Motor data truncated");
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
@ -630,20 +613,17 @@ void handleVisemeSet(const uint8_t* payload, uint16_t len) {
|
||||||
// Parse motor positions
|
// Parse motor positions
|
||||||
std::vector<VisemeMotorPosition> positions;
|
std::vector<VisemeMotorPosition> positions;
|
||||||
for (uint8_t i = 0; i < motorCount; i++) {
|
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;
|
VisemeMotorPosition mp;
|
||||||
mp.motorID = payload[offset];
|
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);
|
positions.push_back(mp);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Use createOrUpdateViseme so VSET can create new visemes or update existing ones
|
if (visemeBehavior.setVisemeMotors(visemeID, positions)) {
|
||||||
if (visemeBehavior.createOrUpdateViseme(visemeID, label, positions)) {
|
|
||||||
// Save config to persist the changes
|
|
||||||
config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior);
|
|
||||||
sendAck(Tag::VSET);
|
sendAck(Tag::VSET);
|
||||||
} else {
|
} else {
|
||||||
sendNack(Tag::VSET, "Failed to update viseme");
|
sendNack(Tag::VSET, "Viseme not found");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
13
feetech.cpp
13
feetech.cpp
|
|
@ -377,16 +377,11 @@ uint8_t Feetech::getMoving(uint8_t id) {
|
||||||
return waitOnData1Byte(10);
|
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) {
|
uint16_t Feetech::getCurrent(uint8_t id) {
|
||||||
if (feetechMode == MODE_STS || feetechMode == MODE_SMSA || feetechMode == MODE_SMSB) {
|
sendRequest(id, REQUEST_CURRENT_CURRENT, 2);
|
||||||
sendRequest(id, REQUEST_CURRENT_CURRENT, 2);
|
//return waitOnData2Bytes(10) * 0.01; FLOAT
|
||||||
return waitOnData2Bytes(10);
|
return waitOnData2Bytes(10);
|
||||||
} else {
|
|
||||||
// SCS doesn't have current register, use load as proxy
|
|
||||||
sendRequest(id, REQUEST_CURRENT_LOAD, 2);
|
|
||||||
return flipBytes(waitOnData2Bytes(10));
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void Feetech::sendRequest(uint8_t id, byte instruction, uint8_t byteCount) {
|
void Feetech::sendRequest(uint8_t id, byte instruction, uint8_t byteCount) {
|
||||||
|
|
|
||||||
221
motoraid.cpp
221
motoraid.cpp
|
|
@ -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
82
motoraid.h
|
|
@ -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
325
robotconfig.cpp
|
|
@ -1,5 +1,4 @@
|
||||||
#include "robotconfig.h"
|
#include "robotconfig.h"
|
||||||
#include "behaviors.h"
|
|
||||||
#include <FFat.h>
|
#include <FFat.h>
|
||||||
|
|
||||||
uint16_t RobotConfig::getMotorPosition(uint8_t motorID) const {
|
uint16_t RobotConfig::getMotorPosition(uint8_t motorID) const {
|
||||||
|
|
@ -12,15 +11,6 @@ uint16_t RobotConfig::getMotorPosition(uint8_t motorID) const {
|
||||||
return 2047;
|
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) {
|
uint8_t RobotConfig::getMotorModel(uint8_t motorID) {
|
||||||
for (const Motor& motor : motors) {
|
for (const Motor& motor : motors) {
|
||||||
if (motor.motorID == motorID) {
|
if (motor.motorID == motorID) {
|
||||||
|
|
@ -42,16 +32,6 @@ bool RobotConfig::setMotorPosition(uint8_t motorID, uint16_t newPosition) {
|
||||||
return false;
|
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) {
|
bool RobotConfig::setMotorEnabled(uint8_t motorID, bool enable) {
|
||||||
for (Motor& motor : motors) {
|
for (Motor& motor : motors) {
|
||||||
if (motor.motorID == motorID) {
|
if (motor.motorID == motorID) {
|
||||||
|
|
@ -218,310 +198,10 @@ bool RobotConfig::loadFromFFat(const char* path) {
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Legacy method - calls new version with null pointers
|
|
||||||
bool RobotConfig::loadOrCreateDefault(const char* path) {
|
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)) {
|
if (FFat.exists(path)) {
|
||||||
Serial.println("Loading robot config from FFat...");
|
Serial.println("Loading robot config from FFat...");
|
||||||
// Try V2 format first
|
return loadFromFFat(path);
|
||||||
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;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
Serial.println("No config found. Creating default config...");
|
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
|
// 🔧 Define your default config here
|
||||||
deviceName = "DefaultBot";
|
deviceName = "DefaultBot";
|
||||||
firmwareVersion = { 1, 0 };
|
firmwareVersion = { 1, 0 };
|
||||||
|
|
||||||
motors.clear();
|
motors.clear();
|
||||||
|
|
||||||
return saveToFFatV2(path, behaviorManager, visemeBehavior);
|
return saveToFFat(path);
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -2,50 +2,6 @@
|
||||||
#include <Arduino.h>
|
#include <Arduino.h>
|
||||||
#include <vector>
|
#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 {
|
struct FirmwareVersion {
|
||||||
uint8_t major;
|
uint8_t major;
|
||||||
uint8_t minor;
|
uint8_t minor;
|
||||||
|
|
@ -61,7 +17,6 @@ struct Motor {
|
||||||
uint8_t motorID;
|
uint8_t motorID;
|
||||||
ServoModel servoModel;
|
ServoModel servoModel;
|
||||||
uint16_t position;
|
uint16_t position;
|
||||||
uint16_t current = 0;
|
|
||||||
bool isEnabled = true;
|
bool isEnabled = true;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
@ -71,34 +26,16 @@ struct RobotConfig {
|
||||||
std::vector<Motor> motors;
|
std::vector<Motor> motors;
|
||||||
|
|
||||||
uint16_t getMotorPosition(uint8_t motorID) const;
|
uint16_t getMotorPosition(uint8_t motorID) const;
|
||||||
uint16_t getMotorCurrent(uint8_t motorID) const;
|
|
||||||
uint8_t getMotorModel(uint8_t motorID);
|
uint8_t getMotorModel(uint8_t motorID);
|
||||||
bool setMotorPosition(uint8_t motorID, uint16_t newPosition);
|
bool setMotorPosition(uint8_t motorID, uint16_t newPosition);
|
||||||
bool setMotorCurrent(uint8_t motorID, uint16_t newCurrent);
|
|
||||||
bool setMotorEnabled(uint8_t motorID, bool enable);
|
bool setMotorEnabled(uint8_t motorID, bool enable);
|
||||||
bool isMotorEnabled(uint8_t motorID);
|
bool isMotorEnabled(uint8_t motorID);
|
||||||
void enableAllMotors();
|
void enableAllMotors();
|
||||||
String serializeJSON() const;
|
String serializeJSON() const;
|
||||||
std::vector<uint8_t> serializeToBytes() const;
|
std::vector<uint8_t> serializeToBytes() const;
|
||||||
|
|
||||||
// Legacy format (v1)
|
|
||||||
bool saveToFFat(const char* path = "/robot_config.bin") const;
|
bool saveToFFat(const char* path = "/robot_config.bin") const;
|
||||||
bool loadFromFFat(const char* path = "/robot_config.bin");
|
bool loadFromFFat(const char* path = "/robot_config.bin");
|
||||||
|
bool loadOrCreateDefault(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);
|
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue