Dynamixel Communication Optimization

2024-05-15 21:56 UTC gpt-4o Open in ChatGPT ↗

You

Here’s a sender code from an arduino due.

#ifndef dynamixels_uno_h #define dynamixels_uno_h

const byte START_BYTE = 0x02; const byte END_BYTE = 0x03; const byte ESCAPE_BYTE = 0x7D; const byte XOR_MASK = 0x20; const unsigned long TRANSMIT_INTERVAL_MS = 250; // 4 FPS

unsigned long lastTransmitTime = 0; // For throttling the transmission

void update_remote_dynamixels(byte a1, byte a2, byte a3, byte a4, byte b1, byte b2, byte b3, byte b4) {

if (millis() - lastTransmitTime >= TRANSMIT_INTERVAL_MS) {

byte data[8] = { a1, a2, a3, a4, b1, b2, b3, b4 };

// Start byte
Serial2.write(START_BYTE);

// Send data with byte-stuffing
for (int i = 0; i < 8; i++) {
  if (data[i] == START_BYTE || data[i] == END_BYTE || data[i] == ESCAPE_BYTE) {
    Serial2.write(ESCAPE_BYTE);         // Escape character
    Serial2.write(data[i] ^ XOR_MASK);  // XOR with mask to 'escape'
  } else {
    Serial2.write(data[i]);
  }
}

// End byte
Serial2.write(END_BYTE);

// Update the last transmit time
lastTransmitTime = millis();

} }

#endif

and here’s a receiver code on the Uno

#include <SoftwareSerial.h> #include <DynamixelShield.h>

//This namespace is required to use Control table item names using namespace ControlTableItem;

// Shield object DynamixelShield dxl;

// MAX SPEED OF THE SERVOS const int MAX_SPEED = 40; //20 for quiet

//4096 = 1x full turn const int GOAL_THRESHOLD = 50;

// Position offset for Dynamixel 1 const long offset_1 = 2139;

// Position offset for Dynamixel 2 const long offset_2 = 1909;

// First Dynamixel (swivel) const uint8_t DXL_ID1 = 1; // Second Dynamixel (tilt) const uint8_t DXL_ID2 = 2; // Boilerplate const float DXL_PROTOCOL_VERSION = 2.0;

long dmx_01_goal = 0; long dmx_02_goal = 0;

// Get current Dynamixel position int pos(int DXL_ID) { return dxl.getPresentPosition(DXL_ID); }

// Check if the Dynamixel has almost reached the goal bool goal_reached(int DXL_ID, int goal) { return (abs(pos(DXL_ID) - goal) < GOAL_THRESHOLD); }

//============= END OF DYNAMIXEL SETUP ===================/

