diff --git a/PROTOCOL_MIGRATION.md b/PROTOCOL_MIGRATION.md index c891296..9b80212 100644 --- a/PROTOCOL_MIGRATION.md +++ b/PROTOCOL_MIGRATION.md @@ -114,6 +114,7 @@ ushort Crc16Ccitt(byte[] data) | `STAT` | System state/heartbeat | | `FACE` | Face detection data (Radxa → host/robot, via WiFi) | | `ALIV` | Remote component alive/heartbeat (component ID + status) | +| `SSET` | Settings set/dump (read/write individual settings by ID) | | `ACK!` | Acknowledge (success) | | `NACK` | Negative acknowledge (failure) | | `BOOT` | Enter bootloader | @@ -421,6 +422,64 @@ For each motor: --- +### Settings + +#### `SSET` - Settings Set/Dump (host ↔ device) + +**Write a single setting:** +**Request:** +``` +[setting_id: 2 bytes LE] +[value: 2 bytes LE] +``` +**Response:** `ACK!` on success, `NACK` if unknown setting ID + +**Dump all settings:** +**Request:** Empty payload (0 bytes) +**Response:** `SSET` packet: +``` +[count: 2 bytes LE] +For each setting: + [setting_id: 2 bytes LE] + [value: 2 bytes LE] +``` + +**Value encoding:** +- `uint8`/`uint16`/`bool`: stored directly as uint16 +- `float` (0.0–65.535): stored as `value × 1000` (e.g., 0.15 → 150) +- `int16` (signed): stored as uint16 reinterpret (e.g., -140 → 0xFF74) + +**Focus Behavior Setting IDs:** + +| ID | Name | Type | Default | Description | +|----|------|------|---------|-------------| +| `0x0500` | FOCUS_EYE_MOTOR_1 | uint8 | 14 | Eye motor 1 ID | +| `0x0501` | FOCUS_EYE_MOTOR_2 | uint8 | 15 | Eye motor 2 ID | +| `0x0502` | FOCUS_NECK_MOTOR | uint8 | 27 | Neck yaw motor ID | +| `0x0503` | FOCUS_EYE_CENTER | uint16 | 2200 | Eye servo center position | +| `0x0504` | FOCUS_EYE_MIN | uint16 | 1700 | Eye servo min position | +| `0x0505` | FOCUS_EYE_MAX | uint16 | 2500 | Eye servo max position | +| `0x0506` | FOCUS_NECK_CENTER | uint16 | 2000 | Neck servo center position | +| `0x0507` | FOCUS_NECK_MIN | uint16 | 1000 | Neck servo min position | +| `0x0508` | FOCUS_NECK_MAX | uint16 | 3000 | Neck servo max position | +| `0x0509` | FOCUS_FACE_X_MIN | int16 | -140 | Face detection X min (pixels) | +| `0x050A` | FOCUS_FACE_X_MAX | int16 | 140 | Face detection X max (pixels) | +| `0x050B` | FOCUS_EYE_SPEED | float×1000 | 150 | Eye dart speed (0–1) | +| `0x050C` | FOCUS_NECK_SPEED | float×1000 | 20 | Neck follow speed (0–1) | +| `0x050D` | FOCUS_EYE_RETURN_SPEED | float×1000 | 50 | Eye return-to-center speed | +| `0x050E` | FOCUS_NECK_DELAY_MS | uint16 | 500 | Neck start delay (ms) | +| `0x050F` | FOCUS_NECK_CONTRIBUTION | float×1000 | 700 | Neck offset coverage (0–1) | +| `0x0510` | FOCUS_NECK_INVERT | bool | 1 | Invert neck motor direction | +| `0x0511` | FOCUS_EYE_CENTERING | float×1000 | 30 | Eye centering speed (no face) | +| `0x0512` | FOCUS_NECK_CENTERING | float×1000 | 20 | Neck centering speed (no face) | + +**Notes:** +- Settings are persisted to flash on write +- The dump response includes all registered settings (currently Focus only, extensible) +- Future setting ranges (e.g., `0x0600+`) can be added for other behaviors + +--- + ### System #### `STAT` - System State/Heartbeat (device → host) diff --git a/behaviors.cpp b/behaviors.cpp index 4b5d739..ff25e34 100644 --- a/behaviors.cpp +++ b/behaviors.cpp @@ -572,6 +572,7 @@ bool VisemeBehavior::getMotorPosition(uint8_t motorID, uint16_t& position) { // ============================================================================ BehaviorManager behaviorManager; +FocusBehavior focusBehavior; VisemeBehavior visemeBehavior; BehaviorManager::BehaviorManager() { diff --git a/behaviors.h b/behaviors.h index 01ea3a3..83febc5 100644 --- a/behaviors.h +++ b/behaviors.h @@ -52,6 +52,44 @@ protected: // Focus Behavior - Tracks faces with eyes/neck via FaceDetect sensor // ============================================================================ +// Setting IDs for SSET protocol command +// Float values are transmitted as fixed-point × 1000 (e.g., 0.15 → 150) +// Signed values are transmitted as int16 reinterpreted as uint16 +namespace SettingID { + // Focus: Motor IDs + constexpr uint16_t FOCUS_EYE_MOTOR_1 = 0x0500; + constexpr uint16_t FOCUS_EYE_MOTOR_2 = 0x0501; + constexpr uint16_t FOCUS_NECK_MOTOR = 0x0502; + // Focus: Eye servo range + constexpr uint16_t FOCUS_EYE_CENTER = 0x0503; + constexpr uint16_t FOCUS_EYE_MIN = 0x0504; + constexpr uint16_t FOCUS_EYE_MAX = 0x0505; + // Focus: Neck servo range + constexpr uint16_t FOCUS_NECK_CENTER = 0x0506; + constexpr uint16_t FOCUS_NECK_MIN = 0x0507; + constexpr uint16_t FOCUS_NECK_MAX = 0x0508; + // Focus: Face x range (int16, signed) + constexpr uint16_t FOCUS_FACE_X_MIN = 0x0509; + constexpr uint16_t FOCUS_FACE_X_MAX = 0x050A; + // Focus: Interpolation speeds (float × 1000) + constexpr uint16_t FOCUS_EYE_SPEED = 0x050B; + constexpr uint16_t FOCUS_NECK_SPEED = 0x050C; + constexpr uint16_t FOCUS_EYE_RETURN_SPEED = 0x050D; + // Focus: Neck delay (ms, uint16) + constexpr uint16_t FOCUS_NECK_DELAY_MS = 0x050E; + // Focus: Neck contribution (float × 1000) + constexpr uint16_t FOCUS_NECK_CONTRIBUTION = 0x050F; + // Focus: Neck invert (0 or 1) + constexpr uint16_t FOCUS_NECK_INVERT = 0x0510; + // Focus: Centering speeds (float × 1000) + constexpr uint16_t FOCUS_EYE_CENTERING = 0x0511; + constexpr uint16_t FOCUS_NECK_CENTERING = 0x0512; + + constexpr uint16_t FOCUS_FIRST = 0x0500; + constexpr uint16_t FOCUS_LAST = 0x0512; + constexpr uint16_t FOCUS_COUNT = FOCUS_LAST - FOCUS_FIRST + 1; +} + // Tuneable settings - all values exposed for external adjustment struct FocusSettings { // Motor IDs @@ -289,5 +327,6 @@ private: // Global behavior manager instance extern BehaviorManager behaviorManager; -// Global viseme behavior instance (for command access) +// Global behavior instances (for command/config access) +extern FocusBehavior focusBehavior; extern VisemeBehavior visemeBehavior; diff --git a/commands.cpp b/commands.cpp index f4928c6..40d1eb0 100644 --- a/commands.cpp +++ b/commands.cpp @@ -152,6 +152,10 @@ void dispatchCommand() { else if (tagMatches(tag, Tag::VSME)) { handleVisemeTrigger(payload, len); } + // Settings + else if (tagMatches(tag, Tag::SSET)) { + handleSettingsSet(payload, len); + } // System else if (tagMatches(tag, Tag::BOOT)) { handleBootloader(payload, len); @@ -467,7 +471,7 @@ void handleBehavior(const uint8_t* payload, uint16_t len) { behaviorManager.setBehaviorEnabled(static_cast(behaviorID), enabled); // Save config to persist the behavior state change - config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior); + config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior); // Send acknowledgment sendAck(Tag::BHVR); @@ -578,7 +582,7 @@ void handleVisemeAdd(const uint8_t* payload, uint16_t len) { uint8_t newID = visemeBehavior.addViseme(label); // Save config to persist the new viseme - config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior); + config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior); // Send ACK with the new ID uint8_t ackPayload[1] = { newID }; @@ -596,7 +600,7 @@ void handleVisemeDelete(const uint8_t* payload, uint16_t len) { if (visemeBehavior.deleteViseme(visemeID)) { // Save config to persist the deletion - config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior); + config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior); sendAck(Tag::VDEL); } else { sendNack(Tag::VDEL, "Viseme not found"); @@ -640,7 +644,7 @@ void handleVisemeSet(const uint8_t* payload, uint16_t len) { // 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); + config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior); sendAck(Tag::VSET); } else { sendNack(Tag::VSET, "Failed to update viseme"); @@ -827,6 +831,115 @@ void handleMotorStream(const uint8_t* payload, uint16_t len) { // ============================================================================ // System Handlers // ============================================================================ +// Settings (SSET) - Read/write individual settings by ID +// ============================================================================ + +// Get a setting value by ID. Returns true if valid ID, fills value. +static bool getSettingValue(uint16_t id, uint16_t& value) { + FocusSettings& fs = focusBehavior.getSettings(); + + switch (id) { + case SettingID::FOCUS_EYE_MOTOR_1: value = fs.eyeMotor1; break; + case SettingID::FOCUS_EYE_MOTOR_2: value = fs.eyeMotor2; break; + case SettingID::FOCUS_NECK_MOTOR: value = fs.neckMotor; break; + case SettingID::FOCUS_EYE_CENTER: value = fs.eyeCenter; break; + case SettingID::FOCUS_EYE_MIN: value = fs.eyeMin; break; + case SettingID::FOCUS_EYE_MAX: value = fs.eyeMax; break; + case SettingID::FOCUS_NECK_CENTER: value = fs.neckCenter; break; + case SettingID::FOCUS_NECK_MIN: value = fs.neckMin; break; + case SettingID::FOCUS_NECK_MAX: value = fs.neckMax; break; + case SettingID::FOCUS_FACE_X_MIN: value = (uint16_t)(int16_t)fs.faceXMin; break; + case SettingID::FOCUS_FACE_X_MAX: value = (uint16_t)(int16_t)fs.faceXMax; break; + case SettingID::FOCUS_EYE_SPEED: value = (uint16_t)(fs.eyeSpeed * 1000.0f); break; + case SettingID::FOCUS_NECK_SPEED: value = (uint16_t)(fs.neckSpeed * 1000.0f); break; + case SettingID::FOCUS_EYE_RETURN_SPEED: value = (uint16_t)(fs.eyeReturnSpeed * 1000.0f); break; + case SettingID::FOCUS_NECK_DELAY_MS: value = (uint16_t)fs.neckDelayMs; break; + case SettingID::FOCUS_NECK_CONTRIBUTION: value = (uint16_t)(fs.neckContribution * 1000.0f); break; + case SettingID::FOCUS_NECK_INVERT: value = fs.neckInvert ? 1 : 0; break; + case SettingID::FOCUS_EYE_CENTERING: value = (uint16_t)(fs.eyeCenteringSpeed * 1000.0f); break; + case SettingID::FOCUS_NECK_CENTERING: value = (uint16_t)(fs.neckCenteringSpeed * 1000.0f); break; + default: return false; + } + return true; +} + +// Set a setting value by ID. Returns true if valid ID. +static bool setSettingValue(uint16_t id, uint16_t value) { + FocusSettings& fs = focusBehavior.getSettings(); + + switch (id) { + case SettingID::FOCUS_EYE_MOTOR_1: fs.eyeMotor1 = (uint8_t)value; break; + case SettingID::FOCUS_EYE_MOTOR_2: fs.eyeMotor2 = (uint8_t)value; break; + case SettingID::FOCUS_NECK_MOTOR: fs.neckMotor = (uint8_t)value; break; + case SettingID::FOCUS_EYE_CENTER: fs.eyeCenter = value; break; + case SettingID::FOCUS_EYE_MIN: fs.eyeMin = value; break; + case SettingID::FOCUS_EYE_MAX: fs.eyeMax = value; break; + case SettingID::FOCUS_NECK_CENTER: fs.neckCenter = value; break; + case SettingID::FOCUS_NECK_MIN: fs.neckMin = value; break; + case SettingID::FOCUS_NECK_MAX: fs.neckMax = value; break; + case SettingID::FOCUS_FACE_X_MIN: fs.faceXMin = (float)(int16_t)value; break; + case SettingID::FOCUS_FACE_X_MAX: fs.faceXMax = (float)(int16_t)value; break; + case SettingID::FOCUS_EYE_SPEED: fs.eyeSpeed = (float)value / 1000.0f; break; + case SettingID::FOCUS_NECK_SPEED: fs.neckSpeed = (float)value / 1000.0f; break; + case SettingID::FOCUS_EYE_RETURN_SPEED: fs.eyeReturnSpeed = (float)value / 1000.0f; break; + case SettingID::FOCUS_NECK_DELAY_MS: fs.neckDelayMs = (unsigned long)value; break; + case SettingID::FOCUS_NECK_CONTRIBUTION: fs.neckContribution = (float)value / 1000.0f; break; + case SettingID::FOCUS_NECK_INVERT: fs.neckInvert = value != 0; break; + case SettingID::FOCUS_EYE_CENTERING: fs.eyeCenteringSpeed = (float)value / 1000.0f; break; + case SettingID::FOCUS_NECK_CENTERING: fs.neckCenteringSpeed = (float)value / 1000.0f; break; + default: return false; + } + return true; +} + +void handleSettingsSet(const uint8_t* payload, uint16_t len) { + if (len == 0) { + // Dump all settings: respond with SSET containing [count:2][id:2][value:2]... + // Collect all focus settings + uint8_t buf[2 + SettingID::FOCUS_COUNT * 4]; // count(2) + N × (id(2) + value(2)) + uint16_t count = 0; + uint16_t offset = 2; // Skip count bytes, fill in later + + for (uint16_t id = SettingID::FOCUS_FIRST; id <= SettingID::FOCUS_LAST; id++) { + uint16_t value; + if (getSettingValue(id, value)) { + buf[offset++] = id & 0xFF; + buf[offset++] = (id >> 8) & 0xFF; + buf[offset++] = value & 0xFF; + buf[offset++] = (value >> 8) & 0xFF; + count++; + } + } + + // Write count at the start + buf[0] = count & 0xFF; + buf[1] = (count >> 8) & 0xFF; + + sendPacket(Tag::SSET, buf, offset); + return; + } + + if (len == 4) { + // Set a single setting: [id:2 LE][value:2 LE] + uint16_t id = payload[0] | (payload[1] << 8); + uint16_t value = payload[2] | (payload[3] << 8); + + if (setSettingValue(id, value)) { + // Persist to flash + config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior); + sendAck(Tag::SSET); + } else { + sendNack(Tag::SSET, "Unknown setting ID"); + } + return; + } + + sendNack(Tag::SSET, "Invalid payload length"); +} + +// ============================================================================ +// System +// ============================================================================ void handleBootloader(const uint8_t* payload, uint16_t len) { sendMessage("Entering bootloader..."); diff --git a/commands.h b/commands.h index d88a2e6..123f5fa 100644 --- a/commands.h +++ b/commands.h @@ -80,6 +80,9 @@ void handleVisemeDelete(const uint8_t* payload, uint16_t len); void handleVisemeSet(const uint8_t* payload, uint16_t len); void handleVisemeTrigger(const uint8_t* payload, uint16_t len); +// Settings +void handleSettingsSet(const uint8_t* payload, uint16_t len); + // System void handleBootloader(const uint8_t* payload, uint16_t len); diff --git a/ls_firmware.ino b/ls_firmware.ino index ee53a2d..4277633 100644 --- a/ls_firmware.ino +++ b/ls_firmware.ino @@ -582,8 +582,7 @@ void setup() { // 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; + // 1. Focus behavior (highest priority - face tracking) behaviorManager.addBehavior(BEHAVIOR_FOCUS, &focusBehavior); // 2. Viseme behavior (medium priority - mouth animation for speech) @@ -600,7 +599,7 @@ void setup() { // 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); + bool configLoaded = config.loadOrCreateDefault("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior); if (configLoaded) { Serial.println("[HansonServo] Config loaded: " + config.deviceName); @@ -613,7 +612,7 @@ void setup() { behaviorManager.setBehaviorEnabled(BEHAVIOR_VISEME, true); behaviorManager.setBehaviorEnabled(BEHAVIOR_IDLE, true); // Save the defaults - config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior); + config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior); } else { Serial.println("[HansonServo] Behavior states loaded from config"); } @@ -634,7 +633,7 @@ void setup() { 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); + config.saveToFFatV2("/robot_config.bin", &behaviorManager, &visemeBehavior, &focusBehavior); } else { Serial.println("[HansonServo] Visemes loaded from config: " + String(visemeBehavior.getVisemes().size())); } diff --git a/protocol.h b/protocol.h index 40052b0..91672e4 100644 --- a/protocol.h +++ b/protocol.h @@ -59,6 +59,9 @@ namespace Tag { constexpr char VSET[4] = {'V','S','E','T'}; // Set viseme motor positions constexpr char VSME[4] = {'V','S','M','E'}; // Trigger a viseme by ID + // Settings + constexpr char SSET[4] = {'S','S','E','T'}; // Settings set/dump (id + value pairs) + // System constexpr char STATE[4] = {'S','T','A','T'}; // System state/heartbeat constexpr char MSGE[4] = {'M','S','G','E'}; // Log/debug message diff --git a/robotconfig.cpp b/robotconfig.cpp index 1032acc..c35c280 100644 --- a/robotconfig.cpp +++ b/robotconfig.cpp @@ -227,7 +227,48 @@ bool RobotConfig::loadOrCreateDefault(const char* path) { // Key-Value Format V2 (Compact, Extensible) // ============================================================================ -bool RobotConfig::saveToFFatV2(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior) const { +// ---- Helpers for reading/writing multi-byte values to file ---- + +static void writeU16(File& f, uint16_t v) { + f.write((uint8_t)(v & 0xFF)); + f.write((uint8_t)((v >> 8) & 0xFF)); +} + +static uint16_t readU16(File& f) { + uint16_t v = f.read(); + v |= ((uint16_t)f.read() << 8); + return v; +} + +static void writeU32(File& f, uint32_t v) { + f.write((uint8_t)(v & 0xFF)); + f.write((uint8_t)((v >> 8) & 0xFF)); + f.write((uint8_t)((v >> 16) & 0xFF)); + f.write((uint8_t)((v >> 24) & 0xFF)); +} + +static uint32_t readU32(File& f) { + uint32_t v = f.read(); + v |= ((uint32_t)f.read() << 8); + v |= ((uint32_t)f.read() << 16); + v |= ((uint32_t)f.read() << 24); + return v; +} + +static void writeFloat(File& f, float v) { + uint32_t raw; + memcpy(&raw, &v, 4); + writeU32(f, raw); +} + +static float readFloat(File& f) { + uint32_t raw = readU32(f); + float v; + memcpy(&v, &raw, 4); + return v; +} + +bool RobotConfig::saveToFFatV2(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior, FocusBehavior* focusBehavior) const { File file = FFat.open(path, FILE_WRITE); if (!file) return false; @@ -343,6 +384,54 @@ bool RobotConfig::saveToFFatV2(const char* path, BehaviorManager* behaviorManage } } + // Setting 7: Focus Behavior Settings + if (focusBehavior) { + const FocusSettings& fs = focusBehavior->getSettings(); + + file.write((uint8_t)(KEY_FOCUS_SETTINGS & 0xFF)); + file.write((uint8_t)((KEY_FOCUS_SETTINGS >> 8) & 0xFF)); + file.write(TYPE_FOCUS_SETTINGS); + + // Motor IDs + file.write(fs.eyeMotor1); + file.write(fs.eyeMotor2); + file.write(fs.neckMotor); + + // Eye servo range + writeU16(file, fs.eyeCenter); + writeU16(file, fs.eyeMin); + writeU16(file, fs.eyeMax); + + // Neck servo range + writeU16(file, fs.neckCenter); + writeU16(file, fs.neckMin); + writeU16(file, fs.neckMax); + + // Face x range + writeFloat(file, fs.faceXMin); + writeFloat(file, fs.faceXMax); + + // Interpolation speeds + writeFloat(file, fs.eyeSpeed); + writeFloat(file, fs.neckSpeed); + writeFloat(file, fs.eyeReturnSpeed); + + // Neck delay + writeU32(file, (uint32_t)fs.neckDelayMs); + + // Neck contribution + writeFloat(file, fs.neckContribution); + + // Neck invert + file.write(fs.neckInvert ? 1 : 0); + + // Centering speeds + writeFloat(file, fs.eyeCenteringSpeed); + writeFloat(file, fs.neckCenteringSpeed); + + settingCount++; + } + // Write setting count at the beginning size_t endPos = file.position(); file.seek(countPos); @@ -354,7 +443,7 @@ bool RobotConfig::saveToFFatV2(const char* path, BehaviorManager* behaviorManage return true; } -bool RobotConfig::loadFromFFatV2(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior) { +bool RobotConfig::loadFromFFatV2(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior, FocusBehavior* focusBehavior) { File file = FFat.open(path, FILE_READ); if (!file) return false; @@ -479,6 +568,55 @@ bool RobotConfig::loadFromFFatV2(const char* path, BehaviorManager* behaviorMana break; } + case KEY_FOCUS_SETTINGS: { + if (type == TYPE_FOCUS_SETTINGS && focusBehavior) { + FocusSettings& fs = focusBehavior->getSettings(); + + // Motor IDs + fs.eyeMotor1 = file.read(); + fs.eyeMotor2 = file.read(); + fs.neckMotor = file.read(); + + // Eye servo range + fs.eyeCenter = readU16(file); + fs.eyeMin = readU16(file); + fs.eyeMax = readU16(file); + + // Neck servo range + fs.neckCenter = readU16(file); + fs.neckMin = readU16(file); + fs.neckMax = readU16(file); + + // Face x range + fs.faceXMin = readFloat(file); + fs.faceXMax = readFloat(file); + + // Interpolation speeds + fs.eyeSpeed = readFloat(file); + fs.neckSpeed = readFloat(file); + fs.eyeReturnSpeed = readFloat(file); + + // Neck delay + fs.neckDelayMs = (unsigned long)readU32(file); + + // Neck contribution + fs.neckContribution = readFloat(file); + + // Neck invert + fs.neckInvert = file.read() != 0; + + // Centering speeds + fs.eyeCenteringSpeed = readFloat(file); + fs.neckCenteringSpeed = readFloat(file); + + Serial.println("[Config] Focus settings loaded"); + } else { + // Skip the focus settings blob (52 bytes) if no focusBehavior + for (int k = 0; k < 52; k++) file.read(); + } + break; + } + default: // Unknown key - skip based on type switch (type) { @@ -507,11 +645,11 @@ bool RobotConfig::loadFromFFatV2(const char* path, BehaviorManager* behaviorMana return true; } -bool RobotConfig::loadOrCreateDefault(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior) { +bool RobotConfig::loadOrCreateDefault(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior, FocusBehavior* focusBehavior) { if (FFat.exists(path)) { Serial.println("Loading robot config from FFat..."); // Try V2 format first - if (loadFromFFatV2(path, behaviorManager, visemeBehavior)) { + if (loadFromFFatV2(path, behaviorManager, visemeBehavior, focusBehavior)) { Serial.println("Loaded V2 format"); return true; } @@ -519,17 +657,17 @@ bool RobotConfig::loadOrCreateDefault(const char* path, BehaviorManager* behavio if (loadFromFFat(path)) { Serial.println("Loaded V1 format (legacy)"); // Upgrade to V2 format - saveToFFatV2(path, behaviorManager, visemeBehavior); + saveToFFatV2(path, behaviorManager, visemeBehavior, focusBehavior); return true; } } Serial.println("No config found. Creating default config..."); - // 🔧 Define your default config here + // Define your default config here deviceName = "DefaultBot"; firmwareVersion = { 1, 0 }; motors.clear(); - return saveToFFatV2(path, behaviorManager, visemeBehavior); + return saveToFFatV2(path, behaviorManager, visemeBehavior, focusBehavior); } diff --git a/robotconfig.h b/robotconfig.h index ec60add..2b43a79 100644 --- a/robotconfig.h +++ b/robotconfig.h @@ -5,6 +5,7 @@ // Forward declarations class BehaviorManager; class VisemeBehavior; +class FocusBehavior; // ============================================================================ // Config Key-Value System @@ -25,6 +26,9 @@ enum ConfigKey : uint16_t { // Viseme array (single entry containing all visemes) KEY_VISEME_ARRAY = 0x0300, + // Focus behavior settings + KEY_FOCUS_SETTINGS = 0x0500, + // Future extensible settings KEY_SERIAL_BAUD = 0x0400, KEY_MOTOR_UPDATE_INTERVAL = 0x0401, @@ -44,6 +48,7 @@ enum ConfigType : uint8_t { 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 + TYPE_FOCUS_SETTINGS = 0x0D, // Focus behavior settings blob }; struct FirmwareVersion { @@ -88,15 +93,18 @@ struct RobotConfig { // New key-value format (v2) bool saveToFFatV2(const char* path = "/robot_config.bin", BehaviorManager* behaviorManager = nullptr, - VisemeBehavior* visemeBehavior = nullptr) const; + VisemeBehavior* visemeBehavior = nullptr, + FocusBehavior* focusBehavior = nullptr) const; bool loadFromFFatV2(const char* path = "/robot_config.bin", BehaviorManager* behaviorManager = nullptr, - VisemeBehavior* visemeBehavior = nullptr); + VisemeBehavior* visemeBehavior = nullptr, + FocusBehavior* focusBehavior = nullptr); // New version with behavior/viseme support bool loadOrCreateDefault(const char* path = "/robot_config.bin", BehaviorManager* behaviorManager = nullptr, - VisemeBehavior* visemeBehavior = nullptr); + VisemeBehavior* visemeBehavior = nullptr, + FocusBehavior* focusBehavior = nullptr); // Legacy version (for backward compatibility) bool loadOrCreateDefault(const char* path);