724 lines
20 KiB
C++
724 lines
20 KiB
C++
#include "robotconfig.h"
|
|
#include "behaviors.h"
|
|
#include "websocket_client.h"
|
|
#include <FFat.h>
|
|
|
|
uint16_t RobotConfig::getMotorPosition(uint8_t motorID) const {
|
|
for (const Motor& motor : motors) {
|
|
if (motor.motorID == motorID) {
|
|
return motor.position;
|
|
}
|
|
}
|
|
// Return 0 if motor not found
|
|
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) {
|
|
return motor.servoModel.major;
|
|
}
|
|
}
|
|
// Return 0 if motor not found
|
|
return 5;
|
|
}
|
|
|
|
bool RobotConfig::setMotorPosition(uint8_t motorID, uint16_t newPosition) {
|
|
for (Motor& motor : motors) {
|
|
if (motor.motorID == motorID) {
|
|
motor.position = newPosition;
|
|
return true;
|
|
}
|
|
}
|
|
// Return false if motor not found
|
|
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) {
|
|
if (motor.isEnabled != enable) {
|
|
motor.isEnabled = enable;
|
|
return true; // ✅ Only return true if the value changed
|
|
}
|
|
return false; // ❌ No change
|
|
}
|
|
}
|
|
return false; // ❌ Motor not found
|
|
}
|
|
|
|
|
|
bool RobotConfig::isMotorEnabled(uint8_t motorID) {
|
|
for (Motor& motor : motors) {
|
|
if (motor.motorID == motorID) {
|
|
return motor.isEnabled;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void RobotConfig::enableAllMotors() {
|
|
for (Motor& motor : motors) {
|
|
motor.isEnabled = true;
|
|
}
|
|
}
|
|
|
|
String RobotConfig::serializeJSON() const {
|
|
String output = "{";
|
|
output += "\"deviceName\":\"" + deviceName + "\",";
|
|
output += "\"firmwareVersion\":{\"major\":" + String(firmwareVersion.major) + ",\"minor\":" + String(firmwareVersion.minor) + "},";
|
|
|
|
output += "\"motors\":[";
|
|
|
|
for (size_t i = 0; i < motors.size(); ++i) {
|
|
const Motor& m = motors[i];
|
|
output += "{";
|
|
output += "\"motorID\":" + String(m.motorID) + ",";
|
|
output += "\"name\":\"" + m.name + "\",";
|
|
output += "\"model\":" + String(m.servoModel.major);
|
|
output += "}";
|
|
|
|
if (i < motors.size() - 1) {
|
|
output += ",";
|
|
}
|
|
}
|
|
|
|
output += "]}";
|
|
return output;
|
|
}
|
|
|
|
std::vector<uint8_t> RobotConfig::serializeToBytes() const {
|
|
std::vector<uint8_t> buffer;
|
|
|
|
// Encode deviceName length + characters
|
|
uint8_t nameLen = deviceName.length();
|
|
buffer.push_back(nameLen);
|
|
for (uint8_t i = 0; i < nameLen; ++i) {
|
|
buffer.push_back(deviceName[i]);
|
|
}
|
|
|
|
// Encode firmwareVersion (int32_t → 4 bytes)
|
|
buffer.push_back(firmwareVersion.major);
|
|
buffer.push_back(firmwareVersion.minor);
|
|
|
|
|
|
// Encode number of motors
|
|
uint8_t motorCount = motors.size();
|
|
buffer.push_back(motorCount);
|
|
|
|
// Encode each motor
|
|
for (const Motor& m : motors) {
|
|
// motorID
|
|
buffer.push_back(m.motorID);
|
|
|
|
// name length + characters
|
|
uint8_t motorNameLen = m.name.length();
|
|
buffer.push_back(motorNameLen);
|
|
for (uint8_t i = 0; i < motorNameLen; ++i) {
|
|
buffer.push_back(m.name[i]);
|
|
}
|
|
|
|
buffer.push_back(m.servoModel.major);
|
|
buffer.push_back(m.servoModel.minor);
|
|
|
|
// position (uint16_t → 2 bytes)
|
|
buffer.push_back((m.position >> 8) & 0xFF);
|
|
buffer.push_back(m.position & 0xFF);
|
|
}
|
|
|
|
return buffer;
|
|
}
|
|
|
|
bool RobotConfig::saveToFFat(const char* path) const {
|
|
File file = FFat.open(path, FILE_WRITE);
|
|
if (!file) return false;
|
|
|
|
uint8_t nameLen = deviceName.length();
|
|
file.write(nameLen);
|
|
file.write((const uint8_t*)deviceName.c_str(), nameLen);
|
|
|
|
file.write(firmwareVersion.major);
|
|
file.write(firmwareVersion.minor);
|
|
|
|
uint8_t motorCount = motors.size();
|
|
file.write(motorCount);
|
|
|
|
for (const Motor& m : motors) {
|
|
file.write(m.servoModel.major);
|
|
file.write(m.servoModel.minor);
|
|
file.write(m.motorID & 0xFF);
|
|
file.write((m.motorID >> 8) & 0xFF);
|
|
|
|
uint8_t nameLen = m.name.length();
|
|
file.write(nameLen);
|
|
file.write((const uint8_t*)m.name.c_str(), nameLen);
|
|
}
|
|
|
|
file.close();
|
|
return true;
|
|
}
|
|
|
|
bool RobotConfig::loadFromFFat(const char* path) {
|
|
File file = FFat.open(path, FILE_READ);
|
|
if (!file) return false;
|
|
|
|
motors.clear();
|
|
|
|
uint8_t nameLen = file.read();
|
|
char nameBuf[64] = { 0 };
|
|
file.readBytes(nameBuf, nameLen);
|
|
deviceName = String(nameBuf);
|
|
|
|
firmwareVersion.major = file.read();
|
|
firmwareVersion.minor = file.read();
|
|
|
|
uint8_t motorCount = file.read();
|
|
|
|
for (uint8_t i = 0; i < motorCount; ++i) {
|
|
ServoModel model;
|
|
model.major = file.read();
|
|
model.minor = file.read();
|
|
|
|
uint16_t id = file.read();
|
|
id |= (file.read() << 8);
|
|
|
|
uint8_t motorNameLen = file.read();
|
|
char motorNameBuf[64] = { 0 };
|
|
file.readBytes(motorNameBuf, motorNameLen);
|
|
|
|
Motor m;
|
|
m.motorID = id;
|
|
m.servoModel = model;
|
|
m.name = String(motorNameBuf);
|
|
m.position = 0;
|
|
m.isEnabled = true;
|
|
|
|
motors.push_back(m);
|
|
}
|
|
|
|
file.close();
|
|
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)
|
|
// ============================================================================
|
|
|
|
// ---- 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;
|
|
}
|
|
|
|
static void writeStr(File& f, const char* s, uint8_t maxLen) {
|
|
uint8_t len = strlen(s);
|
|
if (len > maxLen) len = maxLen;
|
|
f.write(len);
|
|
f.write((const uint8_t*)s, len);
|
|
}
|
|
|
|
static void readStr(File& f, char* dst, uint8_t maxLen) {
|
|
uint8_t len = f.read();
|
|
if (len > maxLen) len = maxLen;
|
|
f.readBytes(dst, len);
|
|
dst[len] = '\0';
|
|
// Skip extra bytes if stored length exceeded maxLen
|
|
}
|
|
|
|
bool RobotConfig::saveToFFatV2(const char* path, BehaviorManager* behaviorManager, VisemeBehavior* visemeBehavior, FocusBehavior* focusBehavior) 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++;
|
|
}
|
|
}
|
|
|
|
// 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++;
|
|
}
|
|
|
|
// Setting 8: WiFi / WebSocket Settings
|
|
{
|
|
file.write((uint8_t)(KEY_WIFI_SETTINGS & 0xFF));
|
|
file.write((uint8_t)((KEY_WIFI_SETTINGS >> 8) & 0xFF));
|
|
file.write(TYPE_WIFI_SETTINGS);
|
|
|
|
writeStr(file, wifiSettings.ssid, 32);
|
|
writeStr(file, wifiSettings.password, 64);
|
|
writeStr(file, wifiSettings.host, 63);
|
|
writeU16(file, wifiSettings.port);
|
|
writeStr(file, wifiSettings.path, 31);
|
|
|
|
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, FocusBehavior* focusBehavior) {
|
|
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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
case KEY_WIFI_SETTINGS: {
|
|
if (type == TYPE_WIFI_SETTINGS) {
|
|
readStr(file, wifiSettings.ssid, 32);
|
|
readStr(file, wifiSettings.password, 64);
|
|
readStr(file, wifiSettings.host, 63);
|
|
wifiSettings.port = readU16(file);
|
|
readStr(file, wifiSettings.path, 31);
|
|
|
|
Serial.println("[Config] WiFi settings loaded");
|
|
} else {
|
|
// Skip: 5 length-prefixed strings + 2 byte port - can't know exact size
|
|
// Best effort: skip based on stored lengths
|
|
for (int s = 0; s < 3; s++) { uint8_t l = file.read(); for (uint8_t k = 0; k < l; k++) file.read(); }
|
|
file.read(); file.read(); // port
|
|
for (int s = 0; s < 2; s++) { uint8_t l = file.read(); for (uint8_t k = 0; k < l; k++) file.read(); }
|
|
}
|
|
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, FocusBehavior* focusBehavior) {
|
|
if (FFat.exists(path)) {
|
|
Serial.println("Loading robot config from FFat...");
|
|
// Try V2 format first
|
|
if (loadFromFFatV2(path, behaviorManager, visemeBehavior, focusBehavior)) {
|
|
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, focusBehavior);
|
|
return true;
|
|
}
|
|
}
|
|
|
|
Serial.println("No config found. Creating default config...");
|
|
|
|
// Define your default config here
|
|
deviceName = "DefaultBot";
|
|
firmwareVersion = { 1, 0 };
|
|
motors.clear();
|
|
|
|
return saveToFFatV2(path, behaviorManager, visemeBehavior, focusBehavior);
|
|
}
|