NoiseNode implemented (octave and persistence hard coded), added servo feedback option to variable

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<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);
 
@@ -349,23 +382,32 @@ void handleDataWrite(const uint8_t* payload, uint16_t length) {
 }
 
 void handleIdRequest() {
-  String payload = String(DEVICE_NAME) + "|" + FIRMWARE_VERSION;
+  std::vector<uint8_t> payload = config.serializeToBytes();
-  uint16_t length = payload.length();
+  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++) {
+  for (uint8_t byte : payload) {
-    checksum ^= payload[i];
+    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 = &animation;
-  currentAnimation->setActive(true);  // ✅ mark it as running
+  // 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) {
+  if (millis() - lastSend > 50) {
-
+    // Update config motor positions
-    //sendMessageFromESP32(String(millis()));
+    for (const Motor& motor : config.motors) {
-    //handleIdRequest();
+      uint16_t position = servos[0]->getPosition(motor.motorID);
-    //PrintFileList();
+      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) {
+      if (value != 65535) {
         motorIDs.push_back(motorID);
-      positions.push_back(map(value, 0, 4095, 0, 1023));
+        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()) {
+      switch (playMode) {
+        case PLAY_ONCE:
           currentAnimation->setActive(false);
-      currentTick = 0;  // optional: reset for next run
+          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<uint8_t> payload;
 
-  uint16_t p0 = servos[0]->getPosition(10);
+  for (const Motor& motor : config.motors) {
-  uint16_t p1 = servos[0]->getPosition(11);
+    uint16_t position = motor.position;
-  uint16_t p2 = servos[0]->getPosition(12);
+    payload.push_back(position >> 8);    // High byte
-  uint16_t p3 = servos[0]->getPosition(13);
+    payload.push_back(position & 0xFF);  // Low byte
-  uint16_t p4 = servos[0]->getPosition(14);
+  }
 
-  uint8_t payload[10];
+  sendMessage(payload.data(), payload.size(), POSITION_STREAM);
-
-  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);
 }
 
+
 void PrintFileList() {
   File root = FFat.open("/");
   if (!root || !root.isDirectory()) {

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<uint8_t> 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<uint8_t> 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";
 

feetech.cpp

@@ -46,10 +46,19 @@ 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
+    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
   }
@@ -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);
   }
 }
 

nodegraph.cpp

@@ -1,7 +1,12 @@
 #include "nodegraph.h"
 #include "animation.h"
+#include "noise.h"
+#include "RobotConfig.h"
 #include <cstring>
 
+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<float>(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;
   }
 }
 
@@ -133,7 +148,13 @@ std::vector<Node*> NodeGraph::getSortedNodes() {
     while (!q.empty()) {
         uint8_t id = q.front();
         q.pop();
-    sorted.push_back(idMap[id]);
+        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) {
@@ -145,6 +166,7 @@ std::vector<Node*> NodeGraph::getSortedNodes() {
     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<VariableSource>(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;
+          if (offset + 6 > length) break;  // 4 bytes for float + 2 bytes for uint16_t
-          offset += 17;  // skip for now
+
-          break;
+          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<const VariableNode*>(node);
           output += " | source: " + String(servo->source);
+          output += " | arg0: " + String(servo->arg0);
+          break;
+        }
+      case TYPE_NOISENODE:
+        {  // NoiseNode
+          const NoiseNode* noise = static_cast<const NoiseNode*>(node);
+          output += " | Frequency " + String(noise->frequency, 4);
+          output += " | Seed " + String(noise->seed);
           break;
         }
-        // case 2: { // NoiseNode
-        //   const NoiseNode* noise = static_cast<const NoiseNode*>(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<uint8_t> NodeGraph::serialize() {
+  std::vector<uint8_t> data;
+
+  // Node count
+  data.push_back(static_cast<uint8_t>(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<uint8_t*>(&x), reinterpret_cast<uint8_t*>(&x) + sizeof(uint16_t));
+    data.insert(data.end(), reinterpret_cast<uint8_t*>(&y), reinterpret_cast<uint8_t*>(&y) + sizeof(uint16_t));
+
+    switch (node->type) {
+      case TYPE_SERVONODE:
+        {
+          auto* n = static_cast<ServoNode*>(node);
+          data.push_back(n->motorID);
+          break;
+        }
+      case TYPE_CURVENODE:
+        {
+          auto* n = static_cast<CurveNode*>(node);
+          data.push_back(n->curveID);
+          break;
+        }
+      case TYPE_VARIABLENODE:
+        {
+          auto* n = static_cast<VariableNode*>(node);
+          data.push_back(static_cast<uint8_t>(n->source));
+          data.push_back(static_cast<uint8_t>(n->arg0));
+          break;
+        }
+      case TYPE_MATHNODE:
+        {
+          auto* n = static_cast<MathNode*>(node);
+          data.push_back(static_cast<uint8_t>(n->op));
+          data.insert(data.end(), reinterpret_cast<uint8_t*>(&n->value), reinterpret_cast<uint8_t*>(&n->value) + sizeof(float));
+          break;
+        }
+      case TYPE_MAPNODE:
+        {
+          auto* n = static_cast<MapNode*>(node);
+          data.insert(data.end(), reinterpret_cast<uint8_t*>(&n->inMin), reinterpret_cast<uint8_t*>(&n->inMin) + sizeof(float));
+          data.insert(data.end(), reinterpret_cast<uint8_t*>(&n->inMax), reinterpret_cast<uint8_t*>(&n->inMax) + sizeof(float));
+          data.insert(data.end(), reinterpret_cast<uint8_t*>(&n->outMin), reinterpret_cast<uint8_t*>(&n->outMin) + sizeof(float));
+          data.insert(data.end(), reinterpret_cast<uint8_t*>(&n->outMax), reinterpret_cast<uint8_t*>(&n->outMax) + sizeof(float));
+          break;
+        }
+      case TYPE_NOISENODE:
+        {
+          auto* n = static_cast<NoiseNode*>(node);
+          data.insert(data.end(), reinterpret_cast<uint8_t*>(&n->frequency), reinterpret_cast<uint8_t*>(&n->frequency) + sizeof(float));
+          data.insert(data.end(), reinterpret_cast<uint8_t*>(&n->seed), reinterpret_cast<uint8_t*>(&n->seed) + sizeof(uint16_t));
+          break;
+        }
+    }
+  }
+
+  // Connection count
+  data.push_back(static_cast<uint8_t>(connections.size()));
+
+  // Serialize connections
+  for (const NodeConnection& conn : connections) {
+    data.push_back(conn.fromID);
+    data.push_back(conn.toID);
+  }
+
+  return data;
+}

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<Node*> nodes;
   std::vector<NodeConnection> connections;
-Node* findNodeByID(uint8_t id) const;
+  Node* findNodeByID(uint8_t id) const;
 
   void tick(uint32_t currentTick, const Animation& animation);
   std::vector<std::pair<uint8_t, uint16_t>> getServoOutputs() const;
   std::vector<Node*> getSortedNodes();
   void bindAnimationContext(Animation* animation);
-
+  std::vector<uint8_t> serialize();
-
 };
 
 

noise.cpp

@@ -0,0 +1,78 @@
+#include "noise.h"
+#include <math.h>
+#include <algorithm>
+
+
+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<int>(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);
+}

noise.h

@@ -0,0 +1,6 @@
+#pragma once
+#include <stdint.h>
+
+extern uint8_t perm[512];
+float perlin1D_octave(uint16_t seed, float x, int octaves, float persistence);
+float perlin1D(uint16_t seed, float x);

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<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;
+}

robotconfig.h

@@ -0,0 +1,31 @@
+#pragma once
+#include <Arduino.h>
+#include <vector>
+
+
+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<Motor> 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<uint8_t> serializeToBytes() const;
+};