full spectrum of data read/write (needs testing), feedback checkbox activates position streaming for the 5 test motors

HansonServo.ino

@@ -20,6 +20,9 @@ uint8_t payload[MAX_PAYLOAD_SIZE];  // Global or static
 #define CMD_SET_POSITION 0x07
 #define CMD_PLAY_FILE 0x08
 #define CMD_SCAN_CHANNEL 0x09
+#define CMD_WRITE_DATA 0x10
+#define CMD_START_POSITION_STREAM 0x14
+#define POSITION_STREAM 0x15
 
 
 // ESP32 S2 PINOUT
@@ -37,6 +40,9 @@ Animation anim;
 Animation sweep;
 Animation stagger;
 
+bool streamPositions = false;
+unsigned long lastStreamPositions = 0;
+
 Feetech* servos[2];
 
 uint16_t flipBytes(uint16_t value) {
@@ -137,7 +143,7 @@ void setup() {
 
 void SetID(uint8_t oldID, uint8_t newID) {
   Serial.println("Setting Lock to 0");
-  Serial.println(servos[0]->setLockSTS(oldID, 0));
+  Serial.println(servos[0]->setLock(oldID, 0));
   delay(1000);
   Serial.print("Changing ID ");
   Serial.print(oldID);
@@ -146,7 +152,7 @@ void SetID(uint8_t oldID, uint8_t newID) {
   Serial.println(servos[0]->setID(oldID, newID));
   delay(1000);
   Serial.println("Setting Lock to 1");
-  Serial.println(servos[0]->setLockSTS(newID, 1));
+  Serial.println(servos[0]->setLock(newID, 1));
   delay(1000);
 }
 
@@ -254,6 +260,14 @@ void handleCommand(uint8_t command, const uint8_t* payload, uint16_t length) {
       handleScanChannel(payload, length);
       break;
 
+    case CMD_WRITE_DATA:
+      handleDataWrite(payload, length);
+      break;
+
+    case CMD_START_POSITION_STREAM:
+      startPositionStream(payload, length);
+      break;
+
     default:
       Serial.print("Unknown command: ");
       Serial.println(command, HEX);
@@ -262,6 +276,40 @@ void handleCommand(uint8_t command, const uint8_t* payload, uint16_t length) {
 }
 
 
+void startPositionStream(const uint8_t* payload, uint16_t length) {
+  bool start = payload[0] != 0;
+  servos[0]->disableTorque(10);
+  servos[0]->waitOnData1Byte(10);
+  servos[0]->disableTorque(11);
+  servos[0]->waitOnData1Byte(10);
+  servos[0]->disableTorque(12);
+  servos[0]->waitOnData1Byte(10);
+  servos[0]->disableTorque(13);
+  servos[0]->waitOnData1Byte(10);
+  servos[0]->disableTorque(14);
+  servos[0]->waitOnData1Byte(10);
+  streamPositions = start;
+}
+
+
+void handleDataWrite(const uint8_t* payload, uint16_t length) {
+  uint8_t channel = payload[0];
+  uint8_t id = payload[1];
+  uint8_t writeByte = payload[2];
+
+  if (payload[4] == 1) {
+    servos[channel]->write1Byte(id, writeByte, payload[4]);
+    uint8_t response = servos[channel]->waitOnData1Byte(10);
+    sendMessage(&response, 1, CMD_WRITE_DATA);
+  } else {
+    servos[channel]->write2Bytes(id, writeByte, payload[4] | (payload[5] << 8));
+    uint16_t response = servos[channel]->waitOnData2Bytes(10);
+    uint8_t buffer[2];
+    buffer[0] = (response >> 8) & 0xFF;  // high byte
+    buffer[1] = response & 0xFF;         // low byte
+    sendMessage(buffer, 2, CMD_WRITE_DATA);
+  }
+}
 
 void handleIdRequest() {
   String payload = String(DEVICE_NAME) + "|" + FIRMWARE_VERSION;
@@ -589,15 +637,34 @@ void handleScanChannel(const uint8_t* payload, uint16_t length) {
     uint8_t val = servos[payload[0]]->waitOnData1Byte(10);
 
     if (val != 0) {
-      uint8_t response[4];
+      uint8_t response[44];      // Adjusted size to fit all values
       response[0] = payload[0];  // channel
       response[1] = i;           // responding address
-      uint16_t mode = servos[payload[0]]->getModel(i);
+
+      uint16_t model = servos[payload[0]]->getModel(i);
       uint16_t minAngleLimit = servos[payload[0]]->getMinAngleLimit(i);
       uint16_t maxAngleLimit = servos[payload[0]]->getMaxAngleLimit(i);
       uint16_t position = servos[payload[0]]->getPosition(i);
-      response[2] = (mode >> 8) & 0xFF;  // high byte
+      uint8_t CWDeadZone = servos[payload[0]]->getCWDeadZone(i);
-      response[3] = mode & 0xFF;         // low byte
+      uint8_t CCWDeadZone = servos[payload[0]]->getCCWDeadZone(i);
+      uint16_t offset = servos[payload[0]]->getOffset(i);
+      uint8_t mode = servos[payload[0]]->getMode(i);
+      uint8_t torqueEnable = servos[payload[0]]->getTorqueEnable(i);
+      uint8_t acceleration = servos[payload[0]]->getAcceleration(i);
+      uint16_t goalPosition = servos[payload[0]]->getGoalPosition(i);
+      uint16_t goalTime = servos[payload[0]]->getGoalTime(i);
+      uint16_t goalSpeed = servos[payload[0]]->getGoalSpeed(i);
+      uint8_t lock = servos[payload[0]]->getLock(i);
+      int16_t speed = servos[payload[0]]->getSpeed(i);
+      uint16_t load = servos[payload[0]]->getLoad(i);
+      uint8_t temperature = servos[payload[0]]->getTemperature(i);
+      uint8_t moving = servos[payload[0]]->getMoving(i);
+      uint8_t current = servos[payload[0]]->getCurrent(i);
+      uint8_t voltage = servos[payload[0]]->getVoltage(i);
+
+      // Pack values into response
+      response[2] = (model >> 8) & 0xFF;
+      response[3] = model & 0xFF;
 
       response[4] = (minAngleLimit >> 8) & 0xFF;
       response[5] = minAngleLimit & 0xFF;
@@ -608,8 +675,44 @@ void handleScanChannel(const uint8_t* payload, uint16_t length) {
       response[8] = (position >> 8) & 0xFF;
       response[9] = position & 0xFF;
 
+      response[10] = CWDeadZone;
+      response[11] = CCWDeadZone;
 
-      sendMessage(response, 10, CMD_SCAN_CHANNEL);  // send all 4 bytes
+      response[12] = (offset >> 8) & 0xFF;
+      response[13] = offset & 0xFF;
+
+      response[14] = mode;
+      response[15] = torqueEnable;
+      response[16] = acceleration;
+
+      response[17] = (goalPosition >> 8) & 0xFF;
+      response[18] = goalPosition & 0xFF;
+
+      response[19] = (goalTime >> 8) & 0xFF;
+      response[20] = goalTime & 0xFF;
+
+      response[21] = (goalSpeed >> 8) & 0xFF;
+      response[22] = goalSpeed & 0xFF;
+
+      response[23] = lock;
+
+      response[24] = (speed >> 8) & 0xFF;
+      response[25] = speed & 0xFF;
+
+      response[26] = (load >> 8) & 0xFF;
+      response[27] = load & 0xFF;
+
+      response[28] = temperature;
+      response[29] = moving;
+      response[30] = (current >> 8) & 0xFF;
+      response[31] = current & 0xFF;
+
+      response[32] = voltage;
+
+
+      // You can continue adding more fields here if needed...
+
+      sendMessage(response, 33, CMD_SCAN_CHANNEL);  // updated length
     }
   }
   uint8_t r[2];
@@ -638,6 +741,12 @@ bool flip = false;
 unsigned long lastSend = 0;
 void loop() {
 
+  // for (int i = 0; i < 1023; i++) {
+  //   servos[0]->sendWritePos(10, i);
+  //   Serial.println(servos[0]->waitOnData1Byte(10));
+  //   delay(20);
+  // }
+
 
   HandleSerialRequests();
   // for (int i = 0; i < 500; i+=32) {
@@ -694,9 +803,38 @@ void loop() {
     //PrintFileList();
     lastSend = millis();
   }
+
+  if (streamPositions) {
+    if (millis() - lastStreamPositions > 50) {
+      lastStreamPositions = millis();
+      SendMotorPositions();
+    }
+  }
 }
 
+void SendMotorPositions() {
 
+  uint16_t p0 = servos[0]->getPosition(10);
+  uint16_t p1 = servos[0]->getPosition(11);
+  uint16_t p2 = servos[0]->getPosition(12);
+  uint16_t p3 = servos[0]->getPosition(13);
+  uint16_t p4 = servos[0]->getPosition(14);
+
+  uint8_t payload[10];
+
+  payload[0] = p0 >> 8;
+  payload[1] = p0 & 0xFF;
+  payload[2] = p1 >> 8;
+  payload[3] = p1 & 0xFF;
+  payload[4] = p2 >> 8;
+  payload[5] = p2 & 0xFF;
+  payload[6] = p3 >> 8;
+  payload[7] = p3 & 0xFF;
+  payload[8] = p4 >> 8;
+  payload[9] = p4 & 0xFF;
+
+  sendMessage(payload, 10, POSITION_STREAM);
+}
 
 void PrintFileList() {
   File root = FFat.open("/");

feetech.cpp

@@ -308,24 +308,27 @@ uint16_t Feetech::getGoalSpeed(uint8_t id) {
 }
 
 uint8_t Feetech::getLock(uint8_t id) {
-  sendRequest(id, REQUEST_LOCK, 1);
+  if (feetechMode == MODE_SCS) {
+    sendRequest(id, REQUEST_LOCK_SCS, 1);
+  } else if (feetechMode == MODE_STS) {
+    sendRequest(id, REQUEST_LOCK_SMS_STS, 1);
+  }
   return waitOnData1Byte(10);
 }
 
 uint8_t Feetech::setLock(uint8_t id, uint8_t lockEnabled) {
-  write1Byte(id, REQUEST_LOCK, lockEnabled);
+  if (feetechMode == MODE_SCS) {
+    write1Byte(id, REQUEST_LOCK_SCS, lockEnabled);
+  } else if (feetechMode == MODE_STS) {
+    write1Byte(id, REQUEST_LOCK_SMS_STS, lockEnabled);
+  }
   return waitOnData1Byte(10);
 }
 
-uint8_t Feetech::setLockSTS(uint8_t id, uint8_t lockEnabled) {
+uint8_t Feetech::getVoltage(uint8_t id) {
-  write1Byte(id, 0x37, lockEnabled);
-  return waitOnData1Byte(10);
-}
-
-float Feetech::getVoltage(uint8_t id) {
   sendRequest(id, REQUEST_VOLTAGE, 1);
-  float voltage = waitOnData1Byte(10) * 0.1;
+  //float voltage = waitOnData1Byte(10) * 0.1;
-  return voltage;
+  return waitOnData1Byte(10);
 }
 
 uint16_t Feetech::getPosition(uint8_t id) {
@@ -364,9 +367,10 @@ uint8_t Feetech::getMoving(uint8_t id) {
 }
 
 // Multiplier could be wrong
-float Feetech::getCurrent(uint8_t id) {
+uint16_t Feetech::getCurrent(uint8_t id) {
   sendRequest(id, REQUEST_CURRENT_CURRENT, 2);
-  return waitOnData2Bytes(10) * 0.01;
+  //return waitOnData2Bytes(10) * 0.01; FLOAT
+  return waitOnData2Bytes(10);
 }
 
 void Feetech::sendRequest(uint8_t id, byte instruction, uint8_t byteCount) {
@@ -418,36 +422,33 @@ void Feetech::write1Byte(uint8_t id, byte instruction, uint8_t data) {
 void Feetech::write2Bytes(uint8_t id, byte instruction, uint16_t data) {
   uint8_t packet[9];
 
-  packet[0] = 0xFF;         // Header
+  packet[0] = 0xFF;        // Header
-  packet[1] = 0xFF;         // Header
+  packet[1] = 0xFF;        // Header
-  packet[2] = id;           // Servo ID
+  packet[2] = id;          // Servo ID
-  packet[3] = 5;            // Length = instruction + address + 2 bytes + checksum
+  packet[3] = 5;           // Length = instruction + address + 2 bytes + checksum
-  packet[4] = WRITE_DATA;   // Instruction: WRITE
+  packet[4] = WRITE_DATA;  // Instruction: WRITE
-  packet[5] = instruction;  // Address: Goal Position
+  packet[5] = instruction;        // Address: Goal Position
   if (feetechMode == MODE_SCS) {
     packet[6] = (data >> 8) & 0xFF;  // High byte
     packet[7] = data & 0xFF;         // Low byte
   } else if (feetechMode == MODE_STS) {
-
     packet[6] = data & 0xFF;         // Low byte first for STS
     packet[7] = (data >> 8) & 0xFF;  // High byte second
   }
-
   // Calculate checksum
   uint8_t sum = 0;
-  for (int i = 2; i <= 7; i++) {
+  for (int i = 2; i < 8; i++) sum += packet[i];
-    sum += packet[i];
-  }
   packet[8] = ~sum;
-  for (int i = 0; i < 9; i++) {
+
-    Serial.print(packet[i], HEX);
+  // Send packet
-    Serial.print(" ");
-  }
-  Serial.println();
   setModeTransmit();
-  serial.write(packet, 9);
+  serial.write(packet, sizeof(packet));
   serial.flush();
   setModeReceive();
+  clearEcho(sizeof(packet));
+
+  // Should later recieve a 1 byte response code packet
+  //return waitOnData2Bytes(10);
 }
 
 void Feetech::pingAll(std::vector<uint8_t>& successfulAddresses) {

feetech.h

@@ -40,7 +40,6 @@ public:
   uint16_t getGoalSpeed(uint8_t id);
   uint8_t getLock(uint8_t id);
   uint8_t setLock(uint8_t id, uint8_t lockEnabled);
-  uint8_t setLockSTS(uint8_t id, uint8_t lockEnabled);
   uint16_t getPosition(uint8_t id);
 
   int16_t getSpeed(uint8_t id);
@@ -49,9 +48,9 @@ public:
 
 
   uint8_t getMoving(uint8_t id);
-  float getCurrent(uint8_t id);
+  uint16_t getCurrent(uint8_t id);
 
-  float getVoltage(uint8_t id);
+  uint8_t getVoltage(uint8_t id);
   void sendRequest(uint8_t id, uint8_t instruction, uint8_t byteCount);
   uint8_t sendWritePos(uint8_t id, uint16_t position);
   void syncWritePos(uint8_t* ids, uint16_t* positions, uint8_t count);
@@ -90,6 +89,8 @@ public:
   static const byte REQUEST_MAX_ANGLE_LIMIT = 0x0B;  // 2 bytes
   static const byte REQUEST_CW_DEAD_ZONE = 0x1A;     // 1 byte
   static const byte REQUEST_CCW_DEAD_ZONE = 0x1B;    // 1 byte
+
+  // REQUEST OFFSET AND MODE NOT IN SCS?????
   static const byte REQUEST_OFFSET = 0x1F;           // 2 bytes
   static const byte REQUEST_MODE = 0x21;             // 1 byte
 
@@ -98,7 +99,9 @@ public:
   static const byte REQUEST_GOAL_POSITION = 0x2A;  // 2 byte
   static const byte REQUEST_GOAL_TIME = 0x2C;      // 2 byte
   static const byte REQUEST_GOAL_SPEED = 0x2E;     // 2 byte
-  static const byte REQUEST_LOCK = 0x30;           // 1 byte
+
+  static const byte REQUEST_LOCK_SCS = 0x30;           // 1 byte  
+  static const byte REQUEST_LOCK_SMS_STS = 0x37;           // 1 byte
 
   static const byte REQUEST_POSITION = 0x38;  // 2 bytes