Mixing of Motor Position Data in Arduino Mega with ODrive

1

The functionality of the code consists of requesting the position from each motor and sending an array through the serial port containing motor1 position, time, and motor2 position. This way, I can plot the data on my computer.

The problem is that sometimes, for some reason, the order is correct, but other times, the array I receive is motor2 position, time, motor1 position. This inconsistency makes it impossible to read the positions accurately, as the graph ends up with strange spikes.

Here is the current code:

void printMotorPositions() {
  float position1Motor0 = readPosition(0);
  float position2Motor1 = readPosition(1);

  unsigned long currentMillis = millis();

  Serial.print("[");
  Serial.print(position1Motor0);
  Serial.print(",");
  Serial.print(currentMillis);
  Serial.print(",");
  Serial.print(position2Motor1);
  Serial.println("]");
}

float readPosition(int axis) {
  String command = String("r axis") + axis + String(".encoder.pos_estimate\n");
  odrive_serial.print(command);

  String response = readResponse();
  // Parse response to ensure it matches the expected format
  // This can be enhanced with more sophisticated checks
  return response.toFloat();
}

String readResponse() {
  String response = "";
  unsigned long startMillis = millis();
  const unsigned long timeout = 1000;
  
  while (millis() - startMillis < timeout) {
    if (odrive_serial.available()) {
      char c = odrive_serial.read();
      if (c == '\n') {
        break;
      }
      response += c;
    }
  }
  return response;
}
2

Have you used an external serial adapter to verify that the data the Arduino is sending is correct? What baudrate are you using? You may need to add a very small delay to ensure the ODrive turnaround data is correct. Why not use the f command, which will be faster? ASCII Protocol — ODrive Documentation 0.5.6 documentation