first

HansonServo.ino

@@ -0,0 +1,65 @@
+#include "feetech.h"
+// ESP32 S2 PINOUT
+#define RX_PIN 17  // DI
+#define TX_PIN 18  // RO
+#define DE_PIN 33  // Driver Enable
+#define RE_PIN 3   // Receiver Enable
+
+
+Feetech servos = Feetech(Serial1, DE_PIN, RE_PIN, TX_PIN, RX_PIN);
+
+uint16_t flipBytes(uint16_t value) {
+  return (value >> 8) | (value << 8);
+}
+
+uint8_t ids[] = { 1, 10, 11, 15 };
+uint16_t pos1[] = { 0, 0, 0, 0 };
+uint16_t pos2[] = { 1023, 1023, 1023, 4095 };
+
+
+void setup() {
+  Serial.begin(115200);
+  for (int i = 0; i < 10; i++) {
+    Serial.println(i);
+    delay(500);
+  }
+
+  pos2[3] = flipBytes(pos2[3]);
+
+  servos.begin();
+
+}
+
+void SetID(uint8_t oldID, uint8_t newID) {
+  Serial.println("Setting Lock to 0");
+  Serial.println(servos.setLock(oldID, 0));
+  delay(1000);
+  Serial.print("Changing ID ");
+  Serial.print(oldID);
+  Serial.print(" to ");
+  Serial.println(newID);
+  Serial.println(servos.setID(oldID, newID));
+  delay(1000);
+  Serial.println("Setting Lock to 1");
+  Serial.println(servos.setLock(newID, 1));
+  delay(1000);
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  PingAll();
+
+}
+
+void PingAll() {
+  std::vector<uint8_t> successfulAddresses;
+  servos.pingAll(successfulAddresses);
+
+  // Now successfulAddresses contains all successful pings
+  Serial.println("Successful Addresses:");
+  for (uint8_t address : successfulAddresses) {
+    Serial.print(address);
+    Serial.print(" ");
+  }
+  Serial.println();
+}

feetech.cpp

