[OpenCM 9.04 + XL-320] Motors not responding after a certain time

[mcaravati]

Hi,

I am currently trying to control a Poppy Ergo Jr arm, which uses 6 XL-320 servos, using an OpenCM9.04 C-Type. Everything works fine when the robot is powered up, but after a few movements, the servos stop moving but the goal position is set and is different from the current position. Also, when one servo stops working, it of course cripples the entire arm.

Please find below the full code. Also, if you want to replicate the issue, sending the command set_motor <id> <angle in degrees> via serial allows you to move the servos.

#include <Dynamixel2Arduino.h>

#define DXL_SERIAL Serial1
#define DEBUG_SERIAL Serial

#define MOTOR_BAUDRATE 1000000 // XL-320 baudrate
#define DXL_DIR_PIN 28         // OpenCM 9.04 DYNAMIXEL TTL Bus
#define DXL_PROTOCOL_VERSION 2.0
#define ANGLE_TOLERANCE 0.5

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);

struct Motor
{
  int id;
  int position;
};

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

void setup()
{
  // Use UART port of DYNAMIXEL Shield to debug.
  DEBUG_SERIAL.begin(115200);
  while (!DEBUG_SERIAL)
    ;

  dxl.begin(MOTOR_BAUDRATE);
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);

  for (int i = 1; i <= 6; i++)
  {
    // Get DYNAMIXEL information
    dxl.ping(i);

    // Turn off torque when configuring items in EEPROM area
    dxl.torqueOff(i);
    dxl.setOperatingMode(i, OP_POSITION);
    dxl.torqueOn(i);

    // Limit the maximum velocity in Position Control Mode. Use 0 for Max speed
    dxl.writeControlTableItem(PROFILE_VELOCITY, i, 0);
  }

  DEBUG_SERIAL.println("ready");
}

/**
 * @brief Sets The goal position of a motor.
 *
 * @param motor The Motor struct representing the motor to set the position for.
 */
void _set_motor(Motor &motor)
{
  float f_present_position = dxl.getPresentPosition(motor.id, UNIT_DEGREE);
  if (abs(f_present_position - motor.position) < ANGLE_TOLERANCE)
  {
#ifdef DEBUG
    DEBUG_SERIAL.println("Too close");
#endif
    return;
  }

  int success = dxl.setGoalPosition(motor.id, motor.position, UNIT_DEGREE);

  if (success != 1)
  {
#ifdef DEBUG
    DEBUG_SERIAL.print("Failed to set goal position for motor ");
    DEBUG_SERIAL.println(dxl.getLastLibErrCode());
#endif
    return;
  }

  // Wait for the motor to reach the goal position
  do
  {
    f_present_position = dxl.getPresentPosition(motor.id, UNIT_DEGREE);
#ifdef DEBUG
    DEBUG_SERIAL.print("Motor ");
    DEBUG_SERIAL.print(motor.id);
    DEBUG_SERIAL.print(" is at position ");
    DEBUG_SERIAL.print(f_present_position);
    DEBUG_SERIAL.print(". Goal position for motor ");
    DEBUG_SERIAL.print(motor.id);
    DEBUG_SERIAL.print(" is ");
    DEBUG_SERIAL.println(motor.position);

    DEBUG_SERIAL.print("Present current: ");
    DEBUG_SERIAL.print(dxl.getPresentCurrent(motor.id));
    DEBUG_SERIAL.print(" Goal current: ");
    DEBUG_SERIAL.println(dxl.readControlTableItem(GOAL_CURRENT, motor.id));
#endif
  } while (abs(f_present_position - motor.position) > ANGLE_TOLERANCE);
}

/**
 * @brief Split a string into an array of strings based on a delimiter
 *
 * @param input The string to split
 * @param delimiter The delimiter to split the string on
 * @param output The array of strings to store the split string in
 * @param max_items The maximum number of items to split the string into
 * @return int The number of items the string was split into
 */
int split_string(String input, char delimiter, String *output, int max_items)
{
  int item_count = 0;
  int start_index = 0;
  int end_index = input.indexOf(delimiter, start_index);

  while (end_index >= 0 && item_count < max_items)
  {
    // Skip the first element in the output array
    if (item_count > 0)
      output[item_count - 1] = input.substring(start_index, end_index);

    item_count++;
    start_index = end_index + 1;
    end_index = input.indexOf(delimiter, start_index);
  }

  if (start_index < input.length() && item_count < max_items)
  {
    output[item_count - 1] = input.substring(start_index);
    item_count++;
  }

  return item_count - 1; // Return item_count - 1 to account for skipping the first element
}

/**
 * @brief Process the set_motor message
 *
 * @param message The message to process
 */
int process_set_motor_message(String message, Motor *motors)
{
  String arguments[12] = {};
  int num_arguments = split_string(message, ' ', arguments, 12);

#ifdef DEBUG
  DEBUG_SERIAL.print("Number of arguments: ");
  DEBUG_SERIAL.println(num_arguments);

  for (unsigned int i = 0; i < num_arguments; i++)
  {
    DEBUG_SERIAL.print("Argument ");
    DEBUG_SERIAL.print(i);
    DEBUG_SERIAL.print(": ");
    DEBUG_SERIAL.println(arguments[i]);
  }
#endif

  if (num_arguments % 2 != 0)
  {
#ifdef DEBUG
    DEBUG_SERIAL.println("Invalid number of arguments");
#endif
    return -1;
  }

  unsigned int motor_index = 0;

  for (unsigned int argument_index = 0; argument_index < num_arguments; argument_index++)
  {
    if (argument_index % 2 == 0) // If even, it is the motor ID
    {
      motors[motor_index].id = arguments[argument_index].toInt();
    }
    else
    {
      motors[motor_index].position = arguments[argument_index].toInt();

      // Avoid blocking the program if the position is out of range
      if (motors[motor_index].position < 0 || motors[motor_index].position > 296)
      {
        motors[motor_index].position = 0;
      }

      motor_index++;
    }
  }

  return num_arguments / 2;
}

void loop()
{
  if (DEBUG_SERIAL.available() > 0)
  {
    String message = DEBUG_SERIAL.readStringUntil('\n');
    message.trim();

    if (message.startsWith("set_motor"))
    {
      Motor motors[6] = {};
      int number_motors = process_set_motor_message(message, motors);

      if (number_motors < 0)
      {
#ifdef DEBUG
        DEBUG_SERIAL.println("Invalid message");
#endif
        return;
      }

      for (unsigned int motor_index; motor_index < number_motors; motor_index++)
      {
#ifdef DEBUG
        // Print the extracted values
        DEBUG_SERIAL.print("Motor ID: ");
        DEBUG_SERIAL.println(motors[motor_index].id);
        DEBUG_SERIAL.print("Motor Position: ");
        DEBUG_SERIAL.println(motors[motor_index].position);
#endif
        _set_motor(motors[motor_index]);
      }
    }
  }
}