A test bench which allows a dynamixel motors to learn and playback motion.

dynamixel_three_motor_teach_repeat.ino

#include <DynamixelWorkbench.h>

#if defined(__OPENCM904__)
  #define DEVICE_NAME "3" //Dynamixel on Serial3(USART3)  <-OpenCM 485EXP
#elif defined(__OPENCR__)
  #define DEVICE_NAME ""
#endif   

#define BAUDRATE  57600
#define DXL_ID    1

DynamixelWorkbench dxl_wb;

int32_t motor_positions[3][300];

void setup() {
  // put your setup code here, to run once:
  Serial.begin(57600);
  while(!Serial); // Wait for Opening Serial Monitor

  const char *log = NULL;
  bool result = false;

  uint8_t scanned_id[16];
  uint8_t dxl_cnt = 0;
  uint8_t range = 100;

  result = dxl_wb.init(DEVICE_NAME, BAUDRATE, &log);
  if (result == false)
  {
    Serial.println(log);
    Serial.println("Failed to init Dynamixel Bus");
    return;
  }

  init_read(1);
  init_read(2);
  init_read(3);
  bool result_init =true;
  /*result_init &= init_motor(1);
  result_init &= init_motor(2);
  result_init &= init_motor(3);*/
}

bool init_motor(int id) {
  
  const char *log = NULL;
  
  bool result = false;
  result = dxl_wb.jointMode(id, 0, 0, &log);
  if (result == false)
  {
    Serial.println(log);
    Serial.print("Failed to set joint mode ");
    Serial.print(id);
    Serial.print("\n");
  }
  return result;
}

bool init_read(int id)
{
  const char *log = NULL;
  uint16_t model_number;
  bool result = dxl_wb.ping(id, &model_number, &log);
  if (result == false)
  {
    Serial.println(log);
    Serial.println("Failed to ping");
  }
  return result;
}

int32_t get_position(int dxl_id){
  const char* log = NULL;
  int32_t get_data = -1;
  bool result = dxl_wb.itemRead(dxl_id, "Present_Position", &get_data, &log);
  if (result == false)
  {
    Serial.println(log);
    Serial.println("Failed to get present position");
  }
  return get_data;
}

int record_pointer = 0;

void record() {
  unsigned long long start = millis();
  while(record_pointer < 300)
  {
    // put your main code here, to run repeatedly:
    int32_t g1 = get_position(1);
    int32_t g2 = get_position(2);
    int32_t g3 = get_position(3);

    Serial.println("[recording]===========");
    Serial.println("Robot state:");
    Serial.print("Motor 1: ");
    Serial.print(g1);
    Serial.print("\n");
    Serial.print("Motor 2: ");
    Serial.print(g2);
    Serial.print("\n");
    Serial.print("Motor 3: ");
    Serial.print(g3); 
    Serial.print("\n");
    
    motor_positions[0][record_pointer] = g1;
    motor_positions[1][record_pointer] = g2;
    motor_positions[2][record_pointer] = g3;
    
    record_pointer+=1;
    delay(10);
  }
  Serial.println("Recording Done!");
}

void reverse()
{
  init_motor(1);
  init_motor(2);
  init_motor(3);
  
  for(int i = record_pointer-1; i >= 0; i--)
  {
      for(int j =1; j <= 3; j++)
      {
        int32_t pos = motor_positions[j-1][i];
        dxl_wb.goalPosition(j, pos);
      }
      delay(10);
  }
}

void forward()
{
  init_motor(1);
  init_motor(2);
  init_motor(3);
  
  for(int i = 0; i < record_pointer; i++)
  {
      for(int j =1; j <= 3; j++)
      {
        int32_t pos = motor_positions[j-1][i];
        dxl_wb.goalPosition(j, pos);
      }
      delay(10);
  }
}

bool disarm_motor(int id)
{
  const char *log = NULL;
  
  bool result = false;
  result = dxl_wb.torqueOff(id, &log);
  if (result == false)
  {
    Serial.println(log);
    Serial.print("Failed to disarm motor");
    Serial.print(id);
    Serial.print("\n");
  }
  return result;
}
void loop() 
{
  int pushed = digitalRead(BDPIN_PUSH_SW_1);
  int pushed2 = digitalRead(BDPIN_PUSH_SW_2);
  if(pushed) {
    record_pointer =0;
    Serial.println("Entering record mode!");
    record();

    Serial.println("Warning! Entering Reverse Mode in 5s");
    delay(5000);
    reverse();
    disarm_motor(1);
    disarm_motor(2);
    disarm_motor(3);
  }
  else {
    if(pushed2)
    {
      delay(5000);
      forward();
      reverse();
      disarm_motor(1);
    disarm_motor(2);
    disarm_motor(3);
    }
  }
}