115 lines
2.8 KiB
C++
115 lines
2.8 KiB
C++
#include "RobotConfig.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;
|
|
}
|
|
|
|
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::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 += "\"position\":" + String(m.position);
|
|
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]);
|
|
}
|
|
|
|
// position (uint16_t → 2 bytes)
|
|
buffer.push_back((m.position >> 8) & 0xFF);
|
|
buffer.push_back(m.position & 0xFF);
|
|
}
|
|
|
|
return buffer;
|
|
}
|