void dx_Init() { // Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate. dxl.begin(57600);

// Set Port Protocol Version. This has to match with DYNAMIXEL protocol version. dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);

// Get DYNAMIXEL information dxl.ping(DXL_ID1); dxl.ping(DXL_ID2);

// DXL1 (turn off torque when configuring items in EEPROM area) dxl.torqueOff(DXL_ID1); dxl.setOperatingMode(DXL_ID1, OP_EXTENDED_POSITION); // dxl.writeControlTableItem(MAX_POSITION_LIMIT, DXL_ID1, DXL_MAX); //1 for sexy // dxl.writeControlTableItem(MIN_POSITION_LIMIT, DXL_ID1, DXL_MIN); //1 for sexy dxl.writeControlTableItem(PROFILE_ACCELERATION, DXL_ID1, 1); //1 for sexy dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID1, MAX_SPEED); dxl.writeControlTableItem(MAX_VOLTAGE_LIMIT, DXL_ID1, 140); //14V dxl.writeControlTableItem(CURRENT_LIMIT, DXL_ID1, 100); //0.25A dxl.writeControlTableItem(VELOCITY_LIMIT, DXL_ID1, 70); //13.74 RPM dxl.writeControlTableItem(MOVING_THRESHOLD, DXL_ID1, 0); //13.74 RPM dxl.writeControlTableItem(TORQUE_LIMIT, DXL_ID1, 5); dxl.torqueOn(DXL_ID1);

// DXL2 (turn off torque when configuring items in EEPROM area) dxl.torqueOff(DXL_ID2); dxl.setOperatingMode(DXL_ID2, OP_EXTENDED_POSITION); // dxl.writeControlTableItem(MAX_POSITION_LIMIT, DXL_ID2, DXL_MAX); //1 for sexy // dxl.writeControlTableItem(MIN_POSITION_LIMIT, DXL_ID2, DXL_MIN); //1 for sexy dxl.writeControlTableItem(PROFILE_ACCELERATION, DXL_ID2, 1); //1 for sexy dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID2, MAX_SPEED); dxl.writeControlTableItem(MAX_VOLTAGE_LIMIT, DXL_ID2, 140); //14V dxl.writeControlTableItem(CURRENT_LIMIT, DXL_ID2, 100); //0.25A dxl.writeControlTableItem(VELOCITY_LIMIT, DXL_ID2, 70); //13.74 RPM dxl.writeControlTableItem(MOVING_THRESHOLD, DXL_ID2, 0); //13.74 RPM dxl.writeControlTableItem(TORQUE_LIMIT, DXL_ID2, 5); dxl.torqueOn(DXL_ID2);

delay(100);

//--------------------------- // DYNAMIXEL SET TO 0 Position //--------------------------- dxl.setGoalPosition(DXL_ID1, offset_1); dxl.setGoalPosition(DXL_ID2, offset_2); }

void move_dynamixel_1(uint32_t value) { dxl.setGoalPosition(DXL_ID1, value + offset_1); }

void move_dynamixel_2(uint32_t value) { dxl.setGoalPosition(DXL_ID2, value + offset_2); }

SoftwareSerial mySerial(9, 10); // RX, TX (TX not used)

void setup() { mySerial.begin(600); Serial.begin(9600);

dx_Init(); delay(2000);

// Serial.println(mySerial.available()); // Serial.println(“Hello UNO”); }

void loop() {

static boolean receiving = false; static byte incomingData[16]; // Large enough to handle escapes static int dataIndex = 0; bool escapeNext = false;

while (mySerial.available()) {

byte incomingByte = mySerial.read();

// Start of packet
if (!receiving && incomingByte == 0x02) {
  receiving = true;
  dataIndex = 0;
  escapeNext = false;
  continue;
}

// End of packet
if (receiving && incomingByte == 0x03 && !escapeNext) {
  receiving = false;
  processPacket(incomingData, dataIndex);

   // Send acknowledgment
   // mySerial.write('A');

  continue;
}

if (receiving) {
  if (escapeNext) {
    incomingByte ^= 0x20;  // Unescape the byte
    escapeNext = false;
  } else if (incomingByte == 0x7D) {
    escapeNext = true;
    continue;  // Skip further processing of this byte
  }

  if (dataIndex < sizeof(incomingData)) {
    incomingData[dataIndex++] = incomingByte;
  }
}

} }

int reconstructInt(uint8_t a, uint8_t b, uint8_t c, uint8_t d) { return (a << 24) | (b << 16) | (c << 8) | d; }

void processPacket(byte *data, int length) {

if (length < 8) return;  // Ensure there's enough data

int32_t a = reconstructInt(data[0], data[1], data[2], data[3]);
int32_t b = reconstructInt(data[4], data[5], data[6], data[7]);

// Convert and clamp values from -512 to 512 to -1.0 to 1.0 range
float ta = max(min(a / 512.0f, 1.0f), -1.0f);
float tb = max(min(b / 512.0f, 1.0f), -1.0f);

// Scale values to motor-specific ranges
a = (int)(ta * 10445);
b = (int)(tb * 12810);

move_dynamixel_1(a);
move_dynamixel_2(b);

// Debug output
// Serial.print("Dynamixel 1 Target: ");
// Serial.print(a);
// Serial.print(" | Dynamixel 2 Target: ");
// Serial.println(b);

}

What could go wrong ??

ChatGPT

Your code is generally well-structured and seems to have a good flow for the intended communication between the Arduino Due (sender) and the Arduino Uno (receiver) to control the Dynamixel servos. However, there are several potential issues and points for improvement that you should consider:

Potential Issues

1. Baud Rate Mismatch:

   • Ensure that the baud rates for the Serial2 on the Due and the SoftwareSerial on the Uno are set correctly. Currently, you have Serial2 (on Due) and mySerial (on Uno) set to different baud rates (Due is unspecified, default might be 9600, and Uno is set to 600).

2. Synchronization Issue:

   • If the start and end bytes are missed, the packet won’t be processed correctly. Consider adding a timeout mechanism to reset the receiving state if a packet takes too long to be completed.

3. Buffer Overflow:

   • You are using a static array incomingData[16] for incoming data. Although you are checking the size, ensure that no more than 8 meaningful bytes are expected after accounting for potential escapes.

4. Escape Byte Handling:

   • Ensure that the escape byte handling logic is correctly implemented and tested. Any mistake here can corrupt your data.

5. Timing Issue:

   • The sender sends data every 250 ms. Ensure that the Uno can process packets at this rate without missing any data due to other processing delays.

6. Process Packet Length Validation:

   • In processPacket, you are only checking if the length is less than 8. Since you are using byte stuffing, the actual packet length might vary. Ensure that the logic accommodates variable-length packets correctly.

Suggestions for Improvement

1. Initialization Debugging:

   • Add debugging messages to verify the initialization of both the sender and receiver. For instance, after setting up Serial2 on Due, print a message indicating it is ready.

2. Error Handling:

   • Add error handling for conditions like unexpected end bytes, missing start bytes, and buffer overflows.

3. Acknowledge Packets:

   • Implement an acknowledgment mechanism where the receiver sends an acknowledgment back to the sender. This can help in ensuring data integrity.

4. Code Optimization:

   • Optimize the byte stuffing mechanism by ensuring that the XOR and write operations are correctly and efficiently handled.

5. Comment and Documentation:

   • Add more comments to explain critical sections of the code, especially where data processing and communication occur.

Revised Code Example

Here’s a revised version of critical sections of your code considering the points above:

Sender Code (Arduino Due)

#ifndef dynamixels_uno_h
#define dynamixels_uno_h

const byte START_BYTE = 0x02;
const byte END_BYTE = 0x03;
const byte ESCAPE_BYTE = 0x7D;
const byte XOR_MASK = 0x20;
const unsigned long TRANSMIT_INTERVAL_MS = 250; // 4 FPS

unsigned long lastTransmitTime = 0;  // For throttling the transmission

void update_remote_dynamixels(byte a1, byte a2, byte a3, byte a4, byte b1, byte b2, byte b3, byte b4) {
  if (millis() - lastTransmitTime >= TRANSMIT_INTERVAL_MS) {
    byte data[8] = { a1, a2, a3, a4, b1, b2, b3, b4 };

    // Start byte
    Serial2.write(START_BYTE);

    // Send data with byte-stuffing
    for (int i = 0; i < 8; i++) {
      if (data[i] == START_BYTE || data[i] == END_BYTE || data[i] == ESCAPE_BYTE) {
        Serial2.write(ESCAPE_BYTE);         // Escape character
        Serial2.write(data[i] ^ XOR_MASK);  // XOR with mask to 'escape'
      } else {
        Serial2.write(data[i]);
      }
    }

    // End byte
    Serial2.write(END_BYTE);

    // Update the last transmit time
    lastTransmitTime = millis();
  }
}

#endif

Receiver Code (Arduino Uno)

#include <SoftwareSerial.h>
#include <DynamixelShield.h>

// This namespace is required to use Control table item names
using namespace ControlTableItem;

// Shield object
DynamixelShield dxl;

// MAX SPEED OF THE SERVOS
const int MAX_SPEED = 40;  //20 for quiet

//4096 = 1x full turn
const int GOAL_THRESHOLD = 50;

// Position offset for Dynamixel 1
const long offset_1 = 2139;

// Position offset for Dynamixel 2
const long offset_2 = 1909;

// First Dynamixel (swivel)
const uint8_t DXL_ID1 = 1;
// Second Dynamixel (tilt)
const uint8_t DXL_ID2 = 2;
// Boilerplate
const float DXL_PROTOCOL_VERSION = 2.0;

long dmx_01_goal = 0;
long dmx_02_goal = 0;

// Get current Dynamixel position
int pos(int DXL_ID) {
  return dxl.getPresentPosition(DXL_ID);
}

// Check if the Dynamixel has almost reached the goal
bool goal_reached(int DXL_ID, int goal) {
  return (abs(pos(DXL_ID) - goal) < GOAL_THRESHOLD);
}

//============= END OF DYNAMIXEL SETUP ===================/

void dx_Init() {
  // Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
  dxl.begin(57600);

  // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);

  // Get DYNAMIXEL information
  dxl.ping(DXL_ID1);
  dxl.ping(DXL_ID2);

  // DXL1 (turn off torque when configuring items in EEPROM area)
  dxl.torqueOff(DXL_ID1);
  dxl.setOperatingMode(DXL_ID1, OP_EXTENDED_POSITION);
  //  dxl.writeControlTableItem(MAX_POSITION_LIMIT, DXL_ID1, DXL_MAX);  //1 for sexy
  //  dxl.writeControlTableItem(MIN_POSITION_LIMIT, DXL_ID1, DXL_MIN);  //1 for sexy
  dxl.writeControlTableItem(PROFILE_ACCELERATION, DXL_ID1, 1);  //1 for sexy
  dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID1, MAX_SPEED);
  dxl.writeControlTableItem(MAX_VOLTAGE_LIMIT, DXL_ID1, 140);  //14V
  dxl.writeControlTableItem(CURRENT_LIMIT, DXL_ID1, 100);      //0.25A
  dxl.writeControlTableItem(VELOCITY_LIMIT, DXL_ID1, 70);      //13.74 RPM
  dxl.writeControlTableItem(MOVING_THRESHOLD, DXL_ID1, 0);     //13.74 RPM
  dxl.writeControlTableItem(TORQUE_LIMIT, DXL_ID1, 5);
  dxl.torqueOn(DXL_ID1);

  // DXL2 (turn off torque when configuring items in EEPROM area)
  dxl.torqueOff(DXL_ID2);
  dxl.setOperatingMode(DXL_ID2, OP_EXTENDED_POSITION);
  //  dxl.writeControlTableItem(MAX_POSITION_LIMIT, DXL_ID2, DXL_MAX);  //1 for sexy
  //  dxl.writeControlTableItem(MIN_POSITION_LIMIT, DXL_ID2, DXL_MIN);  //1 for sexy
  dxl.writeControlTableItem(PROFILE_ACCELERATION, DXL_ID2, 1);  //1 for sexy
  dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID2, MAX_SPEED);
  dxl.writeControlTableItem(MAX_VOLTAGE_LIMIT, DXL_ID2, 140);  //14V
  dxl.writeControlTableItem(CURRENT_LIMIT, DXL_ID2, 100);      //0.25A
  dxl.writeControlTableItem(VELOCITY_LIMIT, DXL_ID2, 70);      //13.74 RPM
  dxl.writeControlTableItem(MOVING_THRESHOLD, DXL_ID2, 0);     //13.74 RPM
  dxl.writeControlTableItem(TORQUE_LIMIT, DXL_ID2, 5);
  dxl.torqueOn(DXL_ID2);

  delay(100);

  //---------------------------
  // DYNAMIXEL SET TO 0 Position
  //---------------------------
  dxl.setGoalPosition(DXL_ID1, offset_1);
  dxl.setGoalPosition(DXL_ID2, offset_2);
}

