A Simpler Way to use SyncWrite with Dynamixel2Arduino Library

Community
, ,
1

I was comparing the programming approach used in the provided Dynamixel2Arduino example sketches: sync_read_write_position.ino and sync_read_write_velocity.ino, and the one used on the ROBOTIS E-manual (https://emanual.robotis.com/docs/en/popup/arduino_api/syncWrite/).

I’ve found the approach shown on the E-manual much easier to understand and expand its use, so I created the following Arduino sketch for MKR ZERO combining SyncWrite for Goal Velocity and Goal Position. I used TWO 2XL-430s: the one with IDs 1-2 is set into Position Control mode, and the one with IDs 11-12 is set into Velocity Control mode.

#include <Dynamixel2Arduino.h>

#define DXL_SERIAL   Serial1
#define DEBUG_SERIAL Serial
const int DXL_DIR_PIN = A6; // DYNAMIXEL Shield DIR PIN for MKR Shield
const uint8_t BROADCAST_ID = 254;
const float DYNAMIXEL_PROTOCOL_VERSION = 2.0;
const uint8_t GP_DXL_ID_CNT = 2;
const uint8_t GP_DXL_ID_LIST[GP_DXL_ID_CNT] = {1, 2};  // servos set in Position Control mode
const uint8_t GV_DXL_ID_CNT = 2;
const uint8_t GV_DXL_ID_LIST[GV_DXL_ID_CNT] = {11, 12};   // servos set in Velocity Control mode
const uint16_t ADDR_X_PRESENT_POSITION = 132;
const uint16_t LEN_X_PRESENT_POSITION = 4;
const uint16_t ADDR_X_GOAL_POSITION = 116;
const uint16_t LEN_X_GOAL_POSITION = 4;
const uint16_t ADDR_X_GOAL_VELOCITY = 104; 
const uint16_t LEN_X_GOAL_VELOCITY = 4;

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);

ParamForSyncWriteInst_t GV_sync_write_param; // Parameters structure for SW GV packet
ParamForSyncWriteInst_t GP_sync_write_param; // Parameters structure for SW GP packet

int32_t goal_velocity[GV_DXL_ID_CNT] = {100, 100};
int32_t goal_position[GP_DXL_ID_CNT] = {2048, 2048}; 

void setup() {

  DEBUG_SERIAL.println("Initial Setup & Test");
  // put your setup code here, to run once:
  uint8_t i;
  dxl.begin(1000000);
  dxl.setPortProtocolVersion(DYNAMIXEL_PROTOCOL_VERSION);
  dxl.scan();
  
  // Preparing for GV SW packet
  GV_sync_write_param.addr = ADDR_X_GOAL_VELOCITY; //Goal Velocity of DYNAMIXEL-X series
  GV_sync_write_param.length = LEN_X_GOAL_VELOCITY;
  GV_sync_write_param.id_count = GV_DXL_ID_CNT;  // Number of servos in Velocity Control Mode
  for(i = 0; i < GV_DXL_ID_CNT; i++){
    GV_sync_write_param.xel[i].id = GV_DXL_ID_LIST[i];
  }
  // Preparing for GP SW Packet
  GP_sync_write_param.addr = ADDR_X_GOAL_POSITION; //Goal Position of DYNAMIXEL-X series
  GP_sync_write_param.length = LEN_X_GOAL_POSITION;
  GP_sync_write_param.id_count = GP_DXL_ID_CNT;  // Number of servos in Position Control Mode
  for(i = 0; i < GP_DXL_ID_CNT; i++){
    GP_sync_write_param.xel[i].id = GP_DXL_ID_LIST[i];
  }

// Setting GP servos into Position Control mode
  for(i = 0; i < GP_DXL_ID_CNT; i++){
    dxl.torqueOff(GP_DXL_ID_LIST[i]);
    dxl.setOperatingMode(GP_DXL_ID_LIST[i], OP_POSITION);
  }

// Setting GV servos into Velocity Control mode
  for(i=0; i<GV_DXL_ID_CNT; i++){
    dxl.torqueOff(GV_DXL_ID_LIST[i]);
    dxl.setOperatingMode(GV_DXL_ID_LIST[i], OP_VELOCITY);
  }
  
// Torque enable all GP and GV servos  
  dxl.torqueOn(BROADCAST_ID);

 // Update actual goal velocity data
  for(i=0; i<GV_DXL_ID_CNT; i++){
    memcpy(GV_sync_write_param.xel[i].data, &goal_velocity[i], sizeof(goal_velocity[i]));
  }
  dxl.syncWrite(GV_sync_write_param);   // send GV SW Packet

  // Update actual goal position data
  for(i=0; i<GP_DXL_ID_CNT; i++){
    memcpy(GP_sync_write_param.xel[i].data, &goal_position[i], sizeof(goal_position[i]));
  }
  dxl.syncWrite(GP_sync_write_param);   // send GP SW Packet

  delay(5000);
  DEBUG_SERIAL.println("END of Initial Setup & Test");

}   // end of setup()

void loop() {
  DEBUG_SERIAL.println("Randomized Test");

   // Update actual goal velocity data with randomized values
  for(int i=0; i<GV_DXL_ID_CNT; i++){
    goal_velocity[i] = (uint32_t) random(10,200);
    memcpy(GV_sync_write_param.xel[i].data, &goal_velocity[i], sizeof(goal_velocity[i]));
  }
  dxl.syncWrite(GV_sync_write_param);   // send GV SW Packet

  // Update actual goal position data with randomized values
  for(int i=0; i<GP_DXL_ID_CNT; i++){
    goal_position[i] = (uint32_t) random(1024,3072);
    memcpy(GP_sync_write_param.xel[i].data, &goal_position[i], sizeof(goal_position[i]));
  }
  dxl.syncWrite(GP_sync_write_param);   // send GP SW Packet

  delay(1000);

}   // end of loop()

Enjoy!

2 Likes