From 5b468195dc16eb8c0b6fb7cb05b016ac473a7c1b Mon Sep 17 00:00:00 2001
From: Jake <realrobotshk@gmail.com>
Date: Wed, 17 Sep 2025 23:07:05 +0800
Subject: [PATCH] complete set of sts data reads implemented

---
 feetech.cpp          | 34 +++++++++++++++++++++++++++++++++-
 feetech.h            | 21 +++++++++++++++++++++
 rs485transmitter.ino | 38 ++++++++++++++++++++++++++------------
 3 files changed, 80 insertions(+), 13 deletions(-)

diff --git a/feetech.cpp b/feetech.cpp
index dd9d1dc..7471704 100644
--- a/feetech.cpp
+++ b/feetech.cpp
@@ -180,6 +180,38 @@ uint16_t Feetech::getPosition(uint8_t id) {
   return waitOnData2Bytes(10);
 }
 
+int16_t Feetech::getSpeed(uint8_t id) {
+  sendRequest(id, REQUEST_CURRENT_SPEED, 2);
+  int16_t val = waitOnData2Bytes(10);
+  if (val < 0){
+    val -= 32767;
+    val = -val;
+  }
+  return val;
+}
+
+// NOT SURE ABOUT THIS ONE from 0-1000 one direction, 0-2024 the other.
+uint16_t Feetech::getLoad(uint8_t id) {
+  sendRequest(id, REQUEST_CURRENT_LOAD, 2);
+  return waitOnData2Bytes(10);
+}
+
+uint8_t Feetech::getTemperature(uint8_t id) {
+  sendRequest(id, REQUEST_TEMPERATURE, 1);
+  return waitOnData1Byte(10);
+}
+
+uint8_t Feetech::getMoving(uint8_t id) {
+  sendRequest(id, REQUEST_MOVING, 1);
+  return waitOnData1Byte(10);
+}
+
+// Multiplier could be wrong
+float Feetech::getCurrent(uint8_t id) {
+  sendRequest(id, REQUEST_CURRENT_CURRENT, 2);
+  return waitOnData2Bytes(10) * 0.01;
+}
+
 void Feetech::sendRequest(uint8_t id, byte instruction, uint8_t byteCount) {
   uint8_t packet[8];
 
@@ -207,7 +239,7 @@ void Feetech::pingAll() {
   for (int i = 0; i < 255; i++) {
     sendPing(i);
 
-    waitOnReply(10);
+    waitOnReply(50);
   }
   Serial.println("PINGING COMPLETE");
 }
diff --git a/feetech.h b/feetech.h
index 797f3ca..4b19c2b 100644
--- a/feetech.h
+++ b/feetech.h
@@ -4,6 +4,10 @@
 #include <Arduino.h>
 #include <HardwareSerial.h>
 
+
+// SCS & HLS big-endian (high byte first)
+// STS little-endian    (low byte first)
+
 class Feetech {
 public:
   Feetech(HardwareSerial& serial, int DE_PIN, int RE_PIN, int TX_PIN, int RX_PIN);
@@ -25,6 +29,15 @@ public:
   uint16_t getGoalSpeed(uint8_t id);
   uint8_t getLock(uint8_t id);
   uint16_t getPosition(uint8_t id);
+
+  int16_t getSpeed(uint8_t id);
+  uint16_t getLoad(uint8_t id);
+  uint8_t getTemperature(uint8_t id);
+
+  
+  uint8_t getMoving(uint8_t id);
+  float getCurrent(uint8_t id);
+
   float getVoltage(uint8_t id);
   void sendRequest(uint8_t id, uint8_t instruction, uint8_t byteCount);
   void sendWritePos(uint8_t id, uint16_t position);
@@ -68,7 +81,15 @@ public:
   static const byte REQUEST_LOCK = 0x37;           // 1 byte
 
   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_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
 
   // TODO
   // SMS_STS_PRESENT_SPEED_L = 0x3A
diff --git a/rs485transmitter.ino b/rs485transmitter.ino
index 923ff45..b7a111e 100644
--- a/rs485transmitter.ino
+++ b/rs485transmitter.ino
@@ -293,19 +293,33 @@ uint8_t ids[] = { 1, 2 };  //, 10, 15, 40, 41, 43, 44};
 uint16_t pos[2];
 
 void loop() {
-  //PingAll();
-  //servos.sendWritePos(103, 4095);
-  for (int i = 0; i < 50; i++) {
-    Serial.println(servos.getAcceleration(103));
-    delay(5);
-  }
-  //servos.sendWritePos(103, 0);
-  for (int i = 0; i < 50; i++) {
-    Serial.println(servos.getAcceleration(103));
-    delay(5);
-  }
+  // Serial.println(servos.getModel(1));
+  uint16_t pos = servos.getModel(1);
+  uint16_t swapped = (pos >> 8) | (pos << 8);
+  Serial.println(swapped);
+  delay(1000);
   return;
-   Serial.println("getTorqueEnable");
+  servos.sendWritePos(1, 0);
+  for (int i = 0; i < 100; i++) {
+    uint16_t pos = servos.getPosition(1);
+    uint16_t swapped = (pos >> 8) | (pos << 8);
+    Serial.println(swapped);
+    delay(10);
+  }
+  delay(1000);
+  servos.sendWritePos(1, 1023);
+  for (int i = 0; i < 100; i++) {
+    uint16_t pos = servos.getPosition(1);
+    uint16_t swapped = (pos >> 8) | (pos << 8);
+    Serial.println(swapped);
+    delay(10);
+  }
+  delay(1000);
+  //servos.pingAll();
+  //servos.waitOnReply(50);
+  delay(1000);
+  return;
+  Serial.println("getTorqueEnable");
   Serial.println(servos.getTorqueEnable(103));
   delay(1000);
   Serial.println("getAcceleration");