void move_dynamixel_1(uint32_t value) {
  dxl.setGoalPosition(DXL_ID1, value + offset_1);
}

void move_dynamixel_2(uint32_t value) {
  dxl.setGoalPosition(DXL_ID2, value + offset_2);
}

SoftwareSerial mySerial(9, 10);  // RX, TX (TX not used)

void setup() {
  mySerial.begin(9600); // Ensure this matches the baud rate of Serial2 on the Due
  Serial.begin(9600);

  dx_Init();
  delay(2000);

  // Serial.println(mySerial.available());
  // Serial.println("Hello UNO");
}

void loop() {
  static boolean receiving = false;
  static byte incomingData[16];  // Large enough to handle escapes
  static int dataIndex = 0;
  bool escapeNext = false;

  while (mySerial.available()) {
    byte incomingByte = mySerial.read();

    // Start of packet
    if (!receiving && incomingByte == 0x02) {
      receiving = true;
      dataIndex = 0;
      escapeNext = false;
      continue;
    }

    // End of packet
    if (receiving && incomingByte == 0x03 && !escapeNext) {
      receiving = false;
      processPacket(incomingData, dataIndex);
      continue;
    }

    if (receiving) {
      if (escapeNext) {
        incomingByte ^= 0x20;  // Unescape the byte
        escapeNext = false;
      } else if (incomingByte == 0x7D) {
        escapeNext = true;
        continue;  // Skip further processing of this byte
      }

      if (dataIndex < sizeof(incomingData)) {
        incomingData[dataIndex++] = incomingByte;
      } else {
        // Buffer overflow, reset receiving
        receiving = false;
        dataIndex = 0;
      }
    }
  }
}

int reconstructInt(uint8_t a, uint8_t b, uint8_t c, uint8_t d) {
  return (a << 24) | (b << 16) | (c << 8) | d;
}

void processPacket(byte *data, int length) {
  if (length < 8) return;  // Ensure there's enough data

  int32_t a = reconstructInt(data[0], data[1], data[2], data[3]);
  int32_t b = reconstructInt(data[4], data[5], data[6], data[7]);

  // Convert and clamp values from -512 to 512 to -1.0 to 1.0 range
  float ta = max(min(a / 512.0f, 1.0f), -1.0f);
  float tb = max(min(b / 512.0f, 1.0f), -1.0f);

  // Scale values to motor-specific ranges
  a = (int)(ta * 10445);
  b = (int)(tb * 12810);

  move_dynamixel_1(a);
  move_dynamixel_2(b);

  // Debug output
  // Serial.print("Dynamixel 1 Target: ");
  // Serial.print(a);
  // Serial.print(" | Dynamixel 2 Target: ");
  // Serial.println(b);
}

Additional Notes

• Ensure both Serial2 on Due and mySerial on Uno are set to the same baud rate. I used 9600 as an example.
• Consider adding more robust error checking and handling mechanisms, especially for communication and synchronization.
• Test the communication in a controlled environment to ensure that the data packets are being sent and received correctly.

You

Show a full example of Synchronization logic

ChatGPT

Implementing synchronization logic ensures that the receiver correctly identifies the start and end of each packet, handles timeouts, and manages errors gracefully. Here’s a full example of sender and receiver codes with added synchronization logic.

Sender Code (Arduino Due)

This code sends packets every 250 milliseconds with proper synchronization logic.

#ifndef dynamixels_uno_h
#define dynamixels_uno_h

const byte START_BYTE = 0x02;
const byte END_BYTE = 0x03;
const byte ESCAPE_BYTE = 0x7D;
const byte XOR_MASK = 0x20;
const unsigned long TRANSMIT_INTERVAL_MS = 250; // 4 FPS

unsigned long lastTransmitTime = 0;  // For throttling the transmission

void setup() {
  Serial2.begin(9600);
  // Optional: Add a debug Serial connection
  Serial.begin(9600);
}

void loop() {
  // Dummy data to send
  byte a1 = 0x10, a2 = 0x20, a3 = 0x30, a4 = 0x40;
  byte b1 = 0x50, b2 = 0x60, b3 = 0x70, b4 = 0x80;

  update_remote_dynamixels(a1, a2, a3, a4, b1, b2, b3, b4);
}

void update_remote_dynamixels(byte a1, byte a2, byte a3, byte a4, byte b1, byte b2, byte b3, byte b4) {
  if (millis() - lastTransmitTime >= TRANSMIT_INTERVAL_MS) {
    byte data[8] = { a1, a2, a3, a4, b1, b2, b3, b4 };

    // Start byte
    Serial2.write(START_BYTE);

    // Send data with byte-stuffing
    for (int i = 0; i < 8; i++) {
      if (data[i] == START_BYTE || data[i] == END_BYTE || data[i] == ESCAPE_BYTE) {
        Serial2.write(ESCAPE_BYTE);         // Escape character
        Serial2.write(data[i] ^ XOR_MASK);  // XOR with mask to 'escape'
      } else {
        Serial2.write(data[i]);
      }
    }

    // End byte
    Serial2.write(END_BYTE);

    // Update the last transmit time
    lastTransmitTime = millis();
  }
}

#endif

Receiver Code (Arduino Uno)

This code receives packets with synchronization logic, handles potential errors, and processes the data.

#include <SoftwareSerial.h>
#include <DynamixelShield.h>

// This namespace is required to use Control table item names
using namespace ControlTableItem;

// Shield object
DynamixelShield dxl;

// MAX SPEED OF THE SERVOS
const int MAX_SPEED = 40;  //20 for quiet

//4096 = 1x full turn
const int GOAL_THRESHOLD = 50;

// Position offset for Dynamixel 1
const long offset_1 = 2139;

// Position offset for Dynamixel 2
const long offset_2 = 1909;

// First Dynamixel (swivel)
const uint8_t DXL_ID1 = 1;
// Second Dynamixel (tilt)
const uint8_t DXL_ID2 = 2;
// Boilerplate
const float DXL_PROTOCOL_VERSION = 2.0;

long dmx_01_goal = 0;
long dmx_02_goal = 0;

// Get current Dynamixel position
int pos(int DXL_ID) {
  return dxl.getPresentPosition(DXL_ID);
}

// Check if the Dynamixel has almost reached the goal
bool goal_reached(int DXL_ID, int goal) {
  return (abs(pos(DXL_ID) - goal) < GOAL_THRESHOLD);
}

//============= END OF DYNAMIXEL SETUP ===================/

