From fb38ca9bdb0de9df142046296c8bd2e657a0beb6 Mon Sep 17 00:00:00 2001 From: Jake Date: Sat, 4 Oct 2025 18:29:07 +0800 Subject: [PATCH] SCS/STS types and automatic dealing with that implemented somewhat. request ping gives id, min/max angle, and position --- HansonServo.ino | 255 ++++++++++++++++++++++++++++++--------- feetech.cpp | 311 ++++++++++++++++++++++++++++++++---------------- feetech.h | 22 +++- 3 files changed, 427 insertions(+), 161 deletions(-) diff --git a/HansonServo.ino b/HansonServo.ino index 081a06a..68415da 100644 --- a/HansonServo.ino +++ b/HansonServo.ino @@ -19,19 +19,25 @@ uint8_t payload[MAX_PAYLOAD_SIZE]; // Global or static #define CMD_MESSAGE 0x06 #define CMD_SET_POSITION 0x07 #define CMD_PLAY_FILE 0x08 +#define CMD_SCAN_CHANNEL 0x09 // ESP32 S2 PINOUT -#define RX_PIN 18 // RO -#define TX_PIN 17 // DI -#define DE_PIN 7 // Driver Enable -#define RE_PIN 8 // Receiver Enable +#define CH0_RX_PIN 13 +#define CH0_TX_PIN 12 + +// #define RX_PIN 18 // RO +// #define TX_PIN 17 // DI +#define CH1_RX_PIN 11 // RO +#define CH1_TX_PIN 10 // DI +#define DE_PIN 7 // Driver Enable +#define RE_PIN 8 // Receiver Enable Animation anim; Animation sweep; Animation stagger; -Feetech servos = Feetech(Serial1, DE_PIN, RE_PIN, TX_PIN, RX_PIN); +Feetech* servos[2]; uint16_t flipBytes(uint16_t value) { return (value >> 8) | (value << 8); @@ -42,6 +48,10 @@ uint8_t ids[NUM_CHANNELS] = { 10, 11, 12, 13, 14 }; uint16_t pos1[] = { 0, 0, 0, 0, 0 }; uint16_t pos2[] = { 1023, 1023, 1023, 1023, 1023 }; +uint8_t idsSTS[NUM_CHANNELS] = { 15, 103 }; +uint16_t pos1STS[] = { 0, 0 }; +uint16_t pos2STS[] = { 1023, 1023 }; + void setup() { Serial.begin(1000000); for (int i = 0; i < 5; i++) { @@ -49,6 +59,11 @@ void setup() { delay(500); } + servos[0] = new Feetech(Serial1, DE_PIN, RE_PIN, CH0_TX_PIN, CH0_RX_PIN); // SCS + servos[0]->setFeetechMode(Feetech::MODE_SCS); + servos[1] = new Feetech(Serial2, DE_PIN, RE_PIN, CH1_TX_PIN, CH1_RX_PIN); // STS + servos[1]->setFeetechMode(Feetech::MODE_STS); + // pinMode(RX_PIN, OUTPUT); // pinMode(TX_PIN, OUTPUT); // pinMode(DE_PIN, OUTPUT); @@ -63,7 +78,8 @@ void setup() { pos2[3] = flipBytes(pos2[3]); - servos.begin(); + servos[0]->begin(); + servos[1]->begin(); if (!FFat.begin(true)) { Serial.println("FFat mount failed"); @@ -102,21 +118,35 @@ void setup() { // Serial.println(anim.getFrame(0, 0)); // Should show saved value - //SetID(11, 14); + //SetID(12, 16); + + //Serial.println(servos[1]->setLockSTS(16, 0)); + // Serial.println("Enable torque"); + // Serial.println(servos[0]->enableTorque(13)); + // Serial.println(servos[1]->enableTorque(13)); + // delay(3000); + // Serial.println("Disable torque"); + // Serial.println(servos[0]->disableTorque(13)); + // Serial.println(servos[1]->disableTorque(13)); + // delay(3000); + // servos[0]->setMinAngleLimit(13, 100); + // servos[1]->setMinAngleLimit(13, 900); + // servos[0]->setMaxAngleLimit(13, 500); + // servos[1]->setMaxAngleLimit(13, 3900); } void SetID(uint8_t oldID, uint8_t newID) { Serial.println("Setting Lock to 0"); - Serial.println(servos.setLock(oldID, 0)); + Serial.println(servos[0]->setLockSTS(oldID, 0)); delay(1000); Serial.print("Changing ID "); Serial.print(oldID); Serial.print(" to "); Serial.println(newID); - Serial.println(servos.setID(oldID, newID)); + Serial.println(servos[0]->setID(oldID, newID)); delay(1000); Serial.println("Setting Lock to 1"); - Serial.println(servos.setLock(newID, 1)); + Serial.println(servos[0]->setLockSTS(newID, 1)); delay(1000); } @@ -158,6 +188,37 @@ void HandleSerialRequests() { } } } +void sendMessage(const String& payload, uint8_t command = CMD_MESSAGE) { + uint16_t length = payload.length(); + + uint8_t checksum = CMD_MESSAGE ^ (length >> 8) ^ (length & 0xFF); + for (int i = 0; i < length; i++) { + checksum ^= payload[i]; + } + + Serial.write(HEADER1); + Serial.write(HEADER2); + Serial.write(command); + Serial.write((length >> 8) & 0xFF); + Serial.write(length & 0xFF); + Serial.write((const uint8_t*)payload.c_str(), length); + Serial.write(checksum); +} + +void sendMessage(const uint8_t* payload, uint16_t length, uint8_t command = CMD_MESSAGE) { + uint8_t checksum = command ^ (length >> 8) ^ (length & 0xFF); + for (uint16_t i = 0; i < length; i++) { + checksum ^= payload[i]; + } + + Serial.write(HEADER1); + Serial.write(HEADER2); + Serial.write(command); + Serial.write((length >> 8) & 0xFF); + Serial.write(length & 0xFF); + Serial.write(payload, length); + Serial.write(checksum); +} void handleCommand(uint8_t command, const uint8_t* payload, uint16_t length) { switch (command) { @@ -187,6 +248,11 @@ void handleCommand(uint8_t command, const uint8_t* payload, uint16_t length) { case CMD_PLAY_FILE: handlePlayAnimation(payload, length); + break; + + case CMD_SCAN_CHANNEL: + handleScanChannel(payload, length); + break; default: Serial.print("Unknown command: "); @@ -195,22 +261,7 @@ void handleCommand(uint8_t command, const uint8_t* payload, uint16_t length) { } } -void sendMessage(const String& payload, uint8_t command = CMD_MESSAGE) { - uint16_t length = payload.length(); - uint8_t checksum = CMD_MESSAGE ^ (length >> 8) ^ (length & 0xFF); - for (int i = 0; i < length; i++) { - checksum ^= payload[i]; - } - - Serial.write(HEADER1); - Serial.write(HEADER2); - Serial.write(command); - Serial.write((length >> 8) & 0xFF); - Serial.write(length & 0xFF); - Serial.write((const uint8_t*)payload.c_str(), length); - Serial.write(checksum); -} void handleIdRequest() { String payload = String(DEVICE_NAME) + "|" + FIRMWARE_VERSION; @@ -520,46 +571,122 @@ void handleSetPosition(const uint8_t* payload, uint16_t length) { //servos.sendWritePos(ids[motorId], position); } - servos.syncWritePos(ids, pos1, NUM_CHANNELS); - // String msg = "Set positions: "; - // for (int i = 0; i < count; i++) { - // msg += "ID " + String(ids[i]) + " → " + String(pos1[i]) + "; "; - // } - // sendMessage(msg.c_str(), CMD_SET_POSITION); + servos[0]->syncWritePos(ids, pos1, NUM_CHANNELS); } +// 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) { + if (length != 1) { + sendMessage("Length of scanChannel Request Wrong"); + return; + } + + + for (int i = 0; i < 254; i++) { + servos[payload[0]]->sendPing(i); + uint8_t val = servos[payload[0]]->waitOnData1Byte(10); + + if (val != 0) { + uint8_t response[4]; + response[0] = payload[0]; // channel + response[1] = i; // responding address + uint16_t mode = servos[payload[0]]->getModel(i); + uint16_t minAngleLimit = servos[payload[0]]->getMinAngleLimit(i); + uint16_t maxAngleLimit = servos[payload[0]]->getMaxAngleLimit(i); + uint16_t position = servos[payload[0]]->getPosition(i); + response[2] = (mode >> 8) & 0xFF; // high byte + response[3] = mode & 0xFF; // low byte + + response[4] = (minAngleLimit >> 8) & 0xFF; + response[5] = minAngleLimit & 0xFF; + + response[6] = (maxAngleLimit >> 8) & 0xFF; + response[7] = maxAngleLimit & 0xFF; + + response[8] = (position >> 8) & 0xFF; + response[9] = position & 0xFF; + + + sendMessage(response, 10, CMD_SCAN_CHANNEL); // send all 4 bytes + } + } + uint8_t r[2]; + r[0] = payload[0]; // channel + r[1] = 255; // responding address + + sendMessage(r, 2, CMD_SCAN_CHANNEL); + + + // std::vector successfulAddresses; + // servos[payload[0]]->pingAll(successfulAddresses); + // std::vector response; + // response.push_back(payload[0]); // channel + // response.push_back(successfulAddresses.size()); // count + + // for (uint8_t address : successfulAddresses) { + // response.push_back(address); + // } + + // sendMessage(response.data(), response.size(), CMD_SCAN_CHANNEL); +} + bool flip = false; unsigned long lastSend = 0; void loop() { + + HandleSerialRequests(); - // put your main code here, to run repeatedly: - // servos.sendWritePos(10, 0); - // delay(1000); - // servos.sendWritePos(10, 200); - // delay(1000); -//PingAll(); - // for (int i = 0; i < 1023; i++) { - // for (int z = 0; z < 5; z++) { - // pos1[z] = i; - // } - // servos.syncWritePos(ids, pos1, 5); - // delay(50); + // for (int i = 0; i < 500; i+=32) { + // // Serial.print("WRITE: "); + // // Serial.print(i); + // // Serial.print(" "); + // // Serial.println(servos[1]->setMinAngleLimit(16, i)); + + // Serial.print("READ: "); + // Serial.println(servos[1]->getMinAngleLimit(16)); + // delay(2000); // } + // servos[0]->sendWritePos(13, 0); + // servos[1]->sendWritePos(13, 0); - // playAnimation(sweep); - // playAnimation(stagger); - // playLayeredAnimation(sweep, stagger); - - - // servos.syncWritePos(ids, pos1, 5); + // Serial.print(servos[0]->getMinAngleLimit(13)); + // Serial.print("\t"); + // Serial.print(servos[0]->getMaxAngleLimit(13)); + // Serial.print("\t"); + // Serial.print(servos[1]->getMinAngleLimit(13)); + // Serial.print("\t"); + // Serial.println(servos[1]->getMaxAngleLimit(13)); // delay(1000); - // servos.syncWritePos(ids, pos2, 5); + // // + // for (int i = 0; i < 4095; i+=16){ + // Serial.println(i); + // servos[0]->sendWritePos(13, i); + // servos[1]->sendWritePos(13, i); + // delay(20); + // } // delay(1000); - + // for (int i = 4095; i > 0; i-=16){ + // Serial.println(i); + // servos[0]->sendWritePos(13, i); + // servos[1]->sendWritePos(13, i); + // delay(20); + // } + // delay(1000); + // delay(1000); + // for (int i = 0; i< 100; i++){ + // Serial.println(servos[1]->getGoalSpeed(13)); + // delay(10); + // } + // servos[0]->sendWritePos(13, 0); + // for (int i = 0; i< 100; i++){ + // Serial.println(servos[1]->getGoalSpeed(13)); + // delay(10); + // } if (millis() - lastSend > 2000) { //sendMessageFromESP32(String(millis())); @@ -627,7 +754,7 @@ void deleteFile(fs::FS& fs, const char* path) { void playAnimation(Animation& anim) { uint16_t positions[NUM_CHANNELS]; - const uint16_t frameCount = 400;//anim.getFrameCount(); + const uint16_t frameCount = 400; //anim.getFrameCount(); const uint32_t frameDelay = 1000 / FRAMES_PER_SECOND; uint32_t nextFrameTime = millis(); Serial.print("Frame Count: "); @@ -681,7 +808,7 @@ void playAnimation(Animation& anim) { positions[ch] = value; } Serial.println(positions[0]); - servos.syncWritePos(ids, positions, NUM_CHANNELS); + servos[0]->syncWritePos(ids, positions, NUM_CHANNELS); nextFrameTime += frameDelay; } } @@ -704,7 +831,7 @@ void playAnimationOLD(Animation& anim) { // Send frame to servos if (anim.getFramePositions(frame, positions)) { - servos.syncWritePos(ids, positions, NUM_CHANNELS); + servos[0]->syncWritePos(ids, positions, NUM_CHANNELS); } // Schedule next frame @@ -731,16 +858,28 @@ void playLayeredAnimation(Animation& base, Animation& overlay) { finalPositions[ch] = (basePositions[ch] + overlayPositions[ch]) / 2; } - servos.syncWritePos(ids, finalPositions, NUM_CHANNELS); + servos[0]->syncWritePos(ids, finalPositions, NUM_CHANNELS); nextFrameTime += frameDelay; } } - -void PingAll() { +void SCSPingAll() { std::vector successfulAddresses; - servos.pingAll(successfulAddresses); + servos[0]->pingAll(successfulAddresses); + + // Now successfulAddresses contains all successful pings + Serial.println("Successful Addresses:"); + for (uint8_t address : successfulAddresses) { + Serial.print(address); + Serial.print(" "); + } + Serial.println(); +} + +void STSPingAll() { + std::vector successfulAddresses; + servos[1]->pingAll(successfulAddresses); // Now successfulAddresses contains all successful pings Serial.println("Successful Addresses:"); diff --git a/feetech.cpp b/feetech.cpp index be70637..c36603f 100644 --- a/feetech.cpp +++ b/feetech.cpp @@ -11,6 +11,21 @@ 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 @@ -52,22 +67,29 @@ void Feetech::syncWritePos(uint8_t* ids, uint16_t* positions, uint8_t count) { Serial1.write(checksum); Serial1.flush(); setModeReceive(); + + clearEcho(sizeof(packet)); } + // Send move command to servo id:0-255, position:0-4095 -void Feetech::sendWritePos(uint8_t id, uint16_t position) { +uint8_t Feetech::sendWritePos(uint8_t id, uint16_t position) { 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] = 0x2A; // Address: Goal Position - packet[7] = (position >> 8) & 0xFF; // High byte - packet[6] = position & 0xFF; // Low byte - + 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] = 0x2A; // Address: Goal Position + if (feetechMode == MODE_SCS) { + packet[6] = (position >> 8) & 0xFF; // High byte + packet[7] = position & 0xFF; // Low byte + } else if (feetechMode == MODE_STS) { + packet[6] = position & 0xFF; // Low byte first for STS + packet[7] = (position >> 8) & 0xFF; // High byte second + } // Calculate checksum uint8_t sum = 0; for (int i = 2; i < 8; i++) sum += packet[i]; @@ -78,6 +100,10 @@ void Feetech::sendWritePos(uint8_t id, uint16_t position) { serial.write(packet, sizeof(packet)); serial.flush(); setModeReceive(); + clearEcho(sizeof(packet)); + + // Should later recieve a 1 byte response code packet + return waitOnData1Byte(10); } void Feetech::sendPing(uint8_t id) { @@ -94,8 +120,11 @@ void Feetech::sendPing(uint8_t id) { for (int i = 2; i < 5; i++) sum += packet[i]; packet[5] = ~sum; // Checksum - memcpy(lastSentPacket, packet, sizeof(packet)); - lastSentLength = sizeof(packet); + // memcpy(lastSentPacket, packet, sizeof(packet)); + // lastSentLength = sizeof(packet); + + + // Send packet //Serial.println("PING"); @@ -105,6 +134,13 @@ void Feetech::sendPing(uint8_t id) { serial.flush(); //delay(20); setModeReceive(); + clearEcho(sizeof(packet)); +} + +void Feetech::clearEcho(uint8_t length) { + for (int i = 0; i < length; i++) { + if (serial.available()) serial.read(); + } } void Feetech::testRequest() { @@ -120,28 +156,52 @@ void Feetech::testRequest() { waitOnReply(100); } -void Feetech::enableTorque(uint8_t id) { - uint8_t packet[8]; // Adjust size based on your packet structure +uint8_t Feetech::enableTorque(uint8_t id) { + uint8_t packet[8]; - packet[0] = 255; // Header - packet[1] = 255; // Header - packet[2] = id; // Servo ID - packet[3] = 4; // Length (instruction + parameters + checksum) - packet[4] = 3; // Instruction to enable torque - packet[5] = 1; // Parameter to enable torque + packet[0] = 0xFF; // Header + packet[1] = 0xFF; // Header + packet[2] = id; // Servo ID (ensure this matches the expected ID, e.g., 0x02) + packet[3] = 0x04; // Length (4 bytes of following data) + packet[4] = WRITE_DATA; // Instruction + packet[5] = REQUEST_TORQUE_ENABLE; // Write first address + packet[6] = 1; // Data to write // Calculate checksum uint8_t sum = 0; - for (int i = 2; i < 6; i++) { - sum += packet[i]; - } - packet[6] = ~sum; // Checksum (bitwise NOT) + for (int i = 2; i < 7; i++) sum += packet[i]; // Include all data bytes for checksum + packet[7] = ~sum; // Checksum setModeTransmit(); - serial.write(packet, sizeof(packet)); // Send the packet + serial.write(packet, 8); serial.flush(); - //delay(10); // Short delay setModeReceive(); + + return waitOnData1Byte(10); +} + +uint8_t Feetech::disableTorque(uint8_t id) { + uint8_t packet[8]; + + packet[0] = 0xFF; // Header + packet[1] = 0xFF; // Header + packet[2] = id; // Servo ID (ensure this matches the expected ID, e.g., 0x02) + packet[3] = 0x04; // Length (4 bytes of following data) + packet[4] = WRITE_DATA; // Instruction + packet[5] = REQUEST_TORQUE_ENABLE; // Write first address + packet[6] = 0; // Data to write + + // Calculate checksum + uint8_t sum = 0; + for (int i = 2; i < 7; i++) sum += packet[i]; // Include all data bytes for checksum + packet[7] = ~sum; // Checksum + + setModeTransmit(); + serial.write(packet, 8); + serial.flush(); + setModeReceive(); + + return waitOnData1Byte(10); } uint16_t Feetech::getModel(uint8_t id) { @@ -166,12 +226,40 @@ uint8_t Feetech::getBaudRate(uint8_t id) { uint16_t Feetech::getMinAngleLimit(uint8_t id) { sendRequest(id, REQUEST_MIN_ANGLE_LIMIT, 2); - return waitOnData2Bytes(10); + if (feetechMode == MODE_STS) { + return waitOnData2Bytes(10); + } else if (feetechMode == MODE_SCS) { + return flipBytes(waitOnData2Bytes(10)); + } } +uint16_t Feetech::setMinAngleLimit(uint8_t id, uint16_t limit) { + write2Bytes(id, REQUEST_MIN_ANGLE_LIMIT, limit); + if (feetechMode == MODE_SCS) { + return flipBytes(waitOnData2Bytes(10)); + } else if (feetechMode == MODE_STS) { + return waitOnData2Bytes(10); + } +} + + + uint16_t Feetech::getMaxAngleLimit(uint8_t id) { sendRequest(id, REQUEST_MAX_ANGLE_LIMIT, 2); - return waitOnData2Bytes(10); + if (feetechMode == MODE_SCS) { + return flipBytes(waitOnData2Bytes(10)); + } else if (feetechMode == MODE_STS) { + return waitOnData2Bytes(10); + } +} + +uint16_t Feetech::setMaxAngleLimit(uint8_t id, uint16_t limit) { + write2Bytes(id, REQUEST_MAX_ANGLE_LIMIT, limit); + if (feetechMode == MODE_SCS) { + return flipBytes(waitOnData2Bytes(10)); + } else if (feetechMode == MODE_STS) { + return waitOnData2Bytes(10); + } } uint8_t Feetech::getCWDeadZone(uint8_t id) { @@ -242,7 +330,11 @@ float Feetech::getVoltage(uint8_t id) { uint16_t Feetech::getPosition(uint8_t id) { sendRequest(id, REQUEST_POSITION, 2); - return waitOnData2Bytes(10); + if (feetechMode == MODE_STS) { + return waitOnData2Bytes(10); + } else if (feetechMode == MODE_SCS) { + return flipBytes(waitOnData2Bytes(10)); + } } int16_t Feetech::getSpeed(uint8_t id) { @@ -297,6 +389,8 @@ void Feetech::sendRequest(uint8_t id, byte instruction, uint8_t byteCount) { serial.write(packet, sizeof(packet)); serial.flush(); setModeReceive(); + + clearEcho(sizeof(packet)); } void Feetech::write1Byte(uint8_t id, byte instruction, uint8_t data) { @@ -316,30 +410,42 @@ void Feetech::write1Byte(uint8_t id, byte instruction, uint8_t data) { packet[7] = ~sum; // Checksum setModeTransmit(); - serial.write(packet, sizeof(packet)); + serial.write(packet, 8); serial.flush(); setModeReceive(); } void Feetech::write2Bytes(uint8_t id, byte instruction, uint16_t data) { - uint8_t packet[8]; + 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[7] = (data >> 8) & 0xFF; // High byte - packet[6] = data & 0xFF; // Low byte + 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 + } else if (feetechMode == MODE_STS) { + + packet[6] = data & 0xFF; // Low byte first for STS + packet[7] = (data >> 8) & 0xFF; // High byte second + } // Calculate checksum uint8_t sum = 0; - for (int i = 2; i < 8; i++) sum += packet[i]; + for (int i = 2; i <= 7; i++) { + sum += packet[i]; + } packet[8] = ~sum; - + for (int i = 0; i < 9; i++) { + Serial.print(packet[i], HEX); + Serial.print(" "); + } + Serial.println(); setModeTransmit(); - serial.write(packet, sizeof(packet)); + serial.write(packet, 9); serial.flush(); setModeReceive(); } @@ -347,10 +453,10 @@ void Feetech::write2Bytes(uint8_t id, byte instruction, uint16_t data) { void Feetech::pingAll(std::vector& successfulAddresses) { Serial.println("PINGING ALL 0-255"); successfulAddresses.clear(); // Clear any previous results - for (int i = 0; i < 20; i++) { + for (int i = 0; i < 255; i++) { //Serial.println(i); sendPing(i); - uint8_t val = waitOnData1Byte(50); + uint8_t val = waitOnData1Byte(10); if (val != 0) { //Serial.println(val); successfulAddresses.push_back(i); // Store the successful address @@ -412,32 +518,28 @@ uint8_t Feetech::waitOnData1Byte(unsigned long timeout) { buffer[count++] = serial.read(); } - // Skip echoed packet if it matches the start of the buffer - int startIndex = 0; - if (count >= lastSentLength && memcmp(buffer, lastSentPacket, lastSentLength) == 0) { - startIndex = lastSentLength; - } + // Look for valid packet starting with 0xFF 0xFF + for (int i = 0; i <= count - 4; i++) { + if (buffer[i] == 0xFF && buffer[i + 1] == 0xFF) { + uint8_t length = buffer[i + 3]; + int packetSize = length + 4; - // Check if there's a valid packet after the echo - if (count - startIndex >= 4 && buffer[startIndex] == 0xFF && buffer[startIndex + 1] == 0xFF) { - uint8_t length = buffer[startIndex + 3]; - int packetSize = length + 4; + if (i + packetSize <= count) { + // Serial.print("recv: "); + // Serial.print(buffer[i + 2]); // ID + // Serial.print(" "); + // Serial.print(buffer[i + 4] == 0x00 ? "OK" : "NOK"); + // Serial.print("\t"); + // for (int j = i; j < i + packetSize; j++) { + // Serial.print("0x"); + // if (buffer[j] < 0x10) Serial.print("0"); + // Serial.print(buffer[j], HEX); + // Serial.print(" "); + // } + // Serial.println(); - if (startIndex + packetSize <= count) { - Serial.print("recv: "); - Serial.print(buffer[startIndex + 2]); // ID - Serial.print(" "); - Serial.print(buffer[startIndex + 4] == 0x00 ? "OK" : "NOK"); - Serial.print("\t"); - for (int i = startIndex; i < startIndex + packetSize; i++) { - Serial.print("0x"); - if (buffer[i] < 0x10) Serial.print("0"); - Serial.print(buffer[i], HEX); - Serial.print(" "); + return buffer[i + 5]; // Return value byte } - Serial.println(); - - return buffer[startIndex + 5]; // Return value byte } } } @@ -447,56 +549,55 @@ uint8_t Feetech::waitOnData1Byte(unsigned long timeout) { } -uint16_t Feetech::waitOnData2Bytes(unsigned long timeout) { - unsigned long startTime = millis(); // Record the start time - while (millis() - startTime < timeout) { // Loop until timeout +uint16_t Feetech::waitOnData2Bytes(unsigned long timeout) { + unsigned long startTime = millis(); + + while (millis() - startTime < timeout) { if (serial.available()) { - Serial.println("RECV"); uint8_t buffer[32]; int count = 0; - // Read all available bytes while (serial.available() && count < sizeof(buffer)) { buffer[count++] = serial.read(); } - Serial.println(count); - if (count != 8) { - Serial.print("ERROR: Expected 8 byte reply, recieved "); - Serial.println(count); - return 0; - } else { - } + // Look for valid packet starting with 0xFF 0xFF + for (int i = 0; i <= count - 5; i++) { + if (buffer[i] == 0xFF && buffer[i + 1] == 0xFF) { + uint8_t length = buffer[i + 3]; + int packetSize = length + 4; - // Display on Serial - Serial.print("recv: "); - Serial.print(buffer[2]); - Serial.print(" "); - if (buffer[4] == 0x00) { - Serial.print("OK"); - } else { - Serial.print("NOK"); - } - Serial.print("\t"); - for (int i = 0; i < count; i++) { - Serial.print("0x"); - if (buffer[i] < 0x10) Serial.print("0"); - Serial.print(buffer[i], HEX); - Serial.print(" "); - } - Serial.println(); + if (i + packetSize <= count) { + // Serial.print("recv: "); + // Serial.print(buffer[i + 2]); // ID + // Serial.print(" "); + // Serial.print(buffer[i + 4] == 0x00 ? "OK" : "NOK"); + // Serial.print("\t"); + // for (int j = i; j < i + packetSize; j++) { + // Serial.print("0x"); + // if (buffer[j] < 0x10) Serial.print("0"); + // Serial.print(buffer[j], HEX); + // Serial.print(" "); + // } + // Serial.println(); - uint16_t val = (buffer[6] * 256) + buffer[5]; - return val; - - break; // Exit the loop after processing the reply + // Combine two bytes into a uint16_t (little-endian) + uint16_t value = buffer[i + 5] | (buffer[i + 6] << 8); + return value; + } + } + } } - delay(1); // Small delay to prevent busy-waiting + delay(1); } return 0; } + + + + void Feetech::sendData(const byte* data, size_t length) { // digitalWrite(_transmitPin, HIGH); // Enable transmit mode // _serial.write(data, length); @@ -524,6 +625,14 @@ void Feetech::setModeReceive() { //delay(10); } +uint16_t Feetech::flipBytes(uint16_t value) { + return (value >> 8) | (value << 8); +} + +void Feetech::setFeetechMode(FeetechMode newMode) { + feetechMode = newMode; +} + void Feetech::update() { if (Serial.available()) { setModeTransmit(); diff --git a/feetech.h b/feetech.h index a020edb..1556b46 100644 --- a/feetech.h +++ b/feetech.h @@ -11,15 +11,24 @@ class Feetech { public: + enum FeetechMode { + MODE_SCS, + MODE_STS, + MODE_SMS + }; 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); uint8_t getBaudRate(uint8_t id); uint16_t getMinAngleLimit(uint8_t id); + uint16_t setMinAngleLimit(uint8_t id, uint16_t limit); uint16_t getMaxAngleLimit(uint8_t id); + uint16_t setMaxAngleLimit(uint8_t id, uint16_t limit); uint8_t getCWDeadZone(uint8_t id); uint8_t getCCWDeadZone(uint8_t id); uint16_t getOffset(uint8_t id); @@ -44,7 +53,7 @@ public: float getVoltage(uint8_t id); void sendRequest(uint8_t id, uint8_t instruction, uint8_t byteCount); - void sendWritePos(uint8_t id, uint16_t position); + uint8_t sendWritePos(uint8_t id, uint16_t position); void syncWritePos(uint8_t* ids, uint16_t* positions, uint8_t count); void write1Byte(uint8_t id, byte instruction, uint8_t data); void write2Bytes(uint8_t id, byte instruction, uint16_t data); @@ -56,9 +65,12 @@ public: size_t receiveData(byte* buffer, size_t bufferSize); void setModeReceive(); void setModeTransmit(); + uint16_t flipBytes(uint16_t value); void update(); void testRequest(); - void enableTorque(uint8_t id); + uint8_t enableTorque(uint8_t id); + uint8_t disableTorque(uint8_t id); + void setFeetechMode(FeetechMode newMode); static const byte PING = 0x01; // QUERY THE WORKING STATUS static const byte READ_DATA = 0x02; // READ DATA @@ -110,6 +122,11 @@ 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; @@ -120,6 +137,7 @@ private: uint8_t RE_PIN = 10; uint8_t TX_PIN = 21; uint8_t RX_PIN = 9; + FeetechMode feetechMode = MODE_SCS; }; #endif // FEETECH_H \ No newline at end of file