From 6b991fa98208157e8c342eed59fce0e197991df9 Mon Sep 17 00:00:00 2001 From: Jake Date: Tue, 21 Oct 2025 00:09:30 +0800 Subject: [PATCH] NoiseNode implemented (octave and persistence hard coded), added servo feedback option to variable --- HansonServo.ino | 141 ++++++++++++++++++++++++--------- animation.cpp | 32 +++++--- feetech.cpp | 31 +++++--- nodegraph.cpp | 204 +++++++++++++++++++++++++++++++++++++----------- nodegraph.h | 21 +++-- noise.cpp | 78 ++++++++++++++++++ noise.h | 6 ++ robotconfig.cpp | 114 +++++++++++++++++++++++++++ robotconfig.h | 31 ++++++++ 9 files changed, 546 insertions(+), 112 deletions(-) create mode 100644 noise.cpp create mode 100644 noise.h create mode 100644 robotconfig.cpp create mode 100644 robotconfig.h diff --git a/HansonServo.ino b/HansonServo.ino index cf97f43..0df278c 100644 --- a/HansonServo.ino +++ b/HansonServo.ino @@ -3,6 +3,9 @@ #include "feetech.h" #include "animation.h" #include "nodegraph.h" +#include "RobotConfig.h" + +RobotConfig config; #define DEVICE_NAME "Little Sophia" #define FIRMWARE_VERSION "0.0.1" @@ -37,6 +40,13 @@ uint8_t payload[MAX_PAYLOAD_SIZE]; // Global or static #define DE_PIN 7 // Driver Enable #define RE_PIN 8 // Receiver Enable +#define PLAY_IDLE 0x00 +#define PLAY_ONCE 0x01 +#define PLAY_LOOP 0x02 +#define PLAY_REPEAT 0x03 +uint8_t playMode = PLAY_IDLE; +uint8_t repeatsRemaining = 0; + Animation anim; Animation* currentAnimation; @@ -77,6 +87,29 @@ void setup() { delay(500); } + 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 }); + + // Print serialized config + Serial.println(config.serializeJSON()); + std::vector 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); @@ -349,23 +382,32 @@ void handleDataWrite(const uint8_t* payload, uint16_t length) { } void handleIdRequest() { - String payload = String(DEVICE_NAME) + "|" + FIRMWARE_VERSION; - uint16_t length = payload.length(); + std::vector payload = config.serializeToBytes(); + uint16_t length = payload.size(); + // Compute checksum: XOR of CMD_ID_REQUEST, length bytes, and all payload bytes uint8_t checksum = CMD_ID_REQUEST ^ (length >> 8) ^ (length & 0xFF); - for (int i = 0; i < length; i++) { - checksum ^= payload[i]; + for (uint8_t byte : payload) { + checksum ^= byte; } + // Send header Serial.write(HEADER1); Serial.write(HEADER2); Serial.write(CMD_ID_REQUEST); Serial.write((length >> 8) & 0xFF); Serial.write(length & 0xFF); - Serial.write((const uint8_t*)payload.c_str(), length); + + // Send payload + for (uint8_t byte : payload) { + Serial.write(byte); + } + + // Send checksum Serial.write(checksum); } + void handleFileList() { File root = FFat.open("/"); if (!root || !root.isDirectory()) { @@ -514,14 +556,20 @@ void handlePlayAnimation(const uint8_t* payload, uint16_t length) { char filename[filenameLength + 1]; - memcpy(filename, payload + 2, filenameLength); filename[filenameLength] = '\0'; + // 🔹 Extract playback mode and repeat count + playMode = payload[2 + filenameLength]; + repeatsRemaining = payload[3 + filenameLength]; + //deleteFile(FFat, ("/" + String(filename)).c_str()); anim.clear(); anim.loadFromFile(("/" + String(filename)).c_str()); - playAnimation(anim); + //playAnimation(anim); + + currentAnimation = &anim; + currentAnimation->setActive(true); // ✅ mark it as running sendMessage("File Played", CMD_PLAY_FILE); } @@ -680,8 +728,8 @@ bool parseAnimationPayload(const uint8_t* payload, uint16_t length, Animation& a } sendMessage(printNodeGraph(animation.nodeGraph)); - currentAnimation = &animation; - currentAnimation->setActive(true); // ✅ mark it as running + // currentAnimation = &animation; + // currentAnimation->setActive(true); // ✅ mark it as running // 🔹 Save using received filename animation.saveToFile(("/" + String(filename)).c_str()); @@ -886,11 +934,12 @@ void loop() { // Serial.println(servos[1]->getGoalSpeed(13)); // delay(10); // } - if (millis() - lastSend > 2000) { - - //sendMessageFromESP32(String(millis())); - //handleIdRequest(); - //PrintFileList(); + if (millis() - lastSend > 50) { + // Update config motor positions + for (const Motor& motor : config.motors) { + uint16_t position = servos[0]->getPosition(motor.motorID); + config.setMotorPosition(motor.motorID, position); + } lastSend = millis(); } @@ -902,6 +951,8 @@ void loop() { } } + + void runNodeAnimation() { static uint32_t lastTickTime = 0; static uint32_t currentTick = 0; @@ -910,6 +961,8 @@ void runNodeAnimation() { if (!currentAnimation) return; // ✅ Prevent crash if (!currentAnimation->isActive()) return; +config.enableAllMotors(); + uint32_t now = millis(); if (now - lastTickTime >= FRAME_INTERVAL_MS) { lastTickTime = now; @@ -928,9 +981,18 @@ void runNodeAnimation() { String message = String(currentTick) + String("\n"); for (const auto& [motorID, value] : outputs) { - motorIDs.push_back(motorID); - positions.push_back(map(value, 0, 4095, 0, 1023)); - + if (value != 65535) { + motorIDs.push_back(motorID); + positions.push_back(value); + config.setMotorPosition(motorID, value); + if (config.setMotorEnabled(motorID, true)) { + servos[0]->enableTorque(motorID); + } + } else { + if (config.setMotorEnabled(motorID, false)) { + servos[0]->disableTorque(motorID); + } + } message += "Motor "; message += String(motorID); message += " → "; @@ -948,12 +1010,24 @@ void runNodeAnimation() { servos[0]->syncWritePos(motorIDs.data(), positions.data(), motorCount); - currentTick++; if (currentTick > currentAnimation->getFrameCount()) { - currentAnimation->setActive(false); - currentTick = 0; // optional: reset for next run + switch (playMode) { + case PLAY_ONCE: + currentAnimation->setActive(false); + break; + case PLAY_LOOP: + + break; + case PLAY_REPEAT: + repeatsRemaining--; + if (repeatsRemaining == 0) { + currentAnimation->setActive(false); + } + break; + } + currentTick = 0; } } } @@ -961,29 +1035,18 @@ void runNodeAnimation() { void SendMotorPositions() { + std::vector payload; - uint16_t p0 = servos[0]->getPosition(10); - uint16_t p1 = servos[0]->getPosition(11); - uint16_t p2 = servos[0]->getPosition(12); - uint16_t p3 = servos[0]->getPosition(13); - uint16_t p4 = servos[0]->getPosition(14); + for (const Motor& motor : config.motors) { + uint16_t position = motor.position; + payload.push_back(position >> 8); // High byte + payload.push_back(position & 0xFF); // Low byte + } - uint8_t payload[10]; - - payload[0] = p0 >> 8; - payload[1] = p0 & 0xFF; - payload[2] = p1 >> 8; - payload[3] = p1 & 0xFF; - payload[4] = p2 >> 8; - payload[5] = p2 & 0xFF; - payload[6] = p3 >> 8; - payload[7] = p3 & 0xFF; - payload[8] = p4 >> 8; - payload[9] = p4 & 0xFF; - - sendMessage(payload, 10, POSITION_STREAM); + sendMessage(payload.data(), payload.size(), POSITION_STREAM); } + void PrintFileList() { File root = FFat.open("/"); if (!root || !root.isDirectory()) { diff --git a/animation.cpp b/animation.cpp index e689295..25c5f10 100644 --- a/animation.cpp +++ b/animation.cpp @@ -70,7 +70,6 @@ uint16_t Animation::getMotorPosition(uint8_t motorID, uint16_t timeCS) { // Remap to PWM range return constrain(value, 0, 4095); - } } @@ -104,29 +103,28 @@ bool Animation::saveToFile(const char* filename) { File file = FFat.open(filename, FILE_WRITE); if (!file) return false; - // Write header file.write((uint8_t*)&header, sizeof(header)); - // Count total curve segments uint16_t curveCount = 0; for (const auto& [motorID, segments] : curves) { curveCount += segments.size(); } - - // Write curve count file.write((uint8_t*)&curveCount, sizeof(curveCount)); - - // Write all curve segments for (const auto& [motorID, segments] : curves) { for (const CurveSegment& seg : segments) { file.write((uint8_t*)&seg, sizeof(CurveSegment)); } } + // ✅ Write serialized node graph + std::vector graphData = nodeGraph.serialize(); + file.write(graphData.data(), graphData.size()); + file.close(); return true; } + bool Animation::loadFromFile(const char* filename) { File file = FFat.open(filename, FILE_READ); if (!file) return false; @@ -152,7 +150,6 @@ bool Animation::loadFromFile(const char* filename) { return false; } - // Clear existing curves clearAllCurves(); // Read curve segments @@ -162,16 +159,31 @@ bool Animation::loadFromFile(const char* filename) { file.close(); return false; } - - // Store segment directly — no motorID filtering curves[seg.motorID].push_back(seg); } + // ✅ Read remaining bytes into buffer + size_t remaining = file.available(); + if (remaining > 0) { + std::vector buffer(remaining); + if (file.read(buffer.data(), remaining) != remaining) { + file.close(); + return false; + } + + // ✅ Load node graph from buffer + nodeGraph.nodes.clear(); + nodeGraph.connections.clear(); + loadNodeGraph(buffer.data(), buffer.size(), nodeGraph); + nodeGraph.bindAnimationContext(this); + } + file.close(); return true; } + String Animation::printCurves() { String output = "PRINTING CURVES\n"; diff --git a/feetech.cpp b/feetech.cpp index f0ee251..b838d68 100644 --- a/feetech.cpp +++ b/feetech.cpp @@ -46,12 +46,21 @@ void Feetech::syncWritePos(uint8_t* ids, uint16_t* positions, uint8_t count) { packet[index++] = DATA_LEN_PER_SERVO; // Servo data + uint16_t pos; for (uint8_t i = 0; i < count; i++) { packet[index++] = ids[i]; - packet[index++] = (uint8_t)((positions[i] >> 8) & 0xFF); // High byte - packet[index++] = (uint8_t)(positions[i] & 0xFF); // Low byte - packet[index++] = 0x00; // Speed high byte - packet[index++] = 0x00; // Speed low byte + + if (feetechMode == MODE_SCS) { + pos = map(positions[i], 0, 4095, 0, 1023); + } else if (feetechMode == MODE_STS) { + pos = positions[i]; + } + + + packet[index++] = (uint8_t)((pos >> 8) & 0xFF); // High byte + packet[index++] = (uint8_t)(pos & 0xFF); // Low byte + packet[index++] = 0x00; // Speed high byte + packet[index++] = 0x00; // Speed low byte } // Checksum @@ -336,7 +345,7 @@ uint16_t Feetech::getPosition(uint8_t id) { if (feetechMode == MODE_STS) { return waitOnData2Bytes(10); } else if (feetechMode == MODE_SCS) { - return flipBytes(waitOnData2Bytes(10)); + return map(flipBytes(waitOnData2Bytes(10)), 0, 1023, 0, 4095); } } @@ -422,12 +431,12 @@ void Feetech::write1Byte(uint8_t id, byte instruction, uint8_t data) { void Feetech::write2Bytes(uint8_t id, byte instruction, uint16_t data) { uint8_t packet[9]; - packet[0] = 0xFF; // Header - packet[1] = 0xFF; // Header - packet[2] = id; // Servo ID - packet[3] = 5; // Length = instruction + address + 2 bytes + checksum - packet[4] = WRITE_DATA; // Instruction: WRITE - packet[5] = instruction; // Address: Goal Position + packet[0] = 0xFF; // Header + packet[1] = 0xFF; // Header + packet[2] = id; // Servo ID + packet[3] = 5; // Length = instruction + address + 2 bytes + checksum + packet[4] = WRITE_DATA; // Instruction: WRITE + packet[5] = instruction; // Address: Goal Position if (feetechMode == MODE_SCS) { packet[6] = (data >> 8) & 0xFF; // High byte packet[7] = data & 0xFF; // Low byte diff --git a/nodegraph.cpp b/nodegraph.cpp index c21e471..06c66e4 100644 --- a/nodegraph.cpp +++ b/nodegraph.cpp @@ -1,7 +1,12 @@ #include "nodegraph.h" #include "animation.h" +#include "noise.h" +#include "RobotConfig.h" #include +extern RobotConfig config; + + // CurveNode evaluation void CurveNode::evaluate(uint32_t tick) { if (!animation) { @@ -22,6 +27,13 @@ Node* NodeGraph::findNodeByID(uint8_t id) const { return nullptr; // not found } +void NoiseNode::evaluate(uint32_t tick) { + float t = (static_cast(tick) / 480.0f) * frequency * 10.0f; + + float raw = perlin1D_octave(seed, t, 4, 0.5f); + outputValue = (raw + 1.0f) * 2047.5f; // → [0, 4095] +} + // ServoNode evaluation void ServoNode::evaluate(uint32_t tick) { @@ -46,6 +58,9 @@ void VariableNode::evaluate(uint32_t currentTick) { case VAR_ANALOG: outputValue = analogRead(7); // or whichever pin break; + case VAR_SERVO_FEEDBACK: + outputValue = config.getMotorPosition(arg0); + break; } } @@ -109,42 +124,49 @@ std::vector> NodeGraph::getServoOutputs() const { std::vector NodeGraph::getSortedNodes() { - std::unordered_map> adj; - std::unordered_map inDegree; - std::unordered_map idMap; + std::unordered_map> adj; + std::unordered_map inDegree; + std::unordered_map idMap; - for (Node* node : nodes) { - idMap[node->id] = node; - inDegree[node->id] = 0; - } - - for (const NodeConnection& conn : connections) { - adj[conn.fromID].push_back(conn.toID); - inDegree[conn.toID]++; - } - - std::vector sorted; - std::queue q; - - for (const auto& [id, deg] : inDegree) { - if (deg == 0) q.push(id); - } - - while (!q.empty()) { - uint8_t id = q.front(); - q.pop(); - sorted.push_back(idMap[id]); - - for (uint8_t neighbor : adj[id]) { - if (--inDegree[neighbor] == 0) { - q.push(neighbor); - } + for (Node* node : nodes) { + idMap[node->id] = node; + inDegree[node->id] = 0; } - } - return sorted; + for (const NodeConnection& conn : connections) { + adj[conn.fromID].push_back(conn.toID); + inDegree[conn.toID]++; + } + + std::vector sorted; + std::queue q; + + for (const auto& [id, deg] : inDegree) { + if (deg == 0) q.push(id); + } + + while (!q.empty()) { + uint8_t id = q.front(); + q.pop(); + Node* node = idMap[id]; + + // ✅ Reset values here + node->inputValue = 65535; + node->outputValue = 65535; + + sorted.push_back(node); + + for (uint8_t neighbor : adj[id]) { + if (--inDegree[neighbor] == 0) { + q.push(neighbor); + } + } + } + + return sorted; } + // This function links nodes to their outside inputs void NodeGraph::bindAnimationContext(Animation* animation) { for (Node* node : nodes) { @@ -211,12 +233,14 @@ void loadNodeGraph(const uint8_t* packet, size_t length, NodeGraph& graph) { { // ServoNode if (offset + 1 > length) break; uint8_t source = packet[offset++]; + uint8_t arg0 = packet[offset++]; auto* newNode = new VariableNode(); newNode->id = id; newNode->type = type; newNode->x = x; newNode->y = y; newNode->source = static_cast(source); + newNode->arg0 = arg0; node = newNode; break; } @@ -264,14 +288,27 @@ void loadNodeGraph(const uint8_t* packet, size_t length, NodeGraph& graph) { node = mapNode; break; } - - - case TYPE_NOISENODE: - { // NoiseNode - if (offset + 17 > length) break; - offset += 17; // skip for now - break; + { + if (offset + 6 > length) break; // 4 bytes for float + 2 bytes for uint16_t + + float frequency; + uint16_t seed; + + memcpy(&frequency, &packet[offset], sizeof(float)); + offset += 4; + + memcpy(&seed, &packet[offset], sizeof(uint16_t)); + offset += 2; + + auto* noiseNode = new NoiseNode(); + noiseNode->id = id; + noiseNode->type = type; + noiseNode->x = x; + noiseNode->y = y; + noiseNode->frequency = frequency; + noiseNode->seed = seed; + node = noiseNode; } default: break; @@ -328,17 +365,16 @@ String printNodeGraph(const NodeGraph& graph) { { // VariableNode const VariableNode* servo = static_cast(node); output += " | source: " + String(servo->source); + output += " | arg0: " + String(servo->arg0); + break; + } + case TYPE_NOISENODE: + { // NoiseNode + const NoiseNode* noise = static_cast(node); + output += " | Frequency " + String(noise->frequency, 4); + output += " | Seed " + String(noise->seed); break; } - // case 2: { // NoiseNode - // const NoiseNode* noise = static_cast(node); - // output += " | Noise a=" + String(noise->a, 2); - // output += " b=" + String(noise->b, 2); - // output += " c=" + String(noise->c, 2); - // output += " d=" + String(noise->d, 2); - // output += " seed=" + String(noise->seed); - // break; - // } } output += "\n"; @@ -351,3 +387,77 @@ String printNodeGraph(const NodeGraph& graph) { return output; } + +std::vector NodeGraph::serialize() { + std::vector data; + + // Node count + data.push_back(static_cast(nodes.size())); + + // Serialize nodes + for (Node* node : nodes) { + data.push_back(node->type); + data.push_back(node->id); + + uint16_t x = node->x; + uint16_t y = node->y; + data.insert(data.end(), reinterpret_cast(&x), reinterpret_cast(&x) + sizeof(uint16_t)); + data.insert(data.end(), reinterpret_cast(&y), reinterpret_cast(&y) + sizeof(uint16_t)); + + switch (node->type) { + case TYPE_SERVONODE: + { + auto* n = static_cast(node); + data.push_back(n->motorID); + break; + } + case TYPE_CURVENODE: + { + auto* n = static_cast(node); + data.push_back(n->curveID); + break; + } + case TYPE_VARIABLENODE: + { + auto* n = static_cast(node); + data.push_back(static_cast(n->source)); + data.push_back(static_cast(n->arg0)); + break; + } + case TYPE_MATHNODE: + { + auto* n = static_cast(node); + data.push_back(static_cast(n->op)); + data.insert(data.end(), reinterpret_cast(&n->value), reinterpret_cast(&n->value) + sizeof(float)); + break; + } + case TYPE_MAPNODE: + { + auto* n = static_cast(node); + data.insert(data.end(), reinterpret_cast(&n->inMin), reinterpret_cast(&n->inMin) + sizeof(float)); + data.insert(data.end(), reinterpret_cast(&n->inMax), reinterpret_cast(&n->inMax) + sizeof(float)); + data.insert(data.end(), reinterpret_cast(&n->outMin), reinterpret_cast(&n->outMin) + sizeof(float)); + data.insert(data.end(), reinterpret_cast(&n->outMax), reinterpret_cast(&n->outMax) + sizeof(float)); + break; + } + case TYPE_NOISENODE: + { + auto* n = static_cast(node); + data.insert(data.end(), reinterpret_cast(&n->frequency), reinterpret_cast(&n->frequency) + sizeof(float)); + data.insert(data.end(), reinterpret_cast(&n->seed), reinterpret_cast(&n->seed) + sizeof(uint16_t)); + break; + } + } + } + + // Connection count + data.push_back(static_cast(connections.size())); + + // Serialize connections + for (const NodeConnection& conn : connections) { + data.push_back(conn.fromID); + data.push_back(conn.toID); + } + + return data; +} diff --git a/nodegraph.h b/nodegraph.h index e71b174..8bac86c 100644 --- a/nodegraph.h +++ b/nodegraph.h @@ -18,7 +18,8 @@ enum VariableSource { VAR_FACE_X, VAR_FACE_Y, VAR_SINE, - VAR_ANALOG + VAR_ANALOG, + VAR_SERVO_FEEDBACK }; @@ -50,6 +51,14 @@ struct CurveNode : public Node { void evaluate(uint32_t tick) override; }; +// +struct NoiseNode : public Node { + float frequency; + uint16_t seed; + + void evaluate(uint32_t tick) override; +}; + // ServoNode: sends a value to a motor struct ServoNode : public Node { uint8_t motorID; @@ -59,9 +68,12 @@ struct ServoNode : public Node { struct VariableNode : public Node { VariableSource source = VAR_SINE; // default + uint8_t arg0; void evaluate(uint32_t tick) override; - void setSource(VariableSource src) { source = src; } + void setSource(VariableSource src) { + source = src; + } }; struct MathNode : public Node { @@ -91,14 +103,13 @@ class NodeGraph { public: std::vector nodes; std::vector connections; -Node* findNodeByID(uint8_t id) const; + Node* findNodeByID(uint8_t id) const; void tick(uint32_t currentTick, const Animation& animation); std::vector> getServoOutputs() const; std::vector getSortedNodes(); void bindAnimationContext(Animation* animation); - - + std::vector serialize(); }; diff --git a/noise.cpp b/noise.cpp new file mode 100644 index 0000000..db5843d --- /dev/null +++ b/noise.cpp @@ -0,0 +1,78 @@ +#include "noise.h" +#include +#include + + +uint8_t perm[512]; +static uint16_t currentSeed = 0xFFFF; + + +inline static float fade(float t) { + return t * t * t * (t * (t * 6 - 15) + 10); +} + +inline static float noiseLerp(float a, float b, float t) { + return a + t * (b - a); +} + +inline static float grad(uint8_t hash, float x) { + uint8_t h = hash & 15; + float grad = 1.0f + (h & 7); // 1 to 8 + if (h & 8) grad = -grad; + return (grad * x) / 4.0f; // match JS scaling +} + +static void generatePermutation(uint16_t seed, uint8_t* perm) { + uint8_t p[256]; + uint32_t s = seed; + + for (int i = 0; i < 256; i++) { + s = (s * 1664525 + 1013904223) % 4294967296; + p[i] = i; + } + + for (int i = 255; i > 0; i--) { + uint32_t j = s % (i + 1); + std::swap(p[i], p[j]); + s = (s * 1664525 + 1013904223) % 4294967296; + } + + for (int i = 0; i < 512; i++) { + perm[i] = p[i & 255]; + } +} + +float perlin1D_octave(uint16_t seed, float x, int octaves = 4, float persistence = 0.5f) { + + if (seed != currentSeed) { + generatePermutation(seed, perm); + currentSeed = seed; + } + + float total = 0.0f; + float amplitude = 1.0f; + float frequency = 1.0f; + float maxValue = 0.0f; + + for (int i = 0; i < octaves; i++) { + total += perlin1D(seed, x * frequency) * amplitude; + maxValue += amplitude; + amplitude *= persistence; + frequency *= 2.0f; + } + + return total / maxValue; +} + + +float perlin1D(uint16_t seed, float x) { + + int xi = static_cast(floor(x)) & 255; + float xf = x - floor(x); + float u = fade(xf); + + uint8_t a = perm[xi]; + uint8_t b = perm[xi + 1]; + + return noiseLerp(grad(a, xf), grad(b, xf - 1.0f), u); +} diff --git a/noise.h b/noise.h new file mode 100644 index 0000000..178fd0e --- /dev/null +++ b/noise.h @@ -0,0 +1,6 @@ +#pragma once +#include + +extern uint8_t perm[512]; +float perlin1D_octave(uint16_t seed, float x, int octaves, float persistence); +float perlin1D(uint16_t seed, float x); diff --git a/robotconfig.cpp b/robotconfig.cpp new file mode 100644 index 0000000..6c031c8 --- /dev/null +++ b/robotconfig.cpp @@ -0,0 +1,114 @@ +#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 RobotConfig::serializeToBytes() const { + std::vector 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; +} diff --git a/robotconfig.h b/robotconfig.h new file mode 100644 index 0000000..2f845f1 --- /dev/null +++ b/robotconfig.h @@ -0,0 +1,31 @@ +#pragma once +#include +#include + + +struct FirmwareVersion { + uint8_t major; + uint8_t minor; +}; + +struct Motor { + String name; // Optional: name or ID of the motor + uint8_t motorID; + uint16_t position; // Current position in encoder ticks or degrees + bool isEnabled = true; +}; + +struct RobotConfig { + String deviceName; // Name of the robot + FirmwareVersion firmwareVersion; + std::vector motors; // Dynamic array of motors + + + uint16_t getMotorPosition(uint8_t motorID) const; + bool setMotorPosition(uint8_t motorID, uint16_t newPosition); + bool setMotorEnabled(uint8_t motorID,bool enable); + bool isMotorEnabled(uint8_t motorID); + void enableAllMotors(); + String serializeJSON() const; + std::vector serializeToBytes() const; +};