complete set of sts data reads implemented

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");
 }

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,8 +81,16 @@ 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
   // SMS_STS_PRESENT_SPEED_H = 0x3B

rs485transmitter.ino

@@ -293,17 +293,31 @@ uint8_t ids[] = { 1, 2 };  //, 10, 15, 40, 41, 43, 44};
 uint16_t pos[2];
 
 void loop() {
-  //PingAll();
+  // Serial.println(servos.getModel(1));
-  //servos.sendWritePos(103, 4095);
+  uint16_t pos = servos.getModel(1);
-  for (int i = 0; i < 50; i++) {
+  uint16_t swapped = (pos >> 8) | (pos << 8);
-    Serial.println(servos.getAcceleration(103));
+  Serial.println(swapped);
-    delay(5);
+  delay(1000);
+  return;
+  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);
   }
-  //servos.sendWritePos(103, 0);
+  delay(1000);
-  for (int i = 0; i < 50; i++) {
+  servos.sendWritePos(1, 1023);
-    Serial.println(servos.getAcceleration(103));
+  for (int i = 0; i < 100; i++) {
-    delay(5);
+    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));