From e5d73fca5d3fe265bcbd7edce8fdade27ce3a7e6 Mon Sep 17 00:00:00 2001 From: Jake Date: Fri, 31 Oct 2025 01:44:19 +0800 Subject: [PATCH] can recieve config updates from control panel, SCS/STS logic offloaded to pos move and play animation in main ino --- HansonServo.ino | 200 +++++++++++++++++++++++++++++++++++++++--------- feetech.cpp | 33 +++----- feetech.h | 25 +++--- robotconfig.cpp | 103 ++++++++++++++++++++++++- robotconfig.h | 24 ++++-- 5 files changed, 302 insertions(+), 83 deletions(-) diff --git a/HansonServo.ino b/HansonServo.ino index 0df278c..01dffa9 100644 --- a/HansonServo.ino +++ b/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 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 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); diff --git a/feetech.cpp b/feetech.cpp index b838d68..677756b 100644 --- a/feetech.cpp +++ b/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(); + } } } diff --git a/feetech.h b/feetech.h index ad7f67a..5fc3748 100644 --- a/feetech.h +++ b/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 diff --git a/robotconfig.cpp b/robotconfig.cpp index 6c031c8..8ac6288 100644 --- a/robotconfig.cpp +++ b/robotconfig.cpp @@ -1,4 +1,5 @@ #include "RobotConfig.h" +#include 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 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); +} diff --git a/robotconfig.h b/robotconfig.h index 2f845f1..f567e6a 100644 --- a/robotconfig.h +++ b/robotconfig.h @@ -2,30 +2,40 @@ #include #include - 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 motors; // Dynamic array of motors - + std::vector 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 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"); + };