void dx_Init() {
  // Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
  dxl.begin(57600);

  // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);

  // Get DYNAMIXEL information
  dxl.ping(DXL_ID1);
  dxl.ping(DXL_ID2);

  // DXL1 (turn off torque when configuring items in EEPROM area)
  dxl.torqueOff(DXL_ID1);
  dxl.setOperatingMode(DXL_ID1, OP_EXTENDED_POSITION);
  //  dxl.writeControlTableItem(MAX_POSITION_LIMIT, DXL_ID1, DXL_MAX);  //1 for sexy
  //  dxl.writeControlTableItem(MIN_POSITION_LIMIT, DXL_ID1, DXL_MIN);  //1 for sexy
  dxl.writeControlTableItem(PROFILE_ACCELERATION, DXL_ID1, 1);  //1 for sexy
  dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID1, MAX_SPEED);
  dxl.writeControlTableItem(MAX_VOLTAGE_LIMIT, DXL_ID1, 140);  //14V
  dxl.writeControlTableItem(CURRENT_LIMIT, DXL_ID1, 100);      //0.25A
  dxl.writeControlTableItem(VELOCITY_LIMIT, DXL_ID1, 70);      //13.74 RPM
  dxl.writeControlTableItem(MOVING_THRESHOLD, DXL_ID1, 0);     //13.74 RPM
  dxl.writeControlTableItem(TORQUE_LIMIT, DXL_ID1, 5);
  dxl.torqueOn(DXL_ID1);

  // DXL2 (turn off torque when configuring items in EEPROM area)
  dxl.torqueOff(DXL_ID2);
  dxl.setOperatingMode(DXL_ID2, OP_EXTENDED_POSITION);
  //  dxl.writeControlTableItem(MAX_POSITION_LIMIT, DXL_ID2, DXL_MAX);  //1 for sexy
  //  dxl.writeControlTableItem(MIN_POSITION_LIMIT, DXL_ID2, DXL_MIN);  //1 for sexy
  dxl.writeControlTableItem(PROFILE_ACCELERATION, DXL_ID2, 1);  //1 for sexy
  dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID2, MAX_SPEED);
  dxl.writeControlTableItem(MAX_VOLTAGE_LIMIT, DXL_ID2, 140);  //14V
  dxl.writeControlTableItem(CURRENT_LIMIT, DXL_ID2, 100);      //0.25A
  dxl.writeControlTableItem(VELOCITY_LIMIT, DXL_ID2, 70);      //13.74 RPM
  dxl.writeControlTableItem(MOVING_THRESHOLD, DXL_ID2, 0);     //13.74 RPM
  dxl.writeControlTableItem(TORQUE_LIMIT, DXL_ID2, 5);
  dxl.torqueOn(DXL_ID2);

  delay(100);

  //---------------------------
  // DYNAMIXEL SET TO 0 Position
  //---------------------------
  dxl.setGoalPosition(DXL_ID1, offset_1);
  dxl.setGoalPosition(DXL_ID2, offset_2);
}

void move_dynamixel_1(uint32_t value) {
  dxl.setGoalPosition(DXL_ID1, value + offset_1);
}

void move_dynamixel_2(uint32_t value) {
  dxl.setGoalPosition(DXL_ID2, value + offset_2);
}

SoftwareSerial mySerial(9, 10);  // RX, TX (TX not used)

const unsigned long TIMEOUT_MS = 500;  // Maximum time to wait for a packet

void setup() {
  mySerial.begin(9600); // Ensure this matches the baud rate of Serial2 on the Due
  Serial.begin(9600);

  dx_Init();
  delay(2000);

  // Serial.println(mySerial.available());
  // Serial.println("Hello UNO");
}

void loop() {
  static boolean receiving = false;
  static byte incomingData[16];  // Large enough to handle escapes
  static int dataIndex = 0;
  static unsigned long lastByteTime = 0;  // Time of the last received byte
  bool escapeNext = false;

  while (mySerial.available()) {
    byte incomingByte = mySerial.read();
    lastByteTime = millis();  // Update the last byte received time

    // Start of packet
    if (!receiving && incomingByte == 0x02) {
      receiving = true;
      dataIndex = 0;
      escapeNext = false;
      continue;
    }

    // End of packet
    if (receiving && incomingByte == 0x03 && !escapeNext) {
      receiving = false;
      processPacket(incomingData, dataIndex);
      continue;
    }

    if (receiving) {
      if (escapeNext) {
        incomingByte ^= 0x20;  // Unescape the byte
        escapeNext = false;
      } else if (incomingByte == 0x7D) {
        escapeNext = true;
        continue;  // Skip further processing of this byte
      }

      if (dataIndex < sizeof(incomingData)) {
        incomingData[dataIndex++] = incomingByte;
      } else {
        // Buffer overflow, reset receiving
        receiving = false;
        dataIndex = 0;
      }
    }
  }

  // Check for timeout
  if (receiving && (millis() - lastByteTime > TIMEOUT_MS)) {
    // Timeout occurred, reset receiving state
    receiving = false;
    dataIndex = 0;
  }
}

int reconstructInt(uint8_t a, uint8_t b, uint8_t c, uint8_t d) {
  return (a << 24) | (b << 16) | (c << 8) | d;
}

void processPacket(byte *data, int length) {
  if (length < 8) return;  // Ensure there's enough data

  int32_t a = reconstructInt(data[0], data[1], data[2], data[3]);
  int32_t b = reconstructInt(data[4], data[5], data[6], data[7]);

  // Convert and clamp values from -512 to 512 to -1.0 to 1.0 range
  float ta = max(min(a / 512.0f, 1.0f), -1.0f);
  float tb = max(min(b / 512.0f, 1.0f), -1.0f);

  // Scale values to motor-specific ranges
  a = (int)(ta * 10445);
  b = (int)(tb * 12810);

  move_dynamixel_1(a);
  move_dynamixel_2(b);

  // Debug output
  // Serial.print("Dynamixel 1 Target: ");
  // Serial.print(a);
  // Serial.print(" | Dynamixel 2 Target: ");
  // Serial.println(b);
}

Explanation of Synchronization Logic

1. Timeout Handling:

   • The lastByteTime variable keeps track of the last time a byte was received.
   • If the time between bytes exceeds TIMEOUT_MS, the receiver state is reset.

2. Start and End Bytes:

   • The start byte (0x02) and end byte (0x03) are used to mark the beginning and end of a packet.
   • If the end byte is detected, the packet is processed.
   • If the start byte is detected, the receiving state is initialized.

3. Escape Byte Handling:

   • The ESCAPE_BYTE (0x7D) is used to escape special bytes (START_BYTE, END_BYTE, and ESCAPE_BYTE).
   • The subsequent byte is XORed with XOR_MASK to restore the original byte.

4. Buffer Overflow Protection:

   • If the incoming data exceeds the buffer size (incomingData), the receiving state is reset to prevent overflow.

This example includes synchronization logic to handle the start and end of packets, manage timeouts, and avoid buffer overflows. Ensure that both the sender and receiver use the same baud rate and are correctly synchronized.

You

why is incoming data 16? I only expect 8 values

ChatGPT

You are correct that you only expect 8 values in your packet data. The reason for using a buffer size of 16 is to account for the potential increase in data size due to byte-stuffing (escaping special characters). Each byte in the original data can potentially be replaced by two bytes (an escape byte followed by the escaped byte), which can double the data size in the worst-case scenario.

However, if you are confident that 8 bytes are enough and you have additional logic to handle buffer overflow, you can reduce the buffer size. I’ll show you how to handle this appropriately with an 8-byte buffer while maintaining the synchronization logic.

Updated Receiver Code (Arduino Uno)

#include <SoftwareSerial.h>
#include <DynamixelShield.h>

// This namespace is required to use Control table item names
using namespace ControlTableItem;

// Shield object
DynamixelShield dxl;

// MAX SPEED OF THE SERVOS
const int MAX_SPEED = 40;  //20 for quiet

//4096 = 1x full turn
const int GOAL_THRESHOLD = 50;

// Position offset for Dynamixel 1
const long offset_1 = 2139;

// Position offset for Dynamixel 2
const long offset_2 = 1909;

// First Dynamixel (swivel)
const uint8_t DXL_ID1 = 1;
// Second Dynamixel (tilt)
const uint8_t DXL_ID2 = 2;
// Boilerplate
const float DXL_PROTOCOL_VERSION = 2.0;

long dmx_01_goal = 0;
long dmx_02_goal = 0;

// Get current Dynamixel position
int pos(int DXL_ID) {
  return dxl.getPresentPosition(DXL_ID);
}

// Check if the Dynamixel has almost reached the goal
bool goal_reached(int DXL_ID, int goal) {
  return (abs(pos(DXL_ID) - goal) < GOAL_THRESHOLD);
}

//============= END OF DYNAMIXEL SETUP ===================/

void dx_Init() {
  // Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
  dxl.begin(57600);

  // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);

  // Get DYNAMIXEL information
  dxl.ping(DXL_ID1);
  dxl.ping(DXL_ID2);

  // DXL1 (turn off torque when configuring items in EEPROM area)
  dxl.torqueOff(DXL_ID1);
  dxl.setOperatingMode(DXL_ID1, OP_EXTENDED_POSITION);
  //  dxl.writeControlTableItem(MAX_POSITION_LIMIT, DXL_ID1, DXL_MAX);  //1 for sexy
  //  dxl.writeControlTableItem(MIN_POSITION_LIMIT, DXL_ID1, DXL_MIN);  //1 for sexy
  dxl.writeControlTableItem(PROFILE_ACCELERATION, DXL_ID1, 1);  //1 for sexy
  dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID1, MAX_SPEED);
  dxl.writeControlTableItem(MAX_VOLTAGE_LIMIT, DXL_ID1, 140);  //14V
  dxl.writeControlTableItem(CURRENT_LIMIT, DXL_ID1, 100);      //0.25A
  dxl.writeControlTableItem(VELOCITY_LIMIT, DXL_ID1, 70);      //13.74 RPM
  dxl.writeControlTableItem(MOVING_THRESHOLD, DXL_ID1, 0);     //13.74 RPM
  dxl.writeControlTableItem(TORQUE_LIMIT, DXL_ID1, 5);
  dxl.torqueOn(DXL_ID1);

  // DXL2 (turn off torque when configuring items in EEPROM area)
  dxl.torqueOff(DXL_ID2);
  dxl.setOperatingMode(DXL_ID2, OP_EXTENDED_POSITION);
  //  dxl.writeControlTableItem(MAX_POSITION_LIMIT, DXL_ID2, DXL_MAX);  //1 for sexy
  //  dxl.writeControlTableItem(MIN_POSITION_LIMIT, DXL_ID2, DXL_MIN);  //1 for sexy
  dxl.writeControlTableItem(PROFILE_ACCELERATION, DXL_ID2, 1);  //1 for sexy
  dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID2, MAX_SPEED);
  dxl.writeControlTableItem(MAX_VOLTAGE_LIMIT, DXL_ID2, 140);  //14V
  dxl.writeControlTableItem(CURRENT_LIMIT, DXL_ID2, 100);      //0.25A
  dxl.writeControlTableItem(VELOCITY_LIMIT, DXL_ID2, 70);      //13.74 RPM
  dxl.writeControlTableItem(MOVING_THRESHOLD, DXL_ID2, 0);     //13.74 RPM
  dxl.writeControlTableItem(TORQUE_LIMIT, DXL_ID2, 5);
  dxl.torqueOn(DXL_ID2);

  delay(100);

  //---------------------------
  // DYNAMIXEL SET TO 0 Position
  //---------------------------
  dxl.setGoalPosition(DXL_ID1, offset_1);
  dxl.setGoalPosition(DXL_ID2, offset_2);
}

void move_dynamixel_1(uint32_t value) {
  dxl.setGoalPosition(DXL_ID1, value + offset_1);
}

void move_dynamixel_2(uint32_t value) {
  dxl.setGoalPosition(DXL_ID2, value + offset_2);
}

SoftwareSerial mySerial(9, 10);  // RX, TX (TX not used)

const unsigned long TIMEOUT_MS = 500;  // Maximum time to wait for a packet

void setup() {
  mySerial.begin(9600); // Ensure this matches the baud rate of Serial2 on the Due
  Serial.begin(9600);

  dx_Init();
  delay(2000);

  // Serial.println(mySerial.available());
  // Serial.println("Hello UNO");
}

void loop() {
  static boolean receiving = false;
  static byte incomingData[8];  // Expecting 8 bytes of meaningful data
  static int dataIndex = 0;
  static unsigned long lastByteTime = 0;  // Time of the last received byte
  bool escapeNext = false;

  while (mySerial.available()) {
    byte incomingByte = mySerial.read();
    lastByteTime = millis();  // Update the last byte received time

    // Start of packet
    if (!receiving && incomingByte == 0x02) {
      receiving = true;
      dataIndex = 0;
      escapeNext = false;
      continue;
    }

    // End of packet
    if (receiving && incomingByte == 0x03 && !escapeNext) {
      receiving = false;
      processPacket(incomingData, dataIndex);
      continue;
    }

    if (receiving) {
      if (escapeNext) {
        incomingByte ^= 0x20;  // Unescape the byte
        escapeNext = false;
      } else if (incomingByte == 0x7D) {
        escapeNext = true;
        continue;  // Skip further processing of this byte
      }

      if (dataIndex < sizeof(incomingData)) {
        incomingData[dataIndex++] = incomingByte;
      } else {
        // Buffer overflow, reset receiving
        receiving = false;
        dataIndex = 0;
      }
    }
  }

  // Check for timeout
  if (receiving && (millis() - lastByteTime > TIMEOUT_MS)) {
    // Timeout occurred, reset receiving state
    receiving = false;
    dataIndex = 0;
  }
}

int reconstructInt(uint8_t a, uint8_t b, uint8_t c, uint8_t d) {
  return (a << 24) | (b << 16) | (c << 8) | d;
}

void processPacket(byte *data, int length) {
  if (length < 8) return;  // Ensure there's enough data

  int32_t a = reconstructInt(data[0], data[1], data[2], data[3]);
  int32_t b = reconstructInt(data[4], data[5], data[6], data[7]);

  // Convert and clamp values from -512 to 512 to -1.0 to 1.0 range
  float ta = max(min(a / 512.0f, 1.0f), -1.0f);
  float tb = max(min(b / 512.0f, 1.0f), -1.0f);

  // Scale values to motor-specific ranges
  a = (int)(ta * 10445);
  b = (int)(tb * 12810);

  move_dynamixel_1(a);
  move_dynamixel_2(b);

  // Debug output
  // Serial.print("Dynamixel 1 Target: ");
  // Serial.print(a);
  // Serial.print(" | Dynamixel 2 Target: ");
  // Serial.println(b);
}

Explanation of Key Changes

1. Buffer Size:

   • Reduced the incomingData buffer size to 8 bytes, as you expect 8 meaningful bytes.

2. Escape Handling:

   • The escape byte handling remains the same, ensuring special characters are correctly processed.

3. Synchronization:

   • lastByteTime is updated with the current time whenever a byte is read.
   • If no bytes

You

How could stuffing affect this? i’m skipping the stuffef bytes, don’t you see??

ChatGPT

You’re right; the current implementation effectively handles stuffing by escaping and unescaping the special bytes. Thus, the buffer size of 8 should be sufficient if you’re only concerned with the 8 meaningful bytes of data, even after handling the stuffing. The focus is on ensuring that any stuffed bytes are processed correctly without causing a buffer overflow.

Let’s ensure the code correctly handles the unescaping process while maintaining the expected packet structure and synchronization logic.

Updated Receiver Code (Arduino Uno)

#include <SoftwareSerial.h>
#include <DynamixelShield.h>

// This namespace is required to use Control table item names
using namespace ControlTableItem;

// Shield object
DynamixelShield dxl;

// MAX SPEED OF THE SERVOS
const int MAX_SPEED = 40;  //20 for quiet

//4096 = 1x full turn
const int GOAL_THRESHOLD = 50;

// Position offset for Dynamixel 1
const long offset_1 = 2139;

// Position offset for Dynamixel 2
const long offset_2 = 1909;

// First Dynamixel (swivel)
const uint8_t DXL_ID1 = 1;
// Second Dynamixel (tilt)
const uint8_t DXL_ID2 = 2;
// Boilerplate
const float DXL_PROTOCOL_VERSION = 2.0;

long dmx_01_goal = 0;
long dmx_02_goal = 0;

// Get current Dynamixel position
int pos(int DXL_ID) {
  return dxl.getPresentPosition(DXL_ID);
}

// Check if the Dynamixel has almost reached the goal
bool goal_reached(int DXL_ID, int goal) {
  return (abs(pos(DXL_ID) - goal) < GOAL_THRESHOLD);
}

//============= END OF DYNAMIXEL SETUP ===================/

void dx_Init() {
  // Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
  dxl.begin(57600);

  // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);

  // Get DYNAMIXEL information
  dxl.ping(DXL_ID1);
  dxl.ping(DXL_ID2);

  // DXL1 (turn off torque when configuring items in EEPROM area)
  dxl.torqueOff(DXL_ID1);
  dxl.setOperatingMode(DXL_ID1, OP_EXTENDED_POSITION);
  dxl.writeControlTableItem(PROFILE_ACCELERATION, DXL_ID1, 1);  //1 for sexy
  dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID1, MAX_SPEED);
  dxl.writeControlTableItem(MAX_VOLTAGE_LIMIT, DXL_ID1, 140);  //14V
  dxl.writeControlTableItem(CURRENT_LIMIT, DXL_ID1, 100);      //0.25A
  dxl.writeControlTableItem(VELOCITY_LIMIT, DXL_ID1, 70);      //13.74 RPM
  dxl.writeControlTableItem(MOVING_THRESHOLD, DXL_ID1, 0);     //13.74 RPM
  dxl.writeControlTableItem(TORQUE_LIMIT, DXL_ID1, 5);
  dxl.torqueOn(DXL_ID1);

  // DXL2 (turn off torque when configuring items in EEPROM area)
  dxl.torqueOff(DXL_ID2);
  dxl.setOperatingMode(DXL_ID2, OP_EXTENDED_POSITION);
  dxl.writeControlTableItem(PROFILE_ACCELERATION, DXL_ID2, 1);  //1 for sexy
  dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID2, MAX_SPEED);
  dxl.writeControlTableItem(MAX_VOLTAGE_LIMIT, DXL_ID2, 140);  //14V
  dxl.writeControlTableItem(CURRENT_LIMIT, DXL_ID2, 100);      //0.25A
  dxl.writeControlTableItem(VELOCITY_LIMIT, DXL_ID2, 70);      //13.74 RPM
  dxl.writeControlTableItem(MOVING_THRESHOLD, DXL_ID2, 0);     //13.74 RPM
  dxl.writeControlTableItem(TORQUE_LIMIT, DXL_ID2, 5);
  dxl.torqueOn(DXL_ID2);

  delay(100);

  // DYNAMIXEL SET TO 0 Position
  dxl.setGoalPosition(DXL_ID1, offset_1);
  dxl.setGoalPosition(DXL_ID2, offset_2);
}

void move_dynamixel_1(uint32_t value) {
  dxl.setGoalPosition(DXL_ID1, value + offset_1);
}

void move_dynamixel_2(uint32_t value) {
  dxl.setGoalPosition(DXL_ID2, value + offset_2);
}

SoftwareSerial mySerial(9, 10);  // RX, TX (TX not used)

const unsigned long TIMEOUT_MS = 500;  // Maximum time to wait for a packet

void setup() {
  mySerial.begin(9600); // Ensure this matches the baud rate of Serial2 on the Due
  Serial.begin(9600);

  dx_Init();
  delay(2000);

  // Serial.println(mySerial.available());
  // Serial.println("Hello UNO");
}

void loop() {
  static boolean receiving = false;
  static byte incomingData[8];  // Expecting 8 bytes of meaningful data
  static int dataIndex = 0;
  static unsigned long lastByteTime = 0;  // Time of the last received byte
  bool escapeNext = false;

  while (mySerial.available()) {
    byte incomingByte = mySerial.read();
    lastByteTime = millis();  // Update the last byte received time

    // Start of packet
    if (!receiving && incomingByte == 0x02) {
      receiving = true;
      dataIndex = 0;
      escapeNext = false;
      continue;
    }

    // End of packet
    if (receiving && incomingByte == 0x03 && !escapeNext) {
      receiving = false;
      processPacket(incomingData, dataIndex);
      continue;
    }

    if (receiving) {
      if (escapeNext) {
        incomingByte ^= 0x20;  // Unescape the byte
        escapeNext = false;
      } else if (incomingByte == 0x7D) {
        escapeNext = true;
        continue;  // Skip further processing of this byte
      }

      if (dataIndex < sizeof(incomingData)) {
        incomingData[dataIndex++] = incomingByte;
      } else {
        // Buffer overflow, reset receiving
        receiving = false;
        dataIndex = 0;
      }
    }
  }

  // Check for timeout
  if (receiving && (millis() - lastByteTime > TIMEOUT_MS)) {
    // Timeout occurred, reset receiving state
    receiving = false;
    dataIndex = 0;
  }
}

int reconstructInt(uint8_t a, uint8_t b, uint8_t c, uint8_t d) {
  return (a << 24) | (b << 16) | (c << 8) | d;
}

void processPacket(byte *data, int length) {
  if (length < 8) return;  // Ensure there's enough data

  int32_t a = reconstructInt(data[0], data[1], data[2], data[3]);
  int32_t b = reconstructInt(data[4], data[5], data[6], data[7]);

  // Convert and clamp values from -512 to 512 to -1.0 to 1.0 range
  float ta = max(min(a / 512.0f, 1.0f), -1.0f);
  float tb = max(min(b / 512.0f, 1.0f), -1.0f);

  // Scale values to motor-specific ranges
  a = (int)(ta * 10445);
  b = (int)(tb * 12810);

  move_dynamixel_1(a);
  move_dynamixel_2(b);

  // Debug output
  // Serial.print("Dynamixel 1 Target: ");
  // Serial.print(a);
  // Serial.print(" | Dynamixel 2 Target: ");
  // Serial.println(b);
}

Explanation of Key Components

1. Buffer Size:

   • The incomingData buffer is set to 8 bytes to match the expected meaningful data size.

2. Escape Handling:

   • If an escape byte (0x7D) is encountered, the next byte is unescaped by XORing it with 0x20.
   • This ensures special bytes (0x02, 0x03, 0x7D) are handled correctly.

3. Synchronization:

   • Start and end bytes (0x02, 0x03) mark the beginning and end of a packet.
   • The receiver only processes packets that have a valid start and end byte.

4. Timeout Handling:

   • The lastByteTime is updated whenever a byte is read.
   • If the time since the last byte exceeds TIMEOUT_MS, the receiving state is reset to prevent partial or corrupted data from being processed.

5. Buffer Overflow Protection:

   • If the incomingData buffer exceeds its size, the receiving state is reset to avoid overflow.

By maintaining the synchronization logic and proper escape handling, this code ensures robust communication between the Arduino Due and Arduino Uno, even with

You

while (mySerial.available()) {

does the existence of a while loop here create a problem?

ChatGPT

Using a while loop within loop() to read all available bytes from the SoftwareSerial buffer can potentially create issues if it blocks other necessary tasks. The loop() function in Arduino should ideally be non-blocking to ensure it can repeatedly execute and handle other tasks without significant delay.

Potential Issues with the while Loop

1. Blocking Other Code Execution:

   • If a lot of data is coming in, the while loop could dominate the CPU time, preventing other parts of loop() from executing.

2. Timeout Handling:

   • Relying solely on while (mySerial.available()) could make it harder to handle timeouts effectively, as the code could remain stuck in the loop if the data keeps arriving slowly or continuously.

3. Responsiveness:

   • The continuous while loop can reduce the responsiveness of your program, especially if you need to handle other inputs, outputs, or tasks concurrently.

Improved Approach: Using if Instead of while

Instead of using while, you can use if to check if data is available and then process one byte at a time. This approach ensures that the loop() function remains responsive.

Updated Receiver Code (Arduino Uno)

#include <SoftwareSerial.h>
#include <DynamixelShield.h>

// This namespace is required to use Control table item names
using namespace ControlTableItem;

// Shield object
DynamixelShield dxl;

// MAX SPEED OF THE SERVOS
const int MAX_SPEED = 40;  //20 for quiet

//4096 = 1x full turn
const int GOAL_THRESHOLD = 50;

// Position offset for Dynamixel 1
const long offset_1 = 2139;

// Position offset for Dynamixel 2
const long offset_2 = 1909;

// First Dynamixel (swivel)
const uint8_t DXL_ID1 = 1;
// Second Dynamixel (tilt)
const uint8_t DXL_ID2 = 2;
// Boilerplate
const float DXL_PROTOCOL_VERSION = 2.0;

long dmx_01_goal = 0;
long dmx_02_goal = 0;

// Get current Dynamixel position
int pos(int DXL_ID) {
  return dxl.getPresentPosition(DXL_ID);
}

// Check if the Dynamixel has almost reached the goal
bool goal_reached(int DXL_ID, int goal) {
  return (abs(pos(DXL_ID) - goal) < GOAL_THRESHOLD);
}

//============= END OF DYNAMIXEL SETUP ===================/

void dx_Init() {
  // Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
  dxl.begin(57600);

  // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);

  // Get DYNAMIXEL information
  dxl.ping(DXL_ID1);
  dxl.ping(DXL_ID2);

  // DXL1 (turn off torque when configuring items in EEPROM area)
  dxl.torqueOff(DXL_ID1);
  dxl.setOperatingMode(DXL_ID1, OP_EXTENDED_POSITION);
  dxl.writeControlTableItem(PROFILE_ACCELERATION, DXL_ID1, 1);
  dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID1, MAX_SPEED);
  dxl.writeControlTableItem(MAX_VOLTAGE_LIMIT, DXL_ID1, 140);  //14V
  dxl.writeControlTableItem(CURRENT_LIMIT, DXL_ID1, 100);      //0.25A
  dxl.writeControlTableItem(VELOCITY_LIMIT, DXL_ID1, 70);      //13.74 RPM
  dxl.writeControlTableItem(MOVING_THRESHOLD, DXL_ID1, 0);     //13.74 RPM
  dxl.writeControlTableItem(TORQUE_LIMIT, DXL_ID1, 5);
  dxl.torqueOn(DXL_ID1);

  // DXL2 (turn off torque when configuring items in EEPROM area)
  dxl.torqueOff(DXL_ID2);
  dxl.setOperatingMode(DXL_ID2, OP_EXTENDED_POSITION);
  dxl.writeControlTableItem(PROFILE_ACCELERATION, DXL_ID2, 1);
  dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID2, MAX_SPEED);
  dxl.writeControlTableItem(MAX_VOLTAGE_LIMIT, DXL_ID2, 140);  //14V
  dxl.writeControlTableItem(CURRENT_LIMIT, DXL_ID2, 100);      //0.25A
  dxl.writeControlTableItem(VELOCITY_LIMIT, DXL_ID2, 70);      //13.74 RPM
  dxl.writeControlTableItem(MOVING_THRESHOLD, DXL_ID2, 0);     //13.74 RPM
  dxl.writeControlTableItem(TORQUE_LIMIT, DXL_ID2, 5);
  dxl.torqueOn(DXL_ID2);

  delay(100);

  // DYNAMIXEL SET TO 0 Position
  dxl.setGoalPosition(DXL_ID1, offset_1);
  dxl.setGoalPosition(DXL_ID2, offset_2);
}

void move_dynamixel_1(uint32_t value) {
  dxl.setGoalPosition(DXL_ID1, value + offset_1);
}

void move_dynamixel_2(uint32_t value) {
  dxl.setGoalPosition(DXL_ID2, value + offset_2);
}

SoftwareSerial mySerial(9, 10);  // RX, TX (TX not used)

const unsigned long TIMEOUT_MS = 500;  // Maximum time to wait for a packet

void setup() {
  mySerial.begin(9600); // Ensure this matches the baud rate of Serial2 on the Due
  Serial.begin(9600);

  dx_Init();
  delay(2000);

  // Serial.println(mySerial.available());
  // Serial.println("Hello UNO");
}

void loop() {
  static boolean receiving = false;
  static byte incomingData[8];  // Expecting 8 bytes of meaningful data
  static int dataIndex = 0;
  static unsigned long lastByteTime = 0;  // Time of the last received byte
  bool escapeNext = false;

  if (mySerial.available()) {
    byte incomingByte = mySerial.read();
    lastByteTime = millis();  // Update the last byte received time

    // Start of packet
    if (!receiving && incomingByte == 0x02) {
      receiving = true;
      dataIndex = 0;
      escapeNext = false;
      return; // Exit early to allow loop to continue
    }

    // End of packet
    if (receiving && incomingByte == 0x03 && !escapeNext) {
      receiving = false;
      processPacket(incomingData, dataIndex);
      return; // Exit early to allow loop to continue
    }

    if (receiving) {
      if (escapeNext) {
        incomingByte ^= 0x20;  // Unescape the byte
        escapeNext = false;
      } else if (incomingByte == 0x7D) {
        escapeNext = true;
        return;  // Skip further processing of this byte
      }

      if (dataIndex < sizeof(incomingData)) {
        incomingData[dataIndex++] = incomingByte;
      } else {
        // Buffer overflow, reset receiving
        receiving = false;
        dataIndex = 0;
      }
    }
  }

  // Check for timeout
  if (receiving && (millis() - lastByteTime > TIMEOUT_MS)) {
    // Timeout occurred, reset receiving state
    receiving = false;
    dataIndex = 0;
  }
}

int reconstructInt(uint8_t a, uint8_t b, uint8_t c, uint8_t d) {
  return (a << 24) | (b << 16) | (c << 8) | d;
}

void processPacket(byte *data, int length) {
  if (length < 8) return;  // Ensure there's enough data

  int32_t a = reconstructInt(data[0], data[1], data[2], data[3]);
  int32_t b = reconstructInt(data[4], data[5], data[6], data[7]);

  // Convert and clamp values from -512 to 512 to -1.0 to 1.0 range
  float ta = max(min(a / 512.0f, 1.0f), -1.0f);
  float tb = max(min(b / 512.0f, 1.0f), -1.0f);

  // Scale values to motor-specific ranges
  a = (int)(ta * 10445);
  b = (int)(tb * 12810);

  move_dynamixel_1(a);
  move_dynamixel_2(b);

  // Debug output
  // Serial.print("Dynamixel 1 Target: ");
  // Serial.print(a);
  // Serial.print(" | Dynamixel 2 Target: ");
  // Serial.println(b);
}

Explanation of Changes

1. Using if Instead of while:

   • The if (mySerial.available()) statement checks if data is available and processes one byte at a time.
   • This allows the loop() function to remain non-blocking and continue executing other tasks if needed.

2. Early Return:

   • The code uses return statements to exit early if a condition is met (e.g., start byte detected, end byte detected, escape byte handling).
   • This ensures the rest of the loop() function can continue executing without being stuck in a long while loop.

3. Timeout Handling:

   • The timeout logic remains intact, resetting the receiving state if no data is received for a specified period (TIMEOUT_MS).

4. Maintaining Synchronization:

   • The code maintains synchronization by correctly handling the start and end bytes, as well as escape sequences.

This approach ensures that your receiver code remains responsive and can handle other tasks within the loop() function, making the overall system more robust and efficient.

You

but is it correct to initialize the variables in the loop?

ChatGPT

You are correct that it’s not ideal to initialize variables inside the loop() function if they don’t need to be reinitialized every iteration. Instead, these variables should be initialized outside the loop() function, either globally or within the setup() function.

Here’s an updated version of the receiver code that corrects this:

Updated Receiver Code (Arduino Uno)

#include <SoftwareSerial.h>
#include <DynamixelShield.h>

// This namespace is required to use Control table item names
using namespace ControlTableItem;

// Shield object
DynamixelShield dxl;

// MAX SPEED OF THE SERVOS
const int MAX_SPEED = 40;  //20 for quiet

//4096 = 1x full turn
const int GOAL_THRESHOLD = 50;

// Position offset for Dynamixel 1
const long offset_1 = 2139;

// Position offset for Dynamixel 2
const long offset_2 = 1909;

// First Dynamixel (swivel)
const uint8_t DXL_ID1 = 1;
// Second Dynamixel (tilt)
const uint8_t DXL_ID2 = 2;
// Boilerplate
const float DXL_PROTOCOL_VERSION = 2.0;

long dmx_01_goal = 0;
long dmx_02_goal = 0;

// Get current Dynamixel position
int pos(int DXL_ID) {
  return dxl.getPresentPosition(DXL_ID);
}

// Check if the Dynamixel has almost reached the goal
bool goal_reached(int DXL_ID, int goal) {
  return (abs(pos(DXL_ID) - goal) < GOAL_THRESHOLD);
}

//============= END OF DYNAMIXEL SETUP ===================/

void dx_Init() {
  // Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
  dxl.begin(57600);

  // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);

  // Get DYNAMIXEL information
  dxl.ping(DXL_ID1);
  dxl.ping(DXL_ID2);

  // DXL1 (turn off torque when configuring items in EEPROM area)
  dxl.torqueOff(DXL_ID1);
  dxl.setOperatingMode(DXL_ID1, OP_EXTENDED_POSITION);
  dxl.writeControlTableItem(PROFILE_ACCELERATION, DXL_ID1, 1);
  dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID1, MAX_SPEED);
  dxl.writeControlTableItem(MAX_VOLTAGE_LIMIT, DXL_ID1, 140);  //14V
  dxl.writeControlTableItem(CURRENT_LIMIT, DXL_ID1, 100);      //0.25A
  dxl.writeControlTableItem(VELOCITY_LIMIT, DXL_ID1, 70);      //13.74 RPM
  dxl.writeControlTableItem(MOVING_THRESHOLD, DXL_ID1, 0);     //13.74 RPM
  dxl.writeControlTableItem(TORQUE_LIMIT, DXL_ID1, 5);
  dxl.torqueOn(DXL_ID1);

  // DXL2 (turn off torque when configuring items in EEPROM area)
  dxl.torqueOff(DXL_ID2);
  dxl.setOperatingMode(DXL_ID2, OP_EXTENDED_POSITION);
  dxl.writeControlTableItem(PROFILE_ACCELERATION, DXL_ID2, 1);
  dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID2, MAX_SPEED);
  dxl.writeControlTableItem(MAX_VOLTAGE_LIMIT, DXL_ID2, 140);  //14V
  dxl.writeControlTableItem(CURRENT_LIMIT, DXL_ID2, 100);      //0.25A
  dxl.writeControlTableItem(VELOCITY_LIMIT, DXL_ID2, 70);      //13.74 RPM
  dxl.writeControlTableItem(MOVING_THRESHOLD, DXL_ID2, 0);     //13.74 RPM
  dxl.writeControlTableItem(TORQUE_LIMIT, DXL_ID2, 5);
  dxl.torqueOn(DXL_ID2);

  delay(100);

  // DYNAMIXEL SET TO 0 Position
  dxl.setGoalPosition(DXL_ID1, offset_1);
  dxl.setGoalPosition(DXL_ID2, offset_2);
}

void move_dynamixel_1(uint32_t value) {
  dxl.setGoalPosition(DXL_ID1, value + offset_1);
}

void move_dynamixel_2(uint32_t value) {
  dxl.setGoalPosition(DXL_ID2, value + offset_2);
}

SoftwareSerial mySerial(9, 10);  // RX, TX (TX not used)

const unsigned long TIMEOUT_MS = 500;  // Maximum time to wait for a packet

// Initialize variables outside of the loop
bool receiving = false;
byte incomingData[8];  // Expecting 8 bytes of meaningful data
int dataIndex = 0;
unsigned long lastByteTime = 0;  // Time of the last received byte
bool escapeNext = false;

void setup() {
  mySerial.begin(9600); // Ensure this matches the baud rate of Serial2 on the Due
  Serial.begin(9600);

  dx_Init();
  delay(2000);

  // Serial.println(mySerial.available());
  // Serial.println("Hello UNO");
}

void loop() {
  if (mySerial.available()) {
    byte incomingByte = mySerial.read();
    lastByteTime = millis();  // Update the last byte received time

    // Start of packet
    if (!receiving && incomingByte == 0x02) {
      receiving = true;
      dataIndex = 0;
      escapeNext = false;
      return; // Exit early to allow loop to continue
    }

    // End of packet
    if (receiving && incomingByte == 0x03 && !escapeNext) {
      receiving = false;
      processPacket(incomingData, dataIndex);
      return; // Exit early to allow loop to continue
    }

    if (receiving) {
      if (escapeNext) {
        incomingByte ^= 0x20;  // Unescape the byte
        escapeNext = false;
      } else if (incomingByte == 0x7D) {
        escapeNext = true;
        return;  // Skip further processing of this byte
      }

      if (dataIndex < sizeof(incomingData)) {
        incomingData[dataIndex++] = incomingByte;
      } else {
        // Buffer overflow, reset receiving
        receiving = false;
        dataIndex = 0;
      }
    }
  }

  // Check for timeout
  if (receiving && (millis() - lastByteTime > TIMEOUT_MS)) {
    // Timeout occurred, reset receiving state
    receiving = false;
    dataIndex = 0;
  }
}

int reconstructInt(uint8_t a, uint8_t b, uint8_t c, uint8_t d) {
  return (a << 24) | (b << 16) | (c << 8) | d;
}

void processPacket(byte *data, int length) {
  if (length < 8) return;  // Ensure there's enough data

  int32_t a = reconstructInt(data[0], data[1], data[2], data[3]);
  int32_t b = reconstructInt(data[4], data[5], data[6], data[7]);

  // Convert and clamp values from -512 to 512 to -1.0 to 1.0 range
  float ta = max(min(a / 512.0f, 1.0f), -1.0f);
  float tb = max(min(b / 512.0f, 1.0f), -1.0f);

  // Scale values to motor-specific ranges
  a = (int)(ta * 10445);
  b = (int)(tb * 12810);

  move_dynamixel_1(a);
  move_dynamixel_2(b);

  // Debug output
  // Serial.print("Dynamixel 1 Target: ");
  // Serial.print(a);
  // Serial.print(" | Dynamixel 2 Target: ");
  // Serial.println(b);
}

Key Points

1. Global Variables:

   • receiving, incomingData, dataIndex, lastByteTime, and escapeNext are declared as global variables, ensuring they retain their values between iterations of loop().

2. Initialization in setup():

   • Only necessary setup code is placed inside the setup() function.

3. Non-blocking loop():

   • The loop() function processes one byte at a time if data is available, maintaining responsiveness.

4. Early Returns:

   • Using return statements to exit the function early allows the loop() to handle other tasks more effectively.

By ensuring variables are not unnecessarily reinitialized and keeping the loop() function non-blocking, the code will run more efficiently and be easier to maintain.