realrobotshk
/
HansonServo
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

master
Jake 2025-10-31 01:44:19 +08:00
parent 6b991fa982
commit e5d73fca5d
5 changed files with 302 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

200
HansonServo.ino
View File

@ -25,6 +25,7 @@ uint8_t payload[MAX_PAYLOAD_SIZE]; // Global or static
#define CMD_PLAY_FILE 0x08 #define CMD_PLAY_FILE 0x08
#define CMD_SCAN_CHANNEL 0x09 #define CMD_SCAN_CHANNEL 0x09
#define CMD_WRITE_DATA 0x10 #define CMD_WRITE_DATA 0x10
#define CMD_WRITE_CONFIG_UPDATE 0x12
#define CMD_START_POSITION_STREAM 0x14 #define CMD_START_POSITION_STREAM 0x14
#define POSITION_STREAM 0x15 #define POSITION_STREAM 0x15
@ -87,28 +88,18 @@ void setup() {
delay(500); 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 // 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(6, INPUT);
pinMode(7, INPUT); pinMode(7, INPUT);
@ -138,17 +129,35 @@ void setup() {
if (!FFat.begin(true)) { if (!FFat.begin(true)) {
Serial.println("FFat mount failed"); Serial.println("FFat mount failed");
return; 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 = "";
// anim.saveToFile("/scurve.anim"); for (size_t i = 0; i < bytes.size(); ++i) {
// Serial.println("SAVED"); 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); //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.print("CurveSegment size: ");
// Serial.println(sizeof(CurveSegment)); // Serial.println(sizeof(CurveSegment));
//anim.createBasicSCurve(); //anim.createBasicSCurve();
@ -209,20 +218,21 @@ void setup() {
// servos[1]->setMinAngleLimit(13, 900); // servos[1]->setMinAngleLimit(13, 900);
// servos[0]->setMaxAngleLimit(13, 500); // servos[0]->setMaxAngleLimit(13, 500);
// servos[1]->setMaxAngleLimit(13, 3900); // servos[1]->setMaxAngleLimit(13, 3900);
//SetID(13, 17);
} }
void SetID(uint8_t oldID, uint8_t newID) { void SetID(uint8_t oldID, uint8_t newID) {
Serial.println("Setting Lock to 0"); Serial.println("Setting Lock to 0");
Serial.println(servos[0]->setLock(oldID, 0)); Serial.println(servos[1]->setLock(oldID, 0));
delay(1000); delay(1000);
Serial.print("Changing ID "); Serial.print("Changing ID ");
Serial.print(oldID); Serial.print(oldID);
Serial.print(" to "); Serial.print(" to ");
Serial.println(newID); Serial.println(newID);
Serial.println(servos[0]->setID(oldID, newID)); Serial.println(servos[1]->setID(oldID, newID));
delay(1000); delay(1000);
Serial.println("Setting Lock to 1"); Serial.println("Setting Lock to 1");
Serial.println(servos[0]->setLock(newID, 1)); Serial.println(servos[1]->setLock(newID, 1));
delay(1000); delay(1000);
} }
@ -338,6 +348,10 @@ void handleCommand(uint8_t command, const uint8_t* payload, uint16_t length) {
startPositionStream(payload, length); startPositionStream(payload, length);
break; break;
case CMD_WRITE_CONFIG_UPDATE:
handleConfigUpdate(payload, length);
break;
default: default:
Serial.print("Unknown command: "); Serial.print("Unknown command: ");
Serial.println(command, HEX); Serial.println(command, HEX);
@ -361,6 +375,69 @@ void startPositionStream(const uint8_t* payload, uint16_t length) {
streamPositions = start; 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) { void handleDataWrite(const uint8_t* payload, uint16_t length) {
uint8_t channel = payload[0]; uint8_t channel = payload[0];
@ -745,20 +822,59 @@ void handleSetPosition(const uint8_t* payload, uint16_t length) {
return; 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 // Scans 0-254 and responds with the channel and ID as each successful ping is received
// Signals end by responding with channel and 255 // Signals end by responding with channel and 255
void handleScanChannel(const uint8_t* payload, uint16_t length) { void handleScanChannel(const uint8_t* payload, uint16_t length) {
@ -961,7 +1077,7 @@ void runNodeAnimation() {
if (!currentAnimation) return; // ✅ Prevent crash if (!currentAnimation) return; // ✅ Prevent crash
if (!currentAnimation->isActive()) return; if (!currentAnimation->isActive()) return;
config.enableAllMotors(); config.enableAllMotors();
uint32_t now = millis(); uint32_t now = millis();
if (now - lastTickTime >= FRAME_INTERVAL_MS) { if (now - lastTickTime >= FRAME_INTERVAL_MS) {
@ -1006,6 +1122,16 @@ config.enableAllMotors();
sendMessage(message); 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 // ✅ Send to servo controller
servos[0]->syncWritePos(motorIDs.data(), positions.data(), motorCount); servos[0]->syncWritePos(motorIDs.data(), positions.data(), motorCount);

27
feetech.cpp
View File

@ -11,21 +11,6 @@ void Feetech::begin() {
setModeReceive(); 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) { 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 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++) { for (uint8_t i = 0; i < count; i++) {
packet[index++] = ids[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]; pos = positions[i];
}
packet[index++] = (uint8_t)((pos >> 8) & 0xFF); // High byte packet[index++] = (uint8_t)((pos >> 8) & 0xFF); // High byte
packet[index++] = (uint8_t)(pos & 0xFF); // Low byte packet[index++] = (uint8_t)(pos & 0xFF); // Low byte
@ -147,9 +132,11 @@ void Feetech::sendPing(uint8_t id) {
} }
void Feetech::clearEcho(uint8_t length) { void Feetech::clearEcho(uint8_t length) {
if (filterEcho) {
for (int i = 0; i < length; i++) { for (int i = 0; i < length; i++) {
if (serial.available()) serial.read(); if (serial.available()) serial.read();
} }
}
} }
void Feetech::testRequest() { void Feetech::testRequest() {

17
feetech.h
View File

@ -8,19 +8,21 @@
// SCS & HLS big-endian (high byte first) // SCS & HLS big-endian (high byte first)
// STS little-endian (low byte first) // STS little-endian (low byte first)
//
class Feetech { class Feetech {
public: public:
enum FeetechMode { 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); Feetech(HardwareSerial& serial, int DE_PIN, int RE_PIN, int TX_PIN, int RX_PIN);
void begin(); void begin();
void sendPing(uint8_t id); void sendPing(uint8_t id);
void clearEcho(uint8_t length); void clearEcho(uint8_t length);
void printModel(uint16_t modelPacket);
uint16_t getModel(uint8_t id); uint16_t getModel(uint8_t id);
uint8_t getID(uint8_t id); uint8_t getID(uint8_t id);
uint8_t setID(uint8_t id, uint8_t newId); 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_57600 = 6;
static const byte SMS_STS_38400 = 7; 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]; uint8_t lastSentPacket[32];
int lastSentLength = 0; int lastSentLength = 0;
bool filterEcho = true; // Filters out echo caused by having simple diode/resistor TTL splitter
private: private:
HardwareSerial& serial; // Reference to the HardwareSerial object HardwareSerial& serial; // Reference to the HardwareSerial object

101
robotconfig.cpp
View File

@ -1,4 +1,5 @@
#include "RobotConfig.h" #include "RobotConfig.h"
#include <FFat.h>
uint16_t RobotConfig::getMotorPosition(uint8_t motorID) const { uint16_t RobotConfig::getMotorPosition(uint8_t motorID) const {
for (const Motor& motor : motors) { for (const Motor& motor : motors) {
@ -10,6 +11,16 @@ uint16_t RobotConfig::getMotorPosition(uint8_t motorID) const {
return 2047; 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) { bool RobotConfig::setMotorPosition(uint8_t motorID, uint16_t newPosition) {
for (Motor& motor : motors) { for (Motor& motor : motors) {
if (motor.motorID == motorID) { if (motor.motorID == motorID) {
@ -62,7 +73,7 @@ String RobotConfig::serializeJSON() const {
output += "{"; output += "{";
output += "\"motorID\":" + String(m.motorID) + ","; output += "\"motorID\":" + String(m.motorID) + ",";
output += "\"name\":\"" + m.name + "\","; output += "\"name\":\"" + m.name + "\",";
output += "\"position\":" + String(m.position); output += "\"model\":" + String(m.servoModel.minor);
output += "}"; output += "}";
if (i < motors.size() - 1) { if (i < motors.size() - 1) {
@ -112,3 +123,91 @@ std::vector<uint8_t> RobotConfig::serializeToBytes() const {
return buffer; 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);
}

24
robotconfig.h
View File

@ -2,30 +2,40 @@
#include <Arduino.h> #include <Arduino.h>
#include <vector> #include <vector>
struct FirmwareVersion { struct FirmwareVersion {
uint8_t major; uint8_t major;
uint8_t minor; uint8_t minor;
}; };
struct ServoModel {
uint8_t major;
uint8_t minor;
};
struct Motor { struct Motor {
String name; // Optional: name or ID of the motor String name;
uint8_t motorID; uint8_t motorID;
uint16_t position; // Current position in encoder ticks or degrees ServoModel servoModel;
uint16_t position;
bool isEnabled = true; bool isEnabled = true;
}; };
struct RobotConfig { struct RobotConfig {
String deviceName; // Name of the robot String deviceName;
FirmwareVersion firmwareVersion; FirmwareVersion firmwareVersion;
std::vector<Motor> motors; // Dynamic array of motors std::vector<Motor> motors;
uint16_t getMotorPosition(uint8_t motorID) const; uint16_t getMotorPosition(uint8_t motorID) const;
uint8_t getMotorModel(uint8_t motorID);
bool setMotorPosition(uint8_t motorID, uint16_t newPosition); 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); bool isMotorEnabled(uint8_t motorID);
void enableAllMotors(); void enableAllMotors();
String serializeJSON() const; String serializeJSON() const;
std::vector<uint8_t> serializeToBytes() 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");
}; };