@@ -0,0 +1,546 @@
+#include "feetech.h"
+
+Feetech::Feetech(HardwareSerial& serial, int DE_PIN, int RE_PIN, int TX_PIN, int RX_PIN)
+  : serial(serial), DE_PIN(DE_PIN), RE_PIN(RE_PIN), TX_PIN(TX_PIN), RX_PIN(RX_PIN) {
+}
+
+void Feetech::begin() {
+  serial.begin(1000000, SERIAL_8N1, RX_PIN, TX_PIN);
+  pinMode(DE_PIN, OUTPUT);
+  //pinMode(RE_PIN, OUTPUT);
+  setModeReceive();
+}
+
+void Feetech::syncWritePos(uint8_t* ids, uint16_t* positions, uint8_t count) {
+  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++] = 0xFF;
+  packet[index++] = 0xFF;
+  packet[index++] = BROADCAST_ID;
+  packet[index++] = packetLen;
+  packet[index++] = SYNCWRITE_DATA;
+  packet[index++] = REQUEST_GOAL_POSITION;
+  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] >> 8) & 0xFF);  // High byte
+    packet[index++] = (uint8_t)(positions[i] & 0xFF);         // Low byte
+    packet[index++] = 0x00;                                   // Speed high byte
+    packet[index++] = 0x00;                                   // Speed low 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 move command to servo id:0-255, position:0-4095
+void Feetech::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[7] = (position >> 8) & 0xFF;  // High byte
+  packet[6] = position & 0xFF;         // Low byte
+
+  // Calculate checksum
+  uint8_t sum = 0;
+  for (int i = 2; i < 8; i++) sum += packet[i];
+  packet[8] = ~sum;
+
+  // Send packet
+  setModeTransmit();
+  serial.write(packet, sizeof(packet));
+  serial.flush();
+  setModeReceive();
+}
+
+void Feetech::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
+  //Serial.println("PING");
+  setModeTransmit();
+  //delay(20);
+  serial.write(packet, sizeof(packet));
+  serial.flush();
+  //delay(20);
+  setModeReceive();
+}
+
+void Feetech::testRequest() {
+  uint8_t packet[6] = { 0XFF, 0XFF, 0X00, 0X02, 0X06, 0XF7 };
+  uint8_t sum = 0;
+  for (int i = 2; i < 5; i++) sum += packet[i];  // Include all data bytes for checksum
+  packet[5] = ~sum;                              // Checksum
+
+  setModeTransmit();
+  serial.write(packet, sizeof(packet));
+  serial.flush();
+  setModeReceive();
+  waitOnReply(100);
+}
+
+void Feetech::enableTorque(uint8_t id) {
+  uint8_t packet[8];  // Adjust size based on your packet structure
+
+  packet[0] = 255;  // Header
+  packet[1] = 255;  // Header
+  packet[2] = id;   // Servo ID
+  packet[3] = 4;    // Length (instruction + parameters + checksum)
+  packet[4] = 3;    // Instruction to enable torque
+  packet[5] = 1;    // Parameter to enable torque
+
+  // Calculate checksum
+  uint8_t sum = 0;
+  for (int i = 2; i < 6; i++) {
+    sum += packet[i];
+  }
+  packet[6] = ~sum;  // Checksum (bitwise NOT)
+
+  setModeTransmit();
+  serial.write(packet, sizeof(packet));  // Send the packet
+  serial.flush();
+  //delay(10); // Short delay
+  setModeReceive();
+}
+
+uint16_t Feetech::getModel(uint8_t id) {
+  sendRequest(id, REQUEST_MODEL, 2);
+  return waitOnData2Bytes(10);
+}
+
+uint8_t Feetech::getID(uint8_t id) {
+  sendRequest(id, REQUEST_ID, 1);
+  return waitOnData1Byte(10);
+}
+
+uint8_t Feetech::setID(uint8_t id, uint8_t newId) {
+  write1Byte(id, REQUEST_ID, newId);
+  return waitOnData1Byte(10);
+}
+
+uint8_t Feetech::getBaudRate(uint8_t id) {
+  sendRequest(id, REQUEST_BAUD_RATE, 1);
+  return waitOnData1Byte(10);
+}
+
+uint16_t Feetech::getMinAngleLimit(uint8_t id) {
+  sendRequest(id, REQUEST_MIN_ANGLE_LIMIT, 2);
+  return waitOnData2Bytes(10);
+}
+
+uint16_t Feetech::getMaxAngleLimit(uint8_t id) {
+  sendRequest(id, REQUEST_MAX_ANGLE_LIMIT, 2);
+  return waitOnData2Bytes(10);
+}
+
+uint8_t Feetech::getCWDeadZone(uint8_t id) {
+  sendRequest(id, REQUEST_CW_DEAD_ZONE, 1);
+  return waitOnData1Byte(10);
+}
+
+uint8_t Feetech::getCCWDeadZone(uint8_t id) {
+  sendRequest(id, REQUEST_CCW_DEAD_ZONE, 1);
+  return waitOnData1Byte(10);
+}
+
+uint16_t Feetech::getOffset(uint8_t id) {
+  sendRequest(id, REQUEST_OFFSET, 2);
+  return waitOnData2Bytes(10);
+}
+
+uint8_t Feetech::getMode(uint8_t id) {
+  sendRequest(id, REQUEST_MODE, 1);
+  return waitOnData1Byte(10);
+}
+
+uint8_t Feetech::getTorqueEnable(uint8_t id) {
+  sendRequest(id, REQUEST_TORQUE_ENABLE, 1);
+  return waitOnData1Byte(10);
+}
+
+uint8_t Feetech::getAcceleration(uint8_t id) {
+  sendRequest(id, REQUEST_ACCELERATION, 1);
+  return waitOnData1Byte(10);
+}
+
+uint16_t Feetech::getGoalPosition(uint8_t id) {
+  sendRequest(id, REQUEST_GOAL_POSITION, 2);
+  return waitOnData2Bytes(10);
+}
+
+uint16_t Feetech::getGoalTime(uint8_t id) {
+  sendRequest(id, REQUEST_GOAL_TIME, 2);
+  return waitOnData2Bytes(10);
+}
+
+uint16_t Feetech::getGoalSpeed(uint8_t id) {
+  sendRequest(id, REQUEST_GOAL_TIME, 2);
+  return waitOnData2Bytes(10);
+}
+
+uint8_t Feetech::getLock(uint8_t id) {
+  sendRequest(id, REQUEST_LOCK, 1);
+  return waitOnData1Byte(10);
+}
+
+uint8_t Feetech::setLock(uint8_t id, uint8_t lockEnabled) {
+  write1Byte(id, REQUEST_LOCK, lockEnabled);
+  return waitOnData1Byte(10);
+}
+
+uint8_t Feetech::setLockSTS(uint8_t id, uint8_t lockEnabled) {
+  write1Byte(id, 0x37, lockEnabled);
+  return waitOnData1Byte(10);
+}
+
+float Feetech::getVoltage(uint8_t id) {
+  sendRequest(id, REQUEST_VOLTAGE, 1);
+  float voltage = waitOnData1Byte(10) * 0.1;
+  return voltage;
+}
+
+uint16_t Feetech::getPosition(uint8_t id) {
+  sendRequest(id, REQUEST_POSITION, 2);
+  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];
+
+  packet[0] = 0xFF;         // Header
+  packet[1] = 0xFF;         // Header
+  packet[2] = id;           // Servo ID (ensure this matches the expected ID, e.g., 0x02)
+  packet[3] = 0x04;         // Length (4 bytes of following data)
+  packet[4] = READ_DATA;    // Instruction
+  packet[5] = instruction;  // Write first address
+  packet[6] = byteCount;    // Number of bytes to read
+
+  // Calculate checksum
+  uint8_t sum = 0;
+  for (int i = 2; i < 7; i++) sum += packet[i];  // Include all data bytes for checksum
+  packet[7] = ~sum;                              // Checksum
+
+  setModeTransmit();
+  serial.write(packet, sizeof(packet));
+  serial.flush();
+  setModeReceive();
+}
+
+void Feetech::write1Byte(uint8_t id, byte instruction, uint8_t data) {
+  uint8_t packet[8];
+
+  packet[0] = 0xFF;         // Header
+  packet[1] = 0xFF;         // Header
+  packet[2] = id;           // Servo ID (ensure this matches the expected ID, e.g., 0x02)
+  packet[3] = 0x04;         // Length (4 bytes of following data)
+  packet[4] = WRITE_DATA;   // Instruction
+  packet[5] = instruction;  // Write first address
+  packet[6] = data;         // Data to write
+
+  // Calculate checksum
+  uint8_t sum = 0;
+  for (int i = 2; i < 7; i++) sum += packet[i];  // Include all data bytes for checksum
+  packet[7] = ~sum;                              // Checksum
+
+  setModeTransmit();
+  serial.write(packet, sizeof(packet));
+  serial.flush();
+  setModeReceive();
+}
+
+void Feetech::write2Bytes(uint8_t id, byte instruction, uint16_t data) {
+  uint8_t packet[8];
+
+  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] = instruction;         // Address: Goal Position
+  packet[7] = (data >> 8) & 0xFF;  // High byte
+  packet[6] = data & 0xFF;         // Low byte
+
+  // Calculate checksum
+  uint8_t sum = 0;
+  for (int i = 2; i < 8; i++) sum += packet[i];
+  packet[8] = ~sum;
+
+  setModeTransmit();
+  serial.write(packet, sizeof(packet));
+  serial.flush();
+  setModeReceive();
+}
+
+void Feetech::pingAll(std::vector<uint8_t>& successfulAddresses) {
+  Serial.println("PINGING ALL 0-255");
+  successfulAddresses.clear();  // Clear any previous results
+  for (int i = 0; i < 254; i++) {
+    //Serial.println(i);
+    sendPing(i);
+    uint8_t val = waitOnData1Byte(50);
+    if (val != 0) {
+      //Serial.println(val);
+      successfulAddresses.push_back(i);  // Store the successful address
+    }
+  }
+  Serial.println("PINGING COMPLETE");
+}
+
+void Feetech::waitOnReply(unsigned long timeout) {
+  unsigned long startTime = millis();  // Record the start time
+
+  while (millis() - startTime < timeout) {  // Loop until timeout
+    if (serial.available()) {
+      //Serial.println("RECV");
+      uint8_t buffer[32];
+      int count = 0;
+
+      // Read all available bytes
+      while (serial.available() && count < sizeof(buffer)) {
+        buffer[count++] = serial.read();
+      }
+
+      //Serial.println(count);
+
+      // 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
+  }
+}
+
+uint8_t Feetech::waitOnData1Byte(unsigned long timeout) {
+  unsigned long startTime = millis();  // Record the start time
+
+  while (millis() - startTime < timeout) {  // Loop until timeout
+    if (serial.available()) {
+      //Serial.println("RECV");
+      uint8_t buffer[32];
+      int count = 0;
+
+      // Read all available bytes
+      while (serial.available() && count < sizeof(buffer)) {
+        buffer[count++] = serial.read();
+      }
+
+      //Serial.println(count);
+      // if (count != 8) {
+      //   Serial.print("ERROR: Expected 8 byte reply, recieved ");
+      //   Serial.println(count);
+      //   return 0;
+      // } else {
+      // }
+
+      // 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();
+
+      uint8_t val = buffer[5];
+      return val;
+
+      break;  // Exit the loop after processing the reply
+    } else {
+      //Serial.println(millis() - startTime);
+    }
+    delay(1);  // Small delay to prevent busy-waiting
+  }
+  return 0;
+}
+
+uint16_t Feetech::waitOnData2Bytes(unsigned long timeout) {
+  unsigned long startTime = millis();  // Record the start time
+
+  while (millis() - startTime < timeout) {  // Loop until timeout
+    if (serial.available()) {
+      Serial.println("RECV");
+      uint8_t buffer[32];
+      int count = 0;
+
+      // Read all available bytes
+      while (serial.available() && count < sizeof(buffer)) {
+        buffer[count++] = serial.read();
+      }
+
+      Serial.println(count);
+      if (count != 8) {
+        Serial.print("ERROR: Expected 8 byte reply, recieved ");
+        Serial.println(count);
+        return 0;
+      } else {
+      }
+
+      // 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();
+
+      uint16_t val = (buffer[6] * 256) + buffer[5];
+      return val;
+
+      break;  // Exit the loop after processing the reply
+    }
+    delay(1);  // Small delay to prevent busy-waiting
+  }
+  return 0;
+}
+
+void Feetech::sendData(const byte* data, size_t length) {
+  // digitalWrite(_transmitPin, HIGH); // Enable transmit mode
+  // _serial.write(data, length);
+  // _serial.flush(); // Wait for transmission to complete
+  // digitalWrite(_transmitPin, LOW); // Disable transmit mode
+}
+
+size_t Feetech::receiveData(byte* buffer, size_t bufferSize) {
+  size_t bytesRead = 0;
+  while (serial.available() > 0 && bytesRead < bufferSize) {
+    buffer[bytesRead++] = serial.read();
+  }
+  return bytesRead;
+}
+
+void Feetech::setModeTransmit() {
+  digitalWrite(DE_PIN, HIGH);
+  digitalWrite(RE_PIN, HIGH);
+  delay(10);
+}
+
+void Feetech::setModeReceive() {
+  digitalWrite(DE_PIN, LOW);
+  digitalWrite(RE_PIN, LOW);
+  delay(10);
+}
+
+void Feetech::update() {
+  if (Serial.available()) {
+    setModeTransmit();
+    while (Serial.available()) {
+      char incomingByte = Serial.read();  // Read from USB Serial
+      serial.write(incomingByte);         // Send to Serial1
+    }
+    setModeReceive();
+  }
+
+  // Pass data from Serial1 (Feetech) to Serial (USB)
+  if (serial.available()) {
+    while (serial.available()) {
+      char incomingByte = serial.read();  // Read from Serial1
+      Serial.write(incomingByte);         // Send to USB Serial
+    }
+  }
+}

feetech.h

@@ -0,0 +1,121 @@
+#ifndef FEETECH_H
+#define FEETECH_H
+
+#include <vector>  
+#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);
+  void begin();
+  void sendPing(uint8_t id);
+  uint16_t getModel(uint8_t id);
+  uint8_t getID(uint8_t id);
+  uint8_t setID(uint8_t id, uint8_t newId);
+  uint8_t getBaudRate(uint8_t id);
+  uint16_t getMinAngleLimit(uint8_t id);
+  uint16_t getMaxAngleLimit(uint8_t id);
+  uint8_t getCWDeadZone(uint8_t id);
+  uint8_t getCCWDeadZone(uint8_t id);
+  uint16_t getOffset(uint8_t id);
+  uint8_t getMode(uint8_t id);
+  uint8_t getTorqueEnable(uint8_t id);
+  uint8_t getAcceleration(uint8_t id);
+  uint16_t getGoalPosition(uint8_t id);
+  uint16_t getGoalTime(uint8_t id);
+  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);
+  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);
+  void syncWritePos(uint8_t* ids, uint16_t* positions, uint8_t count);
+  void write1Byte(uint8_t id, byte instruction, uint8_t data);
+  void write2Bytes(uint8_t id, byte instruction, uint16_t data);
+  void pingAll(std::vector<uint8_t>& successfulAddresses);
+  void waitOnReply(unsigned long timeout);
+  uint8_t waitOnData1Byte(unsigned long timeout);
+  uint16_t waitOnData2Bytes(unsigned long timeout);
+  void sendData(const byte* data, size_t length);
+  size_t receiveData(byte* buffer, size_t bufferSize);
+  void setModeReceive();
+  void setModeTransmit();
+  void update();
+  void testRequest();
+  void enableTorque(uint8_t id);
+
+  static const byte PING = 0x01;            // QUERY THE WORKING STATUS
+  static const byte READ_DATA = 0x02;       // READ DATA
+  static const byte WRITE_DATA = 0x03;      // WRITE DATA
+  static const byte REGWRITE_DATA = 0x04;   // QUEUES MOVES FOR ACTION COMMAND
+  static const byte ACTION = 0x05;          // TRIGGERS REG WRITE WRITES
+  static const byte SYNCWRITE_DATA = 0x83;  // SIMULTANEOUS CONTROL OF MULTIPLE SERVOS
+  static const byte RESET = 0x06;           // RESET TO FACTORY DEFAULT
+  static const byte BROADCAST_ID = 0xFE;
+
+
+  // MEMORY TABLE LOCATIONS SMS-STS
+  static const byte REQUEST_MODEL = 0x03;            // 2 bytes
+  static const byte REQUEST_ID = 0x05;               // 1 byte
+  static const byte REQUEST_BAUD_RATE = 0x06;        // 1 byte
+  static const byte REQUEST_MIN_ANGLE_LIMIT = 0x09;  // 2 bytes
+  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
+  static const byte REQUEST_OFFSET = 0x1F;           // 2 bytes
+  static const byte REQUEST_MODE = 0x21;             // 1 byte
+
+  static const byte REQUEST_TORQUE_ENABLE = 0x28;  // 1 byte
+  static const byte REQUEST_ACCELERATION = 0x29;   // 1 byte
+  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_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
+
+
+  // BAUD RATES (stored as 1 byte)
+  static const byte SMS_STS_1M = 0;
+  static const byte SMS_STS_0_5M = 1;
+  static const byte SMS_STS_250K = 2;
+  static const byte SMS_STS_128K = 3;
+  static const byte SMS_STS_115200 = 4;
+  static const byte SMS_STS_76800 = 5;
+  static const byte SMS_STS_57600 = 6;
+  static const byte SMS_STS_38400 = 7;
+
+private:
+  HardwareSerial& serial;  // Reference to the HardwareSerial object
+  uint8_t DE_PIN = 20;
+  uint8_t RE_PIN = 10;
+  uint8_t TX_PIN = 21;
+  uint8_t RX_PIN = 9;
+};
+
+#endif  // FEETECH_H