From 32cff065cd252a62baacb361f533276fe0ea2216 Mon Sep 17 00:00:00 2001
From: Jake <realrobotshk@gmail.com>
Date: Thu, 11 Sep 2025 01:42:41 +0800
Subject: [PATCH] first

---
 WritePos.ino | 163 +++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 163 insertions(+)
 create mode 100644 WritePos.ino

diff --git a/WritePos.ino b/WritePos.ino
new file mode 100644
index 0000000..7d3cbae
--- /dev/null
+++ b/WritePos.ino
@@ -0,0 +1,163 @@
+#include <SCServo.h>
+
+SMS_STS sms_sts;
+
+bool PRINT_SENT_PACKETS = false;
+#define DE_PIN 20
+#define RE_PIN 10
+#define TX_PIN 21
+#define RX_PIN 9
+
+
+#define PING 0x01            // QUERY THE WORKING STATUS
+#define READ_DATA 0x02       // READ DATA
+#define WRITE_DATA 0x03      // WRITE DATA
+#define REGWRITE_DATA 0x04   // QUEUES MOVES FOR ACTION COMMAND
+#define ACTION 0x05          // TRIGGERS REG WRITE WRITES
+#define SYNCWRITE_DATA 0x83  // SIMULTANEOUS CONTROL OF MULTIPLE SERVOS
+#define RESET 0x06           // RESET TO FACTORY DEFAULT
+
+void setup() {
+  Serial.begin(115200);
+  Serial1.begin(1000000, SERIAL_8N1, RX_PIN, TX_PIN);
+  pinMode(DE_PIN, OUTPUT);
+  pinMode(RE_PIN, OUTPUT);
+  SetModeReceive();
+  delay(1000);
+}
+
+// Send move command to servo id:0-255, position:0-4095
+void 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[6] = position & 0xFF;         // Low byte
+  packet[7] = (position >> 8) & 0xFF;  // High byte
+
+  // Calculate checksum
+  uint8_t sum = 0;
+  for (int i = 2; i < 8; i++) sum += packet[i];
+  packet[8] = ~sum;
+
+  // Send packet
+  SetModeTransmit();
+  Serial1.write(packet, sizeof(packet));
+  Serial1.flush();
+  SetModeReceive();
+
+  digitalWrite(DE_PIN, LOW);  // Back to receive
+  if (PRINT_SENT_PACKETS) {
+    Serial.print("Sent packet: ");
+
+    for (int i = 0; i < 9; i++) {
+      Serial.print("0x");
+      if (packet[i] < 0x10) Serial.print("0");  // Leading zero for single-digit hex
+      Serial.print(packet[i], HEX);
+      Serial.print(" ");
+    }
+    Serial.println();
+  }
+}
+
+// Send a ping command to a specific servo
+void sendPing(uint8_t id) {
+  uint8_t packet[6];
+
+  packet[0] = 0xFF;  // Header
+  packet[1] = 0xFF;  // Header
+  packet[2] = id;    // Servo ID
+  packet[3] = 0x02;  // Length = instruction + address + checksum
+  packet[4] = PING;  // Instruction: PING
+
+  // Calculate checksum
+  uint8_t sum = 0;
+  for (int i = 2; i < 5; i++) sum += packet[i];
+  packet[5] = ~sum;  // Checksum
+
+  // Send packet
+  SetModeTransmit();
+  Serial1.write(packet, sizeof(packet));
+  Serial1.flush();
+  SetModeReceive();
+
+  if (PRINT_SENT_PACKETS) {
+    Serial.print("Sent ping packet: ");
+    for (int i = 0; i < 6; i++) {
+      Serial.print("0x");
+      if (packet[i] < 0x10) Serial.print("0");  // Leading zero for single-digit hex
+      Serial.print(packet[i], HEX);
+      Serial.print(" ");
+    }
+    Serial.println();
+  }
+}
+
+void WaitOnReply(unsigned long timeout) {
+  unsigned long startTime = millis();  // Record the start time
+
+  while (millis() - startTime < timeout) {  // Loop until timeout
+    if (Serial1.available()) {
+      uint8_t buffer[32];
+      int count = 0;
+
+      // Read all available bytes
+      while (Serial1.available() && count < sizeof(buffer)) {
+        buffer[count++] = Serial1.read();
+      }
+
+      // 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();
+
+
+      break;  // Exit the loop after processing the reply
+    }
+    delay(10);  // Small delay to prevent busy-waiting
+  }
+}
+
+void SetModeTransmit() {
+  digitalWrite(DE_PIN, HIGH); 
+  digitalWrite(RE_PIN, HIGH);
+  delay(10);
+}
+
+void SetModeReceive() {
+  digitalWrite(DE_PIN, LOW);   
+  digitalWrite(RE_PIN, LOW);
+  delay(10);
+}
+
+void loop() {
+
+  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);
+}
\ No newline at end of file