From 91f6e269eda26b610c4082f6b4c5ae9fb0071f60 Mon Sep 17 00:00:00 2001 From: Jake Date: Wed, 1 Oct 2025 00:35:00 +0800 Subject: [PATCH] removed DE/RE implementation and delay as no longer required with direct TTL setup --- HansonServo.ino | 34 ++++++++++++++------- feetech.cpp | 80 ++++++++++++++++++++++++------------------------- feetech.h | 18 ++++++----- 3 files changed, 74 insertions(+), 58 deletions(-) diff --git a/HansonServo.ino b/HansonServo.ino index 875cacc..081a06a 100644 --- a/HansonServo.ino +++ b/HansonServo.ino @@ -22,10 +22,10 @@ uint8_t payload[MAX_PAYLOAD_SIZE]; // Global or static // ESP32 S2 PINOUT -#define RX_PIN 17 // DI -#define TX_PIN 18 // RO -#define DE_PIN 33 // Driver Enable -#define RE_PIN 3 // Receiver Enable +#define RX_PIN 18 // RO +#define TX_PIN 17 // DI +#define DE_PIN 7 // Driver Enable +#define RE_PIN 8 // Receiver Enable Animation anim; Animation sweep; @@ -49,18 +49,29 @@ void setup() { delay(500); } + // pinMode(RX_PIN, OUTPUT); + // pinMode(TX_PIN, OUTPUT); + // pinMode(DE_PIN, OUTPUT); + // while (true){ + // Serial.println(!digitalRead(RX_PIN)); + // digitalWrite(RX_PIN, !digitalRead(RX_PIN)); + // digitalWrite(TX_PIN, !digitalRead(TX_PIN)); + // digitalWrite(DE_PIN, !digitalRead(DE_PIN)); + // delay(2000); + // } + pos2[3] = flipBytes(pos2[3]); servos.begin(); - if (!FFat.begin()) { + if (!FFat.begin(true)) { Serial.println("FFat mount failed"); return; } - anim.loadFromFile("/bob.anim"); - anim.printKeyframes(); - playAnimation(anim); + // anim.loadFromFile("/bob.anim"); + // anim.printKeyframes(); + // playAnimation(anim); // ClearFiles(); // // PrintFileList(); // sweep.clear(); @@ -525,8 +536,11 @@ unsigned long lastSend = 0; void loop() { HandleSerialRequests(); // put your main code here, to run repeatedly: - //PingAll(); - + // 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; diff --git a/feetech.cpp b/feetech.cpp index b9bdee4..be70637 100644 --- a/feetech.cpp +++ b/feetech.cpp @@ -94,6 +94,9 @@ 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); + // Send packet //Serial.println("PING"); setModeTransmit(); @@ -398,57 +401,52 @@ void Feetech::waitOnReply(unsigned long timeout) { } uint8_t Feetech::waitOnData1Byte(unsigned long timeout) { - unsigned long startTime = millis(); // Record the start time + unsigned long startTime = millis(); - while (millis() - startTime < timeout) { // Loop until timeout + 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 { - // } - - // Display on Serial - Serial.print("recv: "); - Serial.print(buffer[2]); - Serial.print(" "); - if (buffer[4] == 0x00) { - Serial.print("OK"); - } else { - Serial.print("NOK"); + // Skip echoed packet if it matches the start of the buffer + int startIndex = 0; + if (count >= lastSentLength && memcmp(buffer, lastSentPacket, lastSentLength) == 0) { + startIndex = lastSentLength; } - 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(" "); + + // 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 (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(" "); + } + Serial.println(); + + return buffer[startIndex + 5]; // Return value byte + } } - Serial.println(); - - uint8_t val = buffer[5]; - return val; - - break; // Exit the loop after processing the reply - } else { - //Serial.println(millis() - startTime); } - delay(1); // Small delay to prevent busy-waiting + delay(1); } return 0; } + uint16_t Feetech::waitOnData2Bytes(unsigned long timeout) { unsigned long startTime = millis(); // Record the start time @@ -515,15 +513,15 @@ size_t Feetech::receiveData(byte* buffer, size_t bufferSize) { } void Feetech::setModeTransmit() { - digitalWrite(DE_PIN, HIGH); - digitalWrite(RE_PIN, HIGH); - delay(10); + // digitalWrite(DE_PIN, HIGH); + // digitalWrite(RE_PIN, HIGH); + //delay(10); } void Feetech::setModeReceive() { - digitalWrite(DE_PIN, LOW); - digitalWrite(RE_PIN, LOW); - delay(10); + // digitalWrite(DE_PIN, LOW); + // digitalWrite(RE_PIN, LOW); + //delay(10); } void Feetech::update() { diff --git a/feetech.h b/feetech.h index e79bb68..a020edb 100644 --- a/feetech.h +++ b/feetech.h @@ -1,7 +1,7 @@ #ifndef FEETECH_H #define FEETECH_H -#include +#include #include #include @@ -38,7 +38,7 @@ public: uint16_t getLoad(uint8_t id); uint8_t getTemperature(uint8_t id); - + uint8_t getMoving(uint8_t id); float getCurrent(uint8_t id); @@ -91,12 +91,12 @@ public: static const byte REQUEST_POSITION = 0x38; // 2 bytes static const byte REQUEST_CURRENT_SPEED = 0x3A; // 2 bytes - static const byte REQUEST_CURRENT_LOAD = 0x3C; // 2 bytes + static const byte REQUEST_CURRENT_LOAD = 0x3C; // 2 bytes - static const byte REQUEST_VOLTAGE = 0x3E; // 1 byte - - static const byte REQUEST_TEMPERATURE = 0x3F; // 1 byte - static const byte REQUEST_MOVING = 0x42; // 1 byte + static const byte REQUEST_VOLTAGE = 0x3E; // 1 byte + + static const byte REQUEST_TEMPERATURE = 0x3F; // 1 byte + static const byte REQUEST_MOVING = 0x42; // 1 byte static const byte REQUEST_CURRENT_CURRENT = 0x45; // 2 bytes @@ -110,6 +110,10 @@ public: static const byte SMS_STS_57600 = 6; static const byte SMS_STS_38400 = 7; + uint8_t lastSentPacket[32]; + int lastSentLength = 0; + + private: HardwareSerial& serial; // Reference to the HardwareSerial object uint8_t DE_PIN = 20;