can recieve config updates from control panel, SCS/STS logic offloaded to pos move and play animation in main ino

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.deviceName = "Little Elephant";
-  config.firmwareVersion.major = 0;
+  // config.firmwareVersion.major = 0;
-  config.firmwareVersion.minor = 2;
+  // config.firmwareVersion.minor = 2;
 
   // Add a few motors
-  config.motors.push_back({ "Neck Twist", 10, 2048 });
+  // config.motors.push_back({ "Ankle Left", 10, 2048 });
-  config.motors.push_back({ "Left Shoulder", 11, 2048 });
+  // config.motors.push_back({ "Ankle Right", 11, 2048 });
-  config.motors.push_back({ "Right Shoulder", 12, 2048 });
+  // config.motors.push_back({ "Ankle Center", 12, 2048 });
-  config.motors.push_back({ "Pelvis Tilt", 13, 2048 });
+  // config.motors.push_back({ "Knee", 13, 2048 });
-  config.motors.push_back({ "Neck Tilt", 14, 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 = "";
+
+  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");
+  //Serial.println("Loading test.anim");
-  anim.loadFromFile("/test.anim");
+  //anim.loadFromFile("/test.anim");
-  Serial.println(anim.header.frameCount);
+  //Serial.println(anim.header.frameCount);
-  anim.printCurves();
+  //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,18 +822,57 @@ void handleSetPosition(const uint8_t* payload, uint16_t length) {
     return;
   }
 
-  uint8_t count = 0;
+  uint8_t motorCount = length / 3;
-  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
 
-    //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
@@ -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);
 

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) {
+    // if (feetechMode == MODE_SCS) {
-      pos = map(positions[i], 0, 4095, 0, 1023);
+    //   pos = map(positions[i], 0, 4095, 0, 1023);
-    } else if (feetechMode == MODE_STS) {
+    // } 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,10 +132,12 @@ void Feetech::sendPing(uint8_t id) {
 }
 
 void Feetech::clearEcho(uint8_t length) {
+  if (filterEcho) {
     for (int i = 0; i < length; i++) {
       if (serial.available()) serial.read();
     }
   }
+}
 
 void Feetech::testRequest() {
   uint8_t packet[6] = { 0XFF, 0XFF, 0X00, 0X02, 0X06, 0XF7 };

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_SCS = 5,
-    MODE_STS,
+    MODE_SMSA = 6,
-    MODE_SMS
+    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);
@@ -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

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) {
@@ -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);
+}

robotconfig.h

@@ -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 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");
+
 };