Protocol 1 Syncwrite Goal Position as well as Moving Speed to AX-12A

[v-r-a]

Hello,

I am trying to synchronously write to 12 motors at a time using Dynamixel Arduino Shield on Arduino Uno. I am using a separate usb2serial converter to read data on PC.

I want to write Goal Position as well as Moving Speed to all the motors. (Starting address 30, data length 4 bytes).

I am getting the wrong robot pose. I have previously tested similar (the only difference is in the library functions of Dynamixel SDK vs Dynamixel2Arduino) code using Dynamixel SDK in C++ language using USB2Dynamixel. It worked perfectly fine.

Something is going wrong while defining the sync write packet. Also, there is no example of sync write with multiple sequential control table items written together, e.g., cw compliance slope + ccw compliance slope + goal position + moving speed.

Kindly help me debug the sync write part of the code:

// Bioloid 12 DOF
#include <DynamixelShield.h>

#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560)
#include <SoftwareSerial.h>
SoftwareSerial soft_serial(7, 8);  // DYNAMIXELShield UART RX/TX
#define DEBUG_SERIAL soft_serial
#elif defined(ARDUINO_SAM_DUE) || defined(ARDUINO_SAM_ZERO)
#define DEBUG_SERIAL SerialUSB
#else
#define DEBUG_SERIAL Serial
#endif
/***********************************************************************************************/
// Macros
#define DXL_LOBYTE(w) ((uint8_t)(((uint64_t)(w)) & 0xff))
#define DXL_HIBYTE(w) ((uint8_t)((((uint64_t)(w)) >> 8) & 0xff))
/***********************************************************************************************/
// Control table address
const uint8_t ADDR_AX_TORQUE_ENABLE = 24;
const uint8_t ADDR_AX_GOAL_POSITION = 30;
const uint8_t ADDR_AX_MOVING_SPEED = 32;
const uint8_t ADDR_AX_PRESENT_POSITION = 36;
const uint8_t ADDR_AX_CW_COMP_MARGIN = 26;
const uint8_t ADDR_AX_CCW_COMP_MARGIN = 27;
const uint8_t ADDR_AX_CW_COMP_SLOPE = 28;
const uint8_t ADDR_AX_CCW_COMP_SLOPE = 29;
const uint8_t ADDR_AX_PRESENT_LOAD = 40;
const uint8_t ADDR_AX_LOCK_EEPROM = 47;
const uint8_t ADDR_AX_RDT = 5;

// Data Byte Length
const uint8_t LEN_AX_GOAL_POSITION = 2;
const uint8_t LEN_AX_MOVING_SPEED = 2;
const uint8_t LEN_AX_PRESENT_POSITION = 2;
const uint8_t LEN_AX_CW_COMP_MARGIN = 1;
const uint8_t LEN_AX_CCW_COMP_MARGIN = 1;
const uint8_t LEN_AX_CW_COMP_SLOPE = 1;
const uint8_t LEN_AX_CCW_COMP_SLOPE = 1;
const uint8_t LEN_AX_PRESENT_LOAD = 2;
const uint8_t LEN_AX_LOCK_EEPROM = 1;
const uint8_t LEN_AX_RDT = 1;

const uint8_t TORQUE_ENABLE = 1;
const uint8_t TORQUE_DISABLE = 0;
const uint8_t LOCK_EEPROM = 1;

// Default pose
const uint16_t HY_MIN = 620;
const uint16_t HY_MAX = 720;
const uint16_t HY_NOM = 666;

const uint16_t HR_MIN = 472;
const uint16_t HR_MAX = 552;
const uint16_t HR_NOM = 512;

const uint16_t HP_MIN = 490;
const uint16_t HP_MAX = 780;
const uint16_t HP_NOM = 550;

const uint16_t KP_MIN = 560;
const uint16_t KP_MAX = 885;
const uint16_t KP_NOM = 590;

const uint16_t AP_MIN = 300;
const uint16_t AP_MAX = 500;
const uint16_t AP_NOM = 460;

const uint16_t AR_MIN = 472;
const uint16_t AR_MAX = 552;
const uint16_t AR_NOM = 512;

const uint8_t NMOTOR = 12;
const uint8_t MOTOR_ID_LIST[NMOTOR] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12 };

// Homing position and return to home velocity
uint16_t home_pos_list[NMOTOR] = { HY_NOM, HR_NOM, HP_NOM, KP_NOM, AP_NOM, AR_NOM, HY_NOM, HR_NOM, HP_NOM, KP_NOM, AP_NOM, AR_NOM };
uint16_t home_vel_list[NMOTOR] = { 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50, 50 };

/***********************************************************************************************/
// Protocol version
const float PROTOCOL_VERSION = 1.0;  // See which protocol version is used in the Dynamixel
// Starting address of the Data to write; Goal Position = 30
const uint16_t SW_START_ADDR = ADDR_AX_GOAL_POSITION;
// Debug: Length of the Data to write: 2 bytes: Goal Position
// Length of the Data to write: 4 bytes: Goal Position + Goal Velocity
const uint16_t SW_ADDR_LEN = 4;
uint8_t myswdata[SW_ADDR_LEN] = { 0, 0, 0, 0 };
/***********************************************************************************************/
/*
  typedef struct InfoSyncWriteInst{
    uint16_t addr;
    uint16_t addr_length;
    XELInfoSyncWrite_t* p_xels;
    uint8_t xel_count;
    bool is_info_changed;
    InfoSyncBulkBuffer_t packet;
  } __attribute__((packed)) InfoSyncWriteInst_t;
*/
DYNAMIXEL::InfoSyncWriteInst_t allmotorSW;

/* syncWrite
  Structures containing the necessary information to process the 'syncWrite' packet.

  typedef struct XELInfoSyncWrite{
    uint8_t* p_data;
    uint8_t id;
  } __attribute__((packed)) XELInfoSyncWrite_t;
*/
DYNAMIXEL::XELInfoSyncWrite_t all_info_xels_sw[NMOTOR];
// Create an instance of Master class/ Dynamixel2Arduino class/ whatever has all the required functions
DynamixelShield dxl;
//This namespace is required to use Control table item names
using namespace ControlTableItem;
/***********************************************************************************************/
int debug_motor_pose;
bool sync_write_feedback, pos_write_feedback, vel_write_feedback;
int inputpos;
/***********************************************************************************************/
void setup() {
  // put your setup code here, to run once:

  // Communication with PC starts---------------------------->>
  DEBUG_SERIAL.begin(9600);

  // Communication starts------------------------------------>>
  dxl.begin(1000000);  // dxl baudrate
  dxl.setPortProtocolVersion(1.0);
  uint8_t motor_id;
  // Write control Table Items
  for (int i = 0; i < NMOTOR; i++) {
    motor_id = MOTOR_ID_LIST[i];
    // Redefine the packet return delay time (1 ms)
    dxl.writeControlTableItem(RETURN_DELAY_TIME, motor_id, 5);
    //delay(50);
    // Lock the EEPROM
    dxl.writeControlTableItem(LOCK, motor_id, LOCK_EEPROM);
    //delay(50);
    // Set compliance margin and compliance slope
    dxl.writeControlTableItem(CW_COMPLIANCE_MARGIN, motor_id, 2);
    //delay(50);
    dxl.writeControlTableItem(CCW_COMPLIANCE_MARGIN, motor_id, 2);
    //delay(50);
    dxl.writeControlTableItem(CW_COMPLIANCE_SLOPE, motor_id, 16);
    //delay(50);
    dxl.writeControlTableItem(CCW_COMPLIANCE_SLOPE, motor_id, 16);
    //delay(50);
    // Enable Torques one by one
    dxl.torqueOn(motor_id);
    //delay(50);
  }

  /***********************************************************************************************/
  // Fill the members of structure to syncWrite using internal packet buffer
  // All motors sync write
  allmotorSW.packet.p_buf = nullptr;
  allmotorSW.packet.is_completed = false;
  allmotorSW.addr = SW_START_ADDR;
  allmotorSW.addr_length = SW_ADDR_LEN;
  allmotorSW.p_xels = all_info_xels_sw;
  allmotorSW.xel_count = 0;

  // AddParam equivalent: Go to the home pose
  // SOME ISSUE HERE
  for (int i = 0; i < NMOTOR; i++) {
    myswdata[0] = DXL_LOBYTE(home_pos_list[i]);  // low byte
    myswdata[1] = DXL_HIBYTE(home_pos_list[i]);  // high byte
    myswdata[2] = DXL_LOBYTE(home_vel_list[i]);  // low byte
    myswdata[3] = DXL_HIBYTE(home_vel_list[i]);  // high byte
    all_info_xels_sw[i].id = MOTOR_ID_LIST[i];
    all_info_xels_sw[i].p_data = myswdata;
    allmotorSW.xel_count++;
    allmotorSW.is_info_changed = true;
  }
  dxl.syncWrite(&allmotorSW);
  // Allow some time for going to home position
  delay(1000);
}
/***********************************************************************************************/
void loop() {
  // put your main code here, to run repeatedly:
  // Debugging: Read motor positions and print it on the screen
  for (int i = 0; i < NMOTOR; i++) {
    debug_motor_pose = dxl.readControlTableItem(PRESENT_POSITION, MOTOR_ID_LIST[i]);
    DEBUG_SERIAL.print(debug_motor_pose);
    DEBUG_SERIAL.print(" ");
  }
  DEBUG_SERIAL.println();

}

/***********************************************************************************************/

Also, can you elaborate on when to update the following structure elements?

.is_info_changed 
.is_completed

[v-r-a]

Found my mistake:

The packet building happens in dxl.syncWrite()step.

All all_info_xels_sw[i].p_data were pointing to myswdata which was getting overwritten! There is no memcopy happening here.

Sorted by the following redefinitions:

    uint8_t myswdata[NMOTOR][SW_ADDR_LEN];

    myswdata[i][0] = DXL_LOBYTE(home_pos_list[i]);  // low byte
    myswdata[i][1] = DXL_HIBYTE(home_pos_list[i]);  // high byte
    myswdata[i][2] = DXL_LOBYTE(home_vel_list[i]);  // low byte
    myswdata[i][3] = DXL_HIBYTE(home_vel_list[i]);  // high byte

    all_info_xels_sw[i].p_data = myswdata[i];