can recieve config updates from control panel, SCS/STS logic offloaded to pos move and play animation in main ino
parent
6b991fa982
commit
e5d73fca5d
200
HansonServo.ino
200
HansonServo.ino
|
|
@ -25,6 +25,7 @@ uint8_t payload[MAX_PAYLOAD_SIZE]; // Global or static
|
|||
#define CMD_PLAY_FILE 0x08
|
||||
#define CMD_SCAN_CHANNEL 0x09
|
||||
#define CMD_WRITE_DATA 0x10
|
||||
#define CMD_WRITE_CONFIG_UPDATE 0x12
|
||||
#define CMD_START_POSITION_STREAM 0x14
|
||||
#define POSITION_STREAM 0x15
|
||||
|
||||
|
|
@ -87,28 +88,18 @@ void setup() {
|
|||
delay(500);
|
||||
}
|
||||
|
||||
config.deviceName = "Little Elephant";
|
||||
config.firmwareVersion.major = 0;
|
||||
config.firmwareVersion.minor = 2;
|
||||
// config.deviceName = "Little Elephant";
|
||||
// config.firmwareVersion.major = 0;
|
||||
// config.firmwareVersion.minor = 2;
|
||||
|
||||
// Add a few motors
|
||||
config.motors.push_back({ "Neck Twist", 10, 2048 });
|
||||
config.motors.push_back({ "Left Shoulder", 11, 2048 });
|
||||
config.motors.push_back({ "Right Shoulder", 12, 2048 });
|
||||
config.motors.push_back({ "Pelvis Tilt", 13, 2048 });
|
||||
config.motors.push_back({ "Neck Tilt", 14, 2048 });
|
||||
// config.motors.push_back({ "Ankle Left", 10, 2048 });
|
||||
// config.motors.push_back({ "Ankle Right", 11, 2048 });
|
||||
// config.motors.push_back({ "Ankle Center", 12, 2048 });
|
||||
// config.motors.push_back({ "Knee", 13, 2048 });
|
||||
// config.motors.push_back({ "Upper Leg", 14, 2048 });
|
||||
|
||||
// Print serialized config
|
||||
Serial.println(config.serializeJSON());
|
||||
std::vector<uint8_t> bytes = config.serializeToBytes();
|
||||
String output = "";
|
||||
|
||||
for (size_t i = 0; i < bytes.size(); ++i) {
|
||||
if (bytes[i] < 16) output += "0"; // pad single-digit hex
|
||||
output += String(bytes[i], HEX);
|
||||
if (i < bytes.size() - 1) output += " ";
|
||||
}
|
||||
Serial.println(output);
|
||||
|
||||
pinMode(6, INPUT);
|
||||
pinMode(7, INPUT);
|
||||
|
|
@ -138,17 +129,35 @@ void setup() {
|
|||
if (!FFat.begin(true)) {
|
||||
Serial.println("FFat mount failed");
|
||||
return;
|
||||
} else {
|
||||
Serial.println("FFat mount successful");
|
||||
}
|
||||
|
||||
if (config.loadOrCreateDefault()) {
|
||||
Serial.println("Robot config ready.");
|
||||
} else {
|
||||
Serial.println("Failed to initialize robot config.");
|
||||
}
|
||||
|
||||
// Print serialized config
|
||||
Serial.println(config.serializeJSON());
|
||||
std::vector<uint8_t> bytes = config.serializeToBytes();
|
||||
String output = "";
|
||||
|
||||
// anim.saveToFile("/scurve.anim");
|
||||
// Serial.println("SAVED");
|
||||
for (size_t i = 0; i < bytes.size(); ++i) {
|
||||
if (bytes[i] < 16) output += "0"; // pad single-digit hex
|
||||
output += String(bytes[i], HEX);
|
||||
if (i < bytes.size() - 1) output += " ";
|
||||
}
|
||||
Serial.println(output);
|
||||
|
||||
//anim.saveToFile("/scurve.anim");
|
||||
//Serial.println("SAVED");
|
||||
//handleSaveFile(testPayload, testPayloadLength);
|
||||
Serial.println("Loading test.anim");
|
||||
anim.loadFromFile("/test.anim");
|
||||
Serial.println(anim.header.frameCount);
|
||||
anim.printCurves();
|
||||
//Serial.println("Loading test.anim");
|
||||
//anim.loadFromFile("/test.anim");
|
||||
//Serial.println(anim.header.frameCount);
|
||||
//anim.printCurves();
|
||||
// Serial.print("CurveSegment size: ");
|
||||
// Serial.println(sizeof(CurveSegment));
|
||||
//anim.createBasicSCurve();
|
||||
|
|
@ -209,20 +218,21 @@ void setup() {
|
|||
// servos[1]->setMinAngleLimit(13, 900);
|
||||
// servos[0]->setMaxAngleLimit(13, 500);
|
||||
// servos[1]->setMaxAngleLimit(13, 3900);
|
||||
//SetID(13, 17);
|
||||
}
|
||||
|
||||
void SetID(uint8_t oldID, uint8_t newID) {
|
||||
Serial.println("Setting Lock to 0");
|
||||
Serial.println(servos[0]->setLock(oldID, 0));
|
||||
Serial.println(servos[1]->setLock(oldID, 0));
|
||||
delay(1000);
|
||||
Serial.print("Changing ID ");
|
||||
Serial.print(oldID);
|
||||
Serial.print(" to ");
|
||||
Serial.println(newID);
|
||||
Serial.println(servos[0]->setID(oldID, newID));
|
||||
Serial.println(servos[1]->setID(oldID, newID));
|
||||
delay(1000);
|
||||
Serial.println("Setting Lock to 1");
|
||||
Serial.println(servos[0]->setLock(newID, 1));
|
||||
Serial.println(servos[1]->setLock(newID, 1));
|
||||
delay(1000);
|
||||
}
|
||||
|
||||
|
|
@ -338,6 +348,10 @@ void handleCommand(uint8_t command, const uint8_t* payload, uint16_t length) {
|
|||
startPositionStream(payload, length);
|
||||
break;
|
||||
|
||||
case CMD_WRITE_CONFIG_UPDATE:
|
||||
handleConfigUpdate(payload, length);
|
||||
break;
|
||||
|
||||
default:
|
||||
Serial.print("Unknown command: ");
|
||||
Serial.println(command, HEX);
|
||||
|
|
@ -361,6 +375,69 @@ void startPositionStream(const uint8_t* payload, uint16_t length) {
|
|||
streamPositions = start;
|
||||
}
|
||||
|
||||
void handleConfigUpdate(const uint8_t* payload, uint16_t length) {
|
||||
RobotConfig newConfig;
|
||||
uint16_t offset = 0;
|
||||
|
||||
// 🔹 Decode robot name
|
||||
uint8_t nameLength = payload[offset++];
|
||||
newConfig.deviceName = "";
|
||||
|
||||
for (uint8_t i = 0; i < nameLength && offset < length; ++i) {
|
||||
newConfig.deviceName += (char)payload[offset++];
|
||||
}
|
||||
|
||||
// 🔹 Decode firmware version
|
||||
if (offset + 2 > length) return;
|
||||
newConfig.firmwareVersion.major = payload[offset++];
|
||||
newConfig.firmwareVersion.minor = payload[offset++];
|
||||
|
||||
// 🔹 Decode motor count
|
||||
if (offset >= length) return;
|
||||
uint8_t motorCount = payload[offset++];
|
||||
|
||||
// 🔹 Decode motors
|
||||
for (uint8_t i = 0; i < motorCount && offset < length; ++i) {
|
||||
if (offset + 5 > length) break; // MODEL(2) + ID(2) + nameLength(1)
|
||||
|
||||
ServoModel model;
|
||||
model.minor = payload[offset++];
|
||||
model.major = payload[offset++];
|
||||
|
||||
uint16_t motorID = payload[offset++] | (payload[offset++] << 8);
|
||||
|
||||
uint8_t motorNameLength = payload[offset++];
|
||||
String motorName = "";
|
||||
|
||||
for (uint8_t j = 0; j < motorNameLength && offset < length; ++j) {
|
||||
motorName += (char)payload[offset++];
|
||||
}
|
||||
|
||||
Motor motor;
|
||||
motor.motorID = motorID;
|
||||
motor.servoModel = model;
|
||||
motor.name = motorName;
|
||||
motor.position = 0;
|
||||
motor.isEnabled = true;
|
||||
|
||||
newConfig.motors.push_back(motor);
|
||||
}
|
||||
|
||||
// 🔧 Now you can use `config` however you like
|
||||
Serial.println("Robot name: " + newConfig.deviceName);
|
||||
Serial.printf("Firmware: %d.%d\n", newConfig.firmwareVersion.major, config.firmwareVersion.minor);
|
||||
Serial.printf("Motors: %d\n", newConfig.motors.size());
|
||||
|
||||
for (const Motor& m : newConfig.motors) {
|
||||
Serial.printf("Motor %d (%s) - Model %d.%d\n", m.motorID, m.name.c_str(), m.servoModel.major, m.servoModel.minor);
|
||||
}
|
||||
|
||||
sendMessage(newConfig.deviceName);
|
||||
config = newConfig;
|
||||
config.saveToFFat();
|
||||
}
|
||||
|
||||
|
||||
|
||||
void handleDataWrite(const uint8_t* payload, uint16_t length) {
|
||||
uint8_t channel = payload[0];
|
||||
|
|
@ -745,20 +822,59 @@ void handleSetPosition(const uint8_t* payload, uint16_t length) {
|
|||
return;
|
||||
}
|
||||
|
||||
uint8_t count = 0;
|
||||
for (int i = 0; i < length; i += 3) {
|
||||
uint8_t motorId = payload[i];
|
||||
uint16_t position = (payload[i + 2] << 8) | payload[i + 1];
|
||||
pos1[count] = position;
|
||||
count++;
|
||||
// Apply position to motorId
|
||||
uint8_t motorCount = length / 3;
|
||||
|
||||
//servos.sendWritePos(ids[motorId], position);
|
||||
// Dynamically allocate arrays
|
||||
uint8_t ids[motorCount];
|
||||
uint16_t positions[motorCount];
|
||||
|
||||
for (uint8_t i = 0; i < motorCount; ++i) {
|
||||
uint8_t motorId = payload[i * 3];
|
||||
uint16_t position = (payload[i * 3 + 2] << 8) | payload[i * 3 + 1];
|
||||
|
||||
ids[i] = motorId;
|
||||
positions[i] = position;
|
||||
|
||||
// Optional: update internal state or debug
|
||||
// Serial.printf("Motor %d → %d\n", motorId, position);
|
||||
}
|
||||
servos[0]->syncWritePos(ids, pos1, NUM_CHANNELS);
|
||||
|
||||
// FLIP BYTES FOR STS SERVOS
|
||||
for (int i = 0; i < motorCount; i++) {
|
||||
if (config.getMotorModel(ids[i]) == 9) {
|
||||
positions[i] = flipBytes(positions[i]);
|
||||
} else {
|
||||
positions[i] = map(positions[i], 0, 4095, 0, 1023);
|
||||
}
|
||||
}
|
||||
|
||||
// Send sync write to all motors
|
||||
servos[0]->syncWritePos(ids, positions, motorCount);
|
||||
|
||||
String readablePayload = encodeMotorPositionsReadable(ids, positions, motorCount);
|
||||
sendMessage(readablePayload, CMD_MESSAGE);
|
||||
}
|
||||
|
||||
|
||||
String encodeMotorPositionsReadable(const uint8_t* ids, const uint16_t* positions, uint8_t motorCount) {
|
||||
String payload = "";
|
||||
|
||||
for (uint8_t i = 0; i < motorCount; ++i) {
|
||||
payload += "ID:";
|
||||
payload += String(ids[i]);
|
||||
payload += ",POS:";
|
||||
payload += String(positions[i]);
|
||||
|
||||
if (i < motorCount - 1) {
|
||||
payload += ";"; // separate entries
|
||||
}
|
||||
}
|
||||
|
||||
return payload;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// Scans 0-254 and responds with the channel and ID as each successful ping is received
|
||||
// Signals end by responding with channel and 255
|
||||
void handleScanChannel(const uint8_t* payload, uint16_t length) {
|
||||
|
|
@ -961,7 +1077,7 @@ void runNodeAnimation() {
|
|||
if (!currentAnimation) return; // ✅ Prevent crash
|
||||
if (!currentAnimation->isActive()) return;
|
||||
|
||||
config.enableAllMotors();
|
||||
config.enableAllMotors();
|
||||
|
||||
uint32_t now = millis();
|
||||
if (now - lastTickTime >= FRAME_INTERVAL_MS) {
|
||||
|
|
@ -1006,6 +1122,16 @@ config.enableAllMotors();
|
|||
sendMessage(message);
|
||||
}
|
||||
|
||||
// FLIP BYTES FOR STS SERVOS
|
||||
for (int i = 0; i < motorCount; i++) {
|
||||
if (config.getMotorModel(motorIDs[i]) == 9) {
|
||||
positions[i] = flipBytes(positions[i]);
|
||||
} else {
|
||||
positions[i] = map(positions[i], 0, 4095, 0, 1023);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// ✅ Send to servo controller
|
||||
servos[0]->syncWritePos(motorIDs.data(), positions.data(), motorCount);
|
||||
|
||||
|
|
|
|||
33
feetech.cpp
33
feetech.cpp
|
|
@ -11,21 +11,6 @@ void Feetech::begin() {
|
|||
setModeReceive();
|
||||
}
|
||||
|
||||
void Feetech::printModel(uint16_t modelPacket) {
|
||||
switch (modelPacket) {
|
||||
case MODEL_STS3215:
|
||||
Serial.println("STS3215");
|
||||
break;
|
||||
case MODEL_STS3012:
|
||||
Serial.println("STS3012");
|
||||
break;
|
||||
default:
|
||||
Serial.print(modelPacket);
|
||||
Serial.print("\t");
|
||||
Serial.println("UNKNOWN");
|
||||
}
|
||||
}
|
||||
|
||||
void Feetech::syncWritePos(uint8_t* ids, uint16_t* positions, uint8_t count) {
|
||||
const uint8_t DATA_LEN_PER_SERVO = 0x04; // 2 bytes pos + 2 bytes speed
|
||||
|
||||
|
|
@ -50,12 +35,12 @@ void Feetech::syncWritePos(uint8_t* ids, uint16_t* positions, uint8_t count) {
|
|||
for (uint8_t i = 0; i < count; i++) {
|
||||
packet[index++] = ids[i];
|
||||
|
||||
if (feetechMode == MODE_SCS) {
|
||||
pos = map(positions[i], 0, 4095, 0, 1023);
|
||||
} else if (feetechMode == MODE_STS) {
|
||||
pos = positions[i];
|
||||
}
|
||||
|
||||
// if (feetechMode == MODE_SCS) {
|
||||
// pos = map(positions[i], 0, 4095, 0, 1023);
|
||||
// } else if (feetechMode == MODE_STS) {
|
||||
// pos = positions[i];
|
||||
// }
|
||||
pos = positions[i];
|
||||
|
||||
packet[index++] = (uint8_t)((pos >> 8) & 0xFF); // High byte
|
||||
packet[index++] = (uint8_t)(pos & 0xFF); // Low byte
|
||||
|
|
@ -147,8 +132,10 @@ void Feetech::sendPing(uint8_t id) {
|
|||
}
|
||||
|
||||
void Feetech::clearEcho(uint8_t length) {
|
||||
for (int i = 0; i < length; i++) {
|
||||
if (serial.available()) serial.read();
|
||||
if (filterEcho) {
|
||||
for (int i = 0; i < length; i++) {
|
||||
if (serial.available()) serial.read();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
25
feetech.h
25
feetech.h
|
|
@ -8,19 +8,21 @@
|
|||
|
||||
// SCS & HLS big-endian (high byte first)
|
||||
// STS little-endian (low byte first)
|
||||
//
|
||||
|
||||
class Feetech {
|
||||
public:
|
||||
enum FeetechMode {
|
||||
MODE_SCS,
|
||||
MODE_STS,
|
||||
MODE_SMS
|
||||
MODE_SCS = 5,
|
||||
MODE_SMSA = 6,
|
||||
MODE_SMSB = 8,
|
||||
MODE_STS = 9
|
||||
};
|
||||
|
||||
Feetech(HardwareSerial& serial, int DE_PIN, int RE_PIN, int TX_PIN, int RX_PIN);
|
||||
void begin();
|
||||
void sendPing(uint8_t id);
|
||||
void clearEcho(uint8_t length);
|
||||
void printModel(uint16_t modelPacket);
|
||||
uint16_t getModel(uint8_t id);
|
||||
uint8_t getID(uint8_t id);
|
||||
uint8_t setID(uint8_t id, uint8_t newId);
|
||||
|
|
@ -91,8 +93,8 @@ public:
|
|||
static const byte REQUEST_CCW_DEAD_ZONE = 0x1B; // 1 byte
|
||||
|
||||
// REQUEST OFFSET AND MODE NOT IN SCS?????
|
||||
static const byte REQUEST_OFFSET = 0x1F; // 2 bytes
|
||||
static const byte REQUEST_MODE = 0x21; // 1 byte
|
||||
static const byte REQUEST_OFFSET = 0x1F; // 2 bytes
|
||||
static const byte REQUEST_MODE = 0x21; // 1 byte
|
||||
|
||||
static const byte REQUEST_TORQUE_ENABLE = 0x28; // 1 byte
|
||||
static const byte REQUEST_ACCELERATION = 0x29; // 1 byte
|
||||
|
|
@ -100,8 +102,8 @@ public:
|
|||
static const byte REQUEST_GOAL_TIME = 0x2C; // 2 byte
|
||||
static const byte REQUEST_GOAL_SPEED = 0x2E; // 2 byte
|
||||
|
||||
static const byte REQUEST_LOCK_SCS = 0x30; // 1 byte
|
||||
static const byte REQUEST_LOCK_SMS_STS = 0x37; // 1 byte
|
||||
static const byte REQUEST_LOCK_SCS = 0x30; // 1 byte
|
||||
static const byte REQUEST_LOCK_SMS_STS = 0x37; // 1 byte
|
||||
|
||||
static const byte REQUEST_POSITION = 0x38; // 2 bytes
|
||||
|
||||
|
|
@ -125,14 +127,9 @@ public:
|
|||
static const byte SMS_STS_57600 = 6;
|
||||
static const byte SMS_STS_38400 = 7;
|
||||
|
||||
|
||||
static const uint16_t MODEL_STS3215 = 777;
|
||||
static const uint16_t MODEL_STS3012 = 521;
|
||||
static const uint16_t MODEL_SCS0009 = 1029;
|
||||
|
||||
uint8_t lastSentPacket[32];
|
||||
int lastSentLength = 0;
|
||||
|
||||
bool filterEcho = true; // Filters out echo caused by having simple diode/resistor TTL splitter
|
||||
|
||||
private:
|
||||
HardwareSerial& serial; // Reference to the HardwareSerial object
|
||||
|
|
|
|||
103
robotconfig.cpp
103
robotconfig.cpp
|
|
@ -1,4 +1,5 @@
|
|||
#include "RobotConfig.h"
|
||||
#include <FFat.h>
|
||||
|
||||
uint16_t RobotConfig::getMotorPosition(uint8_t motorID) const {
|
||||
for (const Motor& motor : motors) {
|
||||
|
|
@ -10,6 +11,16 @@ uint16_t RobotConfig::getMotorPosition(uint8_t motorID) const {
|
|||
return 2047;
|
||||
}
|
||||
|
||||
uint8_t RobotConfig::getMotorModel(uint8_t motorID) {
|
||||
for (const Motor& motor : motors) {
|
||||
if (motor.motorID == motorID) {
|
||||
return motor.servoModel.minor;
|
||||
}
|
||||
}
|
||||
// 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) {
|
||||
|
|
@ -46,7 +57,7 @@ bool RobotConfig::isMotorEnabled(uint8_t motorID) {
|
|||
|
||||
void RobotConfig::enableAllMotors() {
|
||||
for (Motor& motor : motors) {
|
||||
motor.isEnabled = true;
|
||||
motor.isEnabled = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -62,7 +73,7 @@ String RobotConfig::serializeJSON() const {
|
|||
output += "{";
|
||||
output += "\"motorID\":" + String(m.motorID) + ",";
|
||||
output += "\"name\":\"" + m.name + "\",";
|
||||
output += "\"position\":" + String(m.position);
|
||||
output += "\"model\":" + String(m.servoModel.minor);
|
||||
output += "}";
|
||||
|
||||
if (i < motors.size() - 1) {
|
||||
|
|
@ -112,3 +123,91 @@ std::vector<uint8_t> RobotConfig::serializeToBytes() const {
|
|||
|
||||
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;
|
||||
}
|
||||
|
||||
bool RobotConfig::loadOrCreateDefault(const char* path) {
|
||||
if (FFat.exists(path)) {
|
||||
Serial.println("Loading robot config from FFat...");
|
||||
return loadFromFFat(path);
|
||||
}
|
||||
|
||||
Serial.println("No config found. Creating default config...");
|
||||
|
||||
// 🔧 Define your default config here
|
||||
deviceName = "DefaultBot";
|
||||
firmwareVersion = { 1, 0 };
|
||||
|
||||
motors.clear();
|
||||
|
||||
return saveToFFat(path);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -2,30 +2,40 @@
|
|||
#include <Arduino.h>
|
||||
#include <vector>
|
||||
|
||||
|
||||
struct FirmwareVersion {
|
||||
uint8_t major;
|
||||
uint8_t minor;
|
||||
};
|
||||
|
||||
struct ServoModel {
|
||||
uint8_t major;
|
||||
uint8_t minor;
|
||||
};
|
||||
|
||||
struct Motor {
|
||||
String name; // Optional: name or ID of the motor
|
||||
String name;
|
||||
uint8_t motorID;
|
||||
uint16_t position; // Current position in encoder ticks or degrees
|
||||
ServoModel servoModel;
|
||||
uint16_t position;
|
||||
bool isEnabled = true;
|
||||
};
|
||||
|
||||
struct RobotConfig {
|
||||
String deviceName; // Name of the robot
|
||||
String deviceName;
|
||||
FirmwareVersion firmwareVersion;
|
||||
std::vector<Motor> motors; // Dynamic array of motors
|
||||
|
||||
std::vector<Motor> motors;
|
||||
|
||||
uint16_t getMotorPosition(uint8_t motorID) const;
|
||||
uint8_t getMotorModel(uint8_t motorID);
|
||||
bool setMotorPosition(uint8_t motorID, uint16_t newPosition);
|
||||
bool setMotorEnabled(uint8_t motorID,bool enable);
|
||||
bool setMotorEnabled(uint8_t motorID, bool enable);
|
||||
bool isMotorEnabled(uint8_t motorID);
|
||||
void enableAllMotors();
|
||||
String serializeJSON() const;
|
||||
std::vector<uint8_t> serializeToBytes() const;
|
||||
|
||||
bool saveToFFat(const char* path = "/robot_config.bin") const;
|
||||
bool loadFromFFat(const char* path = "/robot_config.bin");
|
||||
bool loadOrCreateDefault(const char* path = "/robot_config.bin");
|
||||
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in New Issue