removed DE/RE implementation and delay as no longer required with direct TTL setup

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:
+  // 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;

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,38 +401,35 @@ 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;
       }
+
+      // 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 = 0; i < count; i++) {
+          for (int i = startIndex; i < startIndex + packetSize; i++) {
             Serial.print("0x");
             if (buffer[i] < 0x10) Serial.print("0");
             Serial.print(buffer[i], HEX);
@@ -437,18 +437,16 @@ uint8_t Feetech::waitOnData1Byte(unsigned long timeout) {
           }
           Serial.println();
 
-      uint8_t val = buffer[5];
-      return val;
-
-      break;  // Exit the loop after processing the reply
-    } else {
-      //Serial.println(millis() - startTime);
+          return buffer[startIndex + 5]; // Return value byte
         }
-    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() {

feetech.h

@@ -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;