synwrite received, but not sending correct moves

rs485transmitter.ino

@@ -61,6 +61,89 @@ void sendWritePos(uint8_t id, uint16_t position) {
   }
 }
 
+void syncWriteTwoServos(uint8_t id1, uint16_t pos1, uint8_t id2, uint16_t pos2) {
+  const uint8_t SYNC_WRITE = 0x83;
+  const uint8_t START_BYTE = 0xFF;
+  const uint8_t DATA_LEN = 0x05; // 2 bytes for position + 1 byte for speed (optional)
+
+  // Build packet
+  uint8_t packet[] = {
+    START_BYTE, START_BYTE, 0xFE, // Broadcast ID
+    0x0F,                         // Length = 15 bytes total
+    SYNC_WRITE,
+    0x2A,                         // Starting address for Goal Position (0x2A)
+    0x04,                         // Data length per servo (2 bytes pos + 2 bytes speed)
+    id1,
+    (uint8_t)(pos1 & 0xFF),       // Position low byte
+    (uint8_t)((pos1 >> 8) & 0xFF),// Position high byte
+    0x00, 0x00,                   // Speed (optional, set to 0)
+    id2,
+    (uint8_t)(pos2 & 0xFF),
+    (uint8_t)((pos2 >> 8) & 0xFF),
+    0x00, 0x00
+  };
+
+  // Calculate checksum
+  uint8_t checksum = 0;
+  for (int i = 2; i < sizeof(packet); i++) {
+    checksum += packet[i];
+  }
+  checksum = ~checksum;
+
+// Send packet
+  SetModeTransmit();
+  Serial1.write(packet, sizeof(packet));
+  Serial1.write(checksum);
+  Serial1.flush();
+  SetModeReceive();
+}
+void syncWriteServos(uint8_t* ids, uint16_t* positions, uint8_t count) {
+  const uint8_t SYNC_WRITE = 0x83;
+  const uint8_t START_BYTE = 0xFF;
+  const uint8_t BROADCAST_ID = 0xFE;
+  const uint8_t START_ADDR = 0x2A;     // Goal Position address
+  const uint8_t DATA_LEN_PER_SERVO = 0x04; // 2 bytes pos + 2 bytes speed
+
+  // Calculate packet length: instruction + address + data length + (count × data per servo)
+  uint8_t packetLen = 4 + (count * (1 + DATA_LEN_PER_SERVO)); // instruction + addr + len + servo data
+
+  // Create packet buffer
+  uint8_t packet[3 + packetLen]; // 3 header bytes + payload
+  uint8_t index = 0;
+
+  // Header
+  packet[index++] = START_BYTE;
+  packet[index++] = START_BYTE;
+  packet[index++] = BROADCAST_ID;
+  packet[index++] = packetLen;
+  packet[index++] = SYNC_WRITE;
+  packet[index++] = START_ADDR;
+  packet[index++] = DATA_LEN_PER_SERVO;
+
+  // Servo data
+  for (uint8_t i = 0; i < count; i++) {
+    packet[index++] = ids[i];
+    packet[index++] = (uint8_t)(positions[i] & 0xFF);        // Low byte
+    packet[index++] = (uint8_t)((positions[i] >> 8) & 0xFF); // High byte
+    packet[index++] = 0x00; // Speed low byte
+    packet[index++] = 0x00; // Speed high byte
+  }
+
+  // Checksum
+  uint8_t checksum = 0;
+  for (uint8_t i = 2; i < index; i++) {
+    checksum += packet[i];
+  }
+  checksum = ~checksum;
+
+  // Send packet
+  SetModeTransmit();
+  Serial1.write(packet, index);
+  Serial1.write(checksum);
+  Serial1.flush();
+  SetModeReceive();
+}
+
 // Send a ping command to a specific servo
 void sendPing(uint8_t id) {
   uint8_t packet[6];
@@ -111,7 +194,7 @@ void WaitOnReply(unsigned long timeout) {
       Serial.print("recv: ");
       Serial.print(buffer[2]);
       Serial.print(" ");
-      if (buffer[4] == 0x00){
+      if (buffer[4] == 0x00) {
         Serial.print("OK");
       } else {
         Serial.print("NOK");
@@ -133,28 +216,94 @@ void WaitOnReply(unsigned long timeout) {
 }
 
 void SetModeTransmit() {
-  digitalWrite(DE_PIN, HIGH); 
+  digitalWrite(DE_PIN, HIGH);
   digitalWrite(RE_PIN, HIGH);
   delay(10);
 }
 
 void SetModeReceive() {
-  digitalWrite(DE_PIN, LOW);   
+  digitalWrite(DE_PIN, LOW);
   digitalWrite(RE_PIN, LOW);
   delay(10);
 }
 
-void loop() {
+void PingAll() {
+  Serial.println("PINGING ALL 0-255");
+  for (int i = 0; i < 255; i++) {
 
-  sendWritePos(2, random(0, 4096));
-  WaitOnReply(1000);
-  delay(1000);
-  sendPing(200);
-  Serial.println("WAITING...");
-  WaitOnReply(1000);
-  delay(1000);
-  sendPing(5);
-  Serial.println("WAITING...");
-  WaitOnReply(1000);
-  delay(1000);
+    sendPing(i);
+
+    WaitOnReply(100);
+  }
+  Serial.println("PINGING COMPLETE");
+}
+
+void TestRange(uint8_t id) {
+  for (int i = 0; i < 4095; i += 256) {
+    sendWritePos(id, i);
+    WaitOnReply(100);
+    delay(100);
+  }
+  for (int i = 4095; i > 0; i -= 256) {
+
+    sendWritePos(id, i);
+    WaitOnReply(100);
+    delay(100);
+  }
+}
+
+uint8_t ids[] = { 0, 4, 10, 15, 40, 41, 43, 44};
+uint16_t pos[8];
+
+void loop() {
+  //PingAll();
+  for (int i = 0; i < 4; i += 1) {
+    for (int x = 0; x < 8; x++){
+    pos[x] = i;
+    }
+    syncWriteServos(ids, pos, 8);
+    Serial.println(i);
+    //syncWriteTwoServos(ids[0], pos[0], ids[1], pos[1]);
+    delay(100);
+  }
+  for (int i = 4; i > 0; i -= 8) {
+    for (int x = 0; x < 8; x++){
+    pos[x] = i;
+    }
+    syncWriteServos(ids, pos, 2);
+    Serial.println(i);
+    //syncWriteTwoServos(ids[0], pos[0], ids[1], pos[1]);
+    delay(100);
+  }
+  // TestRange(0);
+  // TestRange(4);
+  // TestRange(10);
+  // TestRange(15);
+  // TestRange(40);
+  // TestRange(41);
+  // TestRange(43);
+  // TestRange(44);
+
+  // sendWritePos(123, 1000);
+  //delay(2000);
+
+  // sendWritePos(123, 2000);
+  // delay(2000);
+
+
+
+  // sendWritePos(34, 4095);
+  // WaitOnReply(50);
+  // delay(100);
+  // sendWritePos(34, 0);
+  // WaitOnReply(50);
+  // delay(100);
+
+  // WaitOnReply(1000);
+  // delay(1000);
+  // delay(1000);
+  // sendPing(5);
+  // Serial.println("WAITING...");
+  // WaitOnReply(1000);
+  // delay(1000);
 }