moved everything to feetech.c

.vscode/arduino.json

@@ -0,0 +1,3 @@
+{
+    "port": "COM78"
+}

.vscode/tasks.json

@@ -0,0 +1,35 @@
+{
+  "version": "2.0.0",
+  "tasks": [
+    {
+      "label": "Arduino: Compile",
+      "type": "shell",
+      "command": "arduino-cli compile --fqbn esp32:esp32:esp32c3 --build-property build.usb_cdc_on_boot=1 --verbose .",
+      "group": "build",
+      "problemMatcher": []
+    },
+    {
+      "label": "Arduino: Upload",
+      "type": "shell",
+      "command": "arduino-cli upload -p COM78 --fqbn esp32:esp32:esp32c3 .",
+      "group": "test",
+      "problemMatcher": []
+    },
+    {
+      "label": "Arduino: Compile & Upload",
+      "type": "shell",
+      "command": "arduino-cli compile --fqbn esp32:esp32:esp32c3 --build-property build.usb_cdc_on_boot=1 --verbose .; if ($?) { arduino-cli upload -p COM78 --fqbn esp32:esp32:esp32c3 --verbose . }",
+      "group": {
+        "kind": "build",
+        "isDefault": true
+      },
+      "problemMatcher": []
+    },
+    {
+      "label": "Arduino: Set Board",
+      "type": "shell",
+      "command": "arduino-cli board attach --port COM78 --fqbn esp32:esp32:esp32c3",
+      "problemMatcher": []
+    }
+  ]
+}

feetech.cpp

@@ -0,0 +1,257 @@
+#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();
+}
+
+// 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
+  setModeTransmit();
+  serial.write(packet, sizeof(packet));
+  serial.flush();
+  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::getPosition(uint8_t id){
+  sendRequest(id, 0x38);
+  waitOnReply(10);
+  return 0;
+}
+
+void Feetech::sendRequest(uint8_t id, byte instruction) {
+  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] = 0x02;              // 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::pingAll() {
+  Serial.println("PINGING ALL 0-255");
+  for (int i = 0; i < 255; i++) {
+    sendPing(i);
+
+    waitOnReply(100);
+  }
+  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();
+      }
+
+      // 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 Feetech::waitOnReply(unsigned long timeout, uint8_t expectedReturnType) {
+  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();
+      }
+
+      // 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 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,44 @@
+#ifndef FEETECH_H
+#define FEETECH_H
+
+#include <Arduino.h>
+#include <HardwareSerial.h>
+
+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 getPosition(uint8_t id);
+  void sendRequest(uint8_t id, uint8_t instruction);
+  void sendWritePos(uint8_t id, uint16_t position);
+  void pingAll();
+  void waitOnReply(unsigned long timeout);
+  void waitOnReply(unsigned long timeout, uint8_t expectedReturnType);
+  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 REQUEST_POSITION = 0x38;   // Current Position request
+
+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

rs485transmitter.ino

@@ -1,10 +1,26 @@
+#include "feetech.h"
+
+
 
 bool PRINT_SENT_PACKETS = false;
+// S3 setup
+// #define RX_PIN 4  // DI
+// #define DE_PIN 5  // Driver Enable
+// #define RE_PIN 6  // Receiver Enable
+// #define TX_PIN 7  // RO
+
+
+// C3 setup
 #define DE_PIN 20
 #define RE_PIN 10
 #define TX_PIN 21
 #define RX_PIN 9
 
+Feetech servos = Feetech(Serial1, DE_PIN, RE_PIN, TX_PIN, RX_PIN);
+
+// SCS are BIG endian    (high byte first)
+// SMS are little endian (low byte first)
+
 
 #define PING 0x01            // QUERY THE WORKING STATUS
 #define READ_DATA 0x02       // READ DATA
@@ -16,11 +32,20 @@ bool PRINT_SENT_PACKETS = false;
 
 void setup() {
   Serial.begin(115200);
-  Serial1.begin(1000000, SERIAL_8N1, RX_PIN, TX_PIN);
+  servos.begin();
-  pinMode(DE_PIN, OUTPUT);
+
-  pinMode(RE_PIN, OUTPUT);
+
-  SetModeReceive();
+  // // while (true){
-  delay(1000);
+  // //   digitalWrite(TX_PIN, !digitalRead(TX_PIN));
+  // //   Serial.println(digitalRead(TX_PIN));
+  // //   delay(2000);
+  // // }
+
+  // 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
@@ -33,8 +58,8 @@ void sendWritePos(uint8_t id, uint16_t position) {
   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
+  packet[6] = position & 0xFF;         // Low byte
 
   // Calculate checksum
   uint8_t sum = 0;
@@ -44,71 +69,83 @@ void sendWritePos(uint8_t id, uint16_t position) {
   // Send packet
   SetModeTransmit();
   Serial1.write(packet, sizeof(packet));
-  Serial1.flush();
+  delay(10);
+  //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();
-  }
 }
 
-void syncWriteTwoServos(uint8_t id1, uint16_t pos1, uint8_t id2, uint16_t pos2) {
+
-  const uint8_t SYNC_WRITE = 0x83;
+void syncWriteTwoServos(uint16_t pos1, uint16_t pos2) {
+  Serial.println(pos1);
   const uint8_t START_BYTE = 0xFF;
-  const uint8_t DATA_LEN = 0x05; // 2 bytes for position + 1 byte for speed (optional)
+  const uint8_t BROADCAST_ID = 0xFE;
+  const uint8_t SYNC_WRITE = 0x83;
+  const uint8_t START_ADDR = 0x2A;
+  const uint8_t DATA_LEN_PER_SERVO = 6;  // pos(2) + time(2) + speed(2)
+  const uint8_t NUM_SERVOS = 2;
 
-  // Build packet
+  uint8_t packetLen = 4 + (NUM_SERVOS * (1 + DATA_LEN_PER_SERVO));  // instruction + addr + len + servo data
-  uint8_t packet[] = {
+  uint8_t packet[3 + packetLen];                                    // header + payload
-    START_BYTE, START_BYTE, 0xFE, // Broadcast ID
+  uint8_t index = 0;
-    0x0F,                         // Length = 15 bytes total
-    SYNC_WRITE,
-    0x2A,                         // Starting address for Goal Position (0x2A)
-    0x04,                         // Data length per servo (2 bytes pos + 2 bytes speed)
-    id1,
-    (uint8_t)(pos1 & 0xFF),       // Position low byte
-    (uint8_t)((pos1 >> 8) & 0xFF),// Position high byte
-    0x00, 0x00,                   // Speed (optional, set to 0)
-    id2,
-    (uint8_t)(pos2 & 0xFF),
-    (uint8_t)((pos2 >> 8) & 0xFF),
-    0x00, 0x00
-  };
 
-  // Calculate checksum
+  // Header
+  packet[index++] = START_BYTE;
+  packet[index++] = START_BYTE;
+  packet[index++] = BROADCAST_ID;
+  packet[index++] = packetLen;
+  packet[index++] = SYNC_WRITE;
+  packet[index++] = START_ADDR;
+  packet[index++] = DATA_LEN_PER_SERVO;
+
+  // Servo 1
+  packet[index++] = 0x01;                // ID
+  packet[index++] = (pos1 >> 8) & 0xFF;  // Position high
+  packet[index++] = pos1 & 0xFF;         // Position low
+  packet[index++] = 0x00;                // Time high
+  packet[index++] = 0x00;                // Time low
+  packet[index++] = 0x07;                // High byte
+  packet[index++] = 0xD0;                // Low byte 2000
+
+
+  // Servo 2
+  packet[index++] = 0x02;  // ID
+  packet[index++] = pos2 & 0xFF;
+  packet[index++] = (pos2 >> 8) & 0xFF;
+  packet[index++] = 0x00;
+  packet[index++] = 0x00;
+  packet[index++] = 0xE8;
+  packet[index++] = 0x03;
+
+  // Checksum
   uint8_t checksum = 0;
-  for (int i = 2; i < sizeof(packet); i++) {
+  for (uint8_t i = 2; i < index; i++) {
     checksum += packet[i];
   }
-  checksum = ~checksum;
+  checksum = ~checksum & 0xFF;
 
-// Send packet
+  // Send packet
   SetModeTransmit();
-  Serial1.write(packet, sizeof(packet));
+  Serial1.write(packet, index);
   Serial1.write(checksum);
   Serial1.flush();
+  delay(2);
   SetModeReceive();
 }
+
+
+
 void syncWriteServos(uint8_t* ids, uint16_t* positions, uint8_t count) {
   const uint8_t SYNC_WRITE = 0x83;
   const uint8_t START_BYTE = 0xFF;
   const uint8_t BROADCAST_ID = 0xFE;
-  const uint8_t START_ADDR = 0x2A;     // Goal Position address
+  const uint8_t START_ADDR = 0x2A;          // Goal Position address
-  const uint8_t DATA_LEN_PER_SERVO = 0x04; // 2 bytes pos + 2 bytes speed
+  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
+  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 packet[3 + packetLen];  // 3 header bytes + payload
   uint8_t index = 0;
 
   // Header
@@ -123,10 +160,10 @@ void syncWriteServos(uint8_t* ids, uint16_t* positions, uint8_t count) {
   // Servo data
   for (uint8_t i = 0; i < count; i++) {
     packet[index++] = ids[i];
-    packet[index++] = (uint8_t)(positions[i] & 0xFF);        // Low byte
+    packet[index++] = (uint8_t)((positions[i] >> 8) & 0xFF);  // High byte
-    packet[index++] = (uint8_t)((positions[i] >> 8) & 0xFF); // High byte
+    packet[index++] = (uint8_t)(positions[i] & 0xFF);         // Low byte
-    packet[index++] = 0x00; // Speed low byte
+    packet[index++] = 0x00;                                   // Speed high byte
-    packet[index++] = 0x00; // Speed high byte
+    packet[index++] = 0x00;                                   // Speed low byte
   }
 
   // Checksum
@@ -229,7 +266,7 @@ void SetModeReceive() {
 
 void PingAll() {
   Serial.println("PINGING ALL 0-255");
-  for (int i = 0; i < 255; i++) {
+  for (int i = 180; i < 255; i++) {
 
     sendPing(i);
 
@@ -252,30 +289,105 @@ void TestRange(uint8_t id) {
   }
 }
 
-uint8_t ids[] = { 0, 4, 10, 15, 40, 41, 43, 44};
+uint8_t ids[] = { 1, 2 };  //, 10, 15, 40, 41, 43, 44};
-uint16_t pos[8];
+uint16_t pos[2];
 
 void loop() {
   //PingAll();
-  for (int i = 0; i < 4; i += 1) {
+
-    for (int x = 0; x < 8; x++){
+  
-    pos[x] = i;
+  
+  if (true) {
+    for (int i = 0; i < 4096; i += 128) {
+      servos.sendWritePos(103, i);
+      servos.waitOnReply(10);
+      servos.getPosition(103);
+      Serial.println(i);
+      delay(2000);
     }
-    syncWriteServos(ids, pos, 8);
+    for (int i = 4095; i > 0; i -= 128) {
-    Serial.println(i);
+      servos.sendWritePos(103, i);
-    //syncWriteTwoServos(ids[0], pos[0], ids[1], pos[1]);
+      servos.waitOnReply(10);
-    delay(100);
+      servos.getPosition(103);
-  }
+      Serial.println(i);
-  for (int i = 4; i > 0; i -= 8) {
+      delay(2000);
-    for (int x = 0; x < 8; x++){
-    pos[x] = i;
     }
-    syncWriteServos(ids, pos, 2);
-    Serial.println(i);
-    //syncWriteTwoServos(ids[0], pos[0], ids[1], pos[1]);
-    delay(100);
   }
-  // TestRange(0);
+
+  //servos.testRequest();
+  //servos.update();
+  //servos.enableTorque(1);
+  //servos.enableTorque(2);
+  // servos.pingAll();
+  // delay(2000);
+  // for (int i = 0; i < 4096; i += 32) {
+  //   servos.sendWritePos(103, i);
+  //   servos.waitOnReply(10);
+  //   delay(100);
+  // }
+  return;
+  //servos.update();
+  // //servos.pingAll();
+
+  // //servos.sendWritePos(1, 100);
+  //delay(2000);
+  // servos.waitOnReply(10);
+  // delay(2000);
+  // servos.sendRequest(103, 0x02);
+  // servos.waitOnReply(10);
+  // delay(2000);
+  // for (int i = 0; i < 1024; i++){
+  //     servos.sendWritePos(103, i);
+  //     servos.waitOnReply(10);
+  //     Serial.println(i);
+  //     delay(50);
+  // }
+
+  // servos.waitOnReply(10);
+  // delay(2000);
+  // servos.sendWritePos(103, 500);
+  // servos.waitOnReply(10);
+  // delay(2000);
+  // servos.sendWritePos(103, 1000);
+  // //servos.sendRequest(103, 0x02);
+  // servos.waitOnReply(10);
+  // delay(2000);
+  return;
+
+  //WaitOnReply(100);
+  //PingAll();
+  //return;
+  // for (uint16_t pos = 0; pos <= 1024; pos += 16) {
+  //   syncWriteTwoServos(pos, 1024 - pos);
+  //   delay(500);
+  // }
+
+  // for (int i = 0; i < 1024; i += 8) {
+  //   for (int x = 0; x < 2; x++) {
+  //     pos[x] = i;
+  //   }
+  //   //sendWritePos(1, i);
+  //   sendWritePos(103, i);
+  //   //syncWriteServos(ids, pos, 2);
+  //   Serial.println(i);
+  //   //syncWriteTwoServos(ids[0], pos[0], ids[1], pos[1]);
+  //   delay(10);
+  // }
+
+  // for (int i = 1025; i > 0; i -= 8) {
+  //   for (int x = 0; x < 2; x++) {
+  //     pos[x] = i;
+  //   }
+  //   //sendWritePos(1, i);
+  //   sendWritePos(103, i);
+  //   //syncWriteServos(ids, pos, 2);
+  //   Serial.println(i);
+  //   //syncWriteTwoServos(ids[0], pos[0], ids[1], pos[1]);
+  //   delay(10);
+  // }
+
+
+  //TestRange(1);
   // TestRange(4);
   // TestRange(10);
   // TestRange(15);

sketch.yaml

@@ -0,0 +1,2 @@
+default_fqbn: arduino:avr:uno
+default_port: COM3