Sync Read Write Position Example Not Responding

[IchalSinjai]

I try to sync write position on AX-18 with opencm 9.04 using example. And nothing happen.

Serial Monitor :

=======================================================

Sync Instruction Test : 576
[SyncWrite] Success
ID: 1
Goal Velocity: 85
ID: 2
Goal Velocity: 185

[SyncRead] Fail, Lib error code: 2

[ROBOTIS-Will]

Hi @IchalSinjai

The AX series use Protocol 1.0 that does not support Sync Read instruction.
https://emanual.robotis.com/docs/en/dxl/protocol1/#instruction

Please note that the example is written based on X series.
Thank you.

[IchalSinjai]

Ax series did not support sync write instruction too?

[IchalSinjai]

I try this example :
https://emanual.robotis.com/docs/en/popup/arduino_api/syncWrite/

#include <Dynamixel2Arduino.h>
#define DXL_SERIAL Serial1
const uint8_t RS485_DIR_PIN = 28; //DYNAMIXEL Shield

ParamForSyncWriteInst_t sync_write_param;
const int DXL_DIR_PIN = 2;
int32_t velocity = 0;

Dynamixel2Arduino dxl(DXL_SERIAL, RS485_DIR_PIN);

void setup() {
dxl.begin(1000000);
dxl.scan();

sync_write_param.addr = 104; //Goal Velocity of DYNAMIXEL-X series
sync_write_param.length = 4;
sync_write_param.xel[0].id = 1;
sync_write_param.xel[1].id = 3;
sync_write_param.id_count = 2;

dxl.torqueOff(1);
dxl.setOperatingMode(1, OP_VELOCITY);
dxl.torqueOn(1);
dxl.torqueOff(3);
dxl.setOperatingMode(3, OP_VELOCITY);
dxl.torqueOn(3);

memcpy(sync_write_param.xel[0].data, 200, sizeof(velocity)); // Error In This Line
memcpy(sync_write_param.xel[1].data, -200, sizeof(velocity)); // Error In This Line

dxl.syncWrite(sync_write_param);
}

void loop() {

}

And I got this :

C:\Users\Ichal\AppData\Local\Temp\arduino_modified_sketch_597743\sketch_jun07a.ino: In function 'void setup()':
sketch_jun07a:28:61: error: invalid conversion from 'int' to 'const void*' [-fpermissive]
memcpy(sync_write_param.xel[0].data, 200, sizeof(velocity));
^
In file included from c:\users\ichal\appdata\local\arduino15\packages\opencm904\tools\opencm_gcc\5.4.0-2016q2\arm-none-eabi\include\stdlib.h:11:0,
from C:\Users\Ichal\AppData\Local\Arduino15\packages\OpenCM904\hardware\OpenCM904\1.5.1\cores\arduino/Arduino.h:24,
from sketch\sketch_jun07a.ino.cpp:1:
c:\users\ichal\appdata\local\arduino15\packages\opencm904\tools\opencm_gcc\5.4.0-2016q2\arm-none-eabi\include\string.h:23:8: note: initializing argument 2 of 'void* memcpy(void*, const void*, size_t)'
_PTR _EXFUN(memcpy,(_PTR __restrict, const _PTR __restrict, size_t));
^
sketch_jun07a:29:62: error: invalid conversion from 'int' to 'const void*' [-fpermissive]
memcpy(sync_write_param.xel[1].data, -200, sizeof(velocity));
^
In file included from c:\users\ichal\appdata\local\arduino15\packages\opencm904\tools\opencm_gcc\5.4.0-2016q2\arm-none-eabi\include\stdlib.h:11:0,
from C:\Users\Ichal\AppData\Local\Arduino15\packages\OpenCM904\hardware\OpenCM904\1.5.1\cores\arduino/Arduino.h:24,
from sketch\sketch_jun07a.ino.cpp:1:
c:\users\ichal\appdata\local\arduino15\packages\opencm904\tools\opencm_gcc\5.4.0-2016q2\arm-none-eabi\include\string.h:23:8: note: initializing argument 2 of 'void* memcpy(void*, const void*, size_t)'
_PTR _EXFUN(memcpy,(_PTR __restrict, const _PTR __restrict, size_t));
^
exit status 1
invalid conversion from 'int' to 'const void*' [-fpermissive]

[ROBOTIS-David]

@IchalSinjai

1. According to DYNAMIXEL Protocol 1.0
   DYNAMIXEL AX series can interact with the Sync Write Instruction.(Sync Read not be supported for DYNAMIXEL Protocol 1.0 rule)

2. Checkout the Control Table of AX series,

• Seems that you are assigining the wrong control table in your code for operating DYNAMIXEL-AX series.
• The address '104' is the only addr for DYNAMIXEL-X series, unlikely Moving Speed (Same as Goal Velocity, but just different name) is addr 32. And its assigned space is 2 Bytes, not 4 Bytes. Guessing you were using an example 'DEPRECATED_sync_read_write.ino' designed for DYNAMIXEL-X Series. Be aware that the control table has different data field from DYNAMIXEL AX/MX series.

Highly recommend visiting our Community page and raise a discussion ticket for help, where our power users hang around.

[ROBOTIS-Will]

Hi @IchalSinjai

I'm sorry about the delayed reply.
The API example is to explain the general usage of the function and needs to be updated.
Could you try with the code below that I tested with ID 1 & 3 on OpenCM9.04 + 485EXP board?
I modified the sync_read_write_position example in the DYNAMIXEL2Arduino library.

// Copyright 2021 ROBOTIS CO., LTD.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

// Example Environment
//
// - DYNAMIXEL: X series
//              ID = 1 & 2, Baudrate = 57600bps, DYNAMIXEL Protocol 2.0
// - Controller: Arduino MKR ZERO
//               DYNAMIXEL Shield for Arduino MKR
// - https://emanual.robotis.com/docs/en/parts/interface/mkr_shield/
//
// Author: David Park

#include <Dynamixel2Arduino.h>

// Please modify it to suit your hardware.
#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield
  #include <SoftwareSerial.h>
  SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX
  #define DXL_SERIAL   Serial
  #define DEBUG_SERIAL soft_serial
  const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield
  #define DXL_SERIAL   Serial
  #define DEBUG_SERIAL SerialUSB
  const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield
  #define DXL_SERIAL   Serial1
  #define DEBUG_SERIAL SerialUSB
  const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit.
  #define DXL_SERIAL   Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board)
  #define DEBUG_SERIAL Serial
  const int DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board)
#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit.
  // For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it.
  // Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78
  #define DXL_SERIAL   Serial3
  #define DEBUG_SERIAL Serial
  const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN.    
#elif defined(ARDUINO_OpenRB)  // When using OpenRB-150
  //OpenRB does not require the DIR control pin.
  #define DXL_SERIAL Serial1
  #define DEBUG_SERIAL Serial
  const int DXL_DIR_PIN = -1;
#else // Other boards when using DynamixelShield
  #define DXL_SERIAL   Serial1
  #define DEBUG_SERIAL Serial
  const int DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
#endif

/* 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;

  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;
*/

const uint8_t BROADCAST_ID = 254;
const float DYNAMIXEL_PROTOCOL_VERSION = 1.0;
const uint8_t DXL_ID_CNT = 2;
const uint8_t DXL_ID_LIST[DXL_ID_CNT] = {1, 3};
const uint16_t user_pkt_buf_cap = 128;
uint8_t user_pkt_buf[user_pkt_buf_cap];

// Starting address of the Data to write; Moving Speed = 32
const uint16_t SW_START_ADDR = 32;
// Length of the Data to write; Length of Moving Speed of AX series is 2 byte
const uint16_t SW_ADDR_LEN = 2;

typedef struct sw_data{
  int16_t moving_speed;
} __attribute__((packed)) sw_data_t;

sw_data_t sw_data[DXL_ID_CNT];
DYNAMIXEL::InfoSyncWriteInst_t sw_infos;
DYNAMIXEL::XELInfoSyncWrite_t info_xels_sw[DXL_ID_CNT];

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);

//This namespace is required to use DYNAMIXEL Control table item name definitions
using namespace ControlTableItem;

int16_t moving_speed[2] = {50, 1075};
uint8_t moving_speed_index = 0;

void setup() {
  // put your setup code here, to run once:
  uint8_t i;
  pinMode(LED_BUILTIN, OUTPUT);
  DEBUG_SERIAL.begin(57600);
  dxl.begin(1000000);
  dxl.setPortProtocolVersion(DYNAMIXEL_PROTOCOL_VERSION);

// Prepare the SyncRead structure
  for(i = 0; i < DXL_ID_CNT; i++){
    dxl.torqueOff(DXL_ID_LIST[i]);
    dxl.setOperatingMode(DXL_ID_LIST[i], OP_VELOCITY);
  }
  dxl.torqueOn(BROADCAST_ID);

  // Fill the members of structure to syncWrite using internal packet buffer
  sw_infos.packet.p_buf = nullptr;
  sw_infos.packet.is_completed = false;
  sw_infos.addr = SW_START_ADDR;
  sw_infos.addr_length = SW_ADDR_LEN;
  sw_infos.p_xels = info_xels_sw;
  sw_infos.xel_count = 0;

  for(i = 0; i < DXL_ID_CNT; i++){
    info_xels_sw[i].id = DXL_ID_LIST[i];
    info_xels_sw[i].p_data = (uint8_t*)&sw_data[i].moving_speed;
    sw_infos.xel_count++;
  }
  sw_infos.is_info_changed = true;
}

void loop() {
  // put your main code here, to run repeatedly:
  static uint32_t try_count = 0;
  uint8_t i, recv_cnt;
  
  // Insert a new Goal Position to the SyncWrite Packet
  for(i = 0; i < DXL_ID_CNT; i++){
    sw_data[i].moving_speed = moving_speed[moving_speed_index];
  }

  // Update the SyncWrite packet status
  sw_infos.is_info_changed = true;

  DEBUG_SERIAL.print("\n>>>>>> Sync Instruction Test : ");
  DEBUG_SERIAL.println(try_count++);
  
  // Build a SyncWrite Packet and transmit to DYNAMIXEL  
  if(dxl.syncWrite(&sw_infos) == true){
    DEBUG_SERIAL.println("[SyncWrite] Success");
    for(i = 0; i<sw_infos.xel_count; i++){
      DEBUG_SERIAL.print("  ID: ");DEBUG_SERIAL.println(sw_infos.p_xels[i].id);
      DEBUG_SERIAL.print("\t Moving Speed: ");DEBUG_SERIAL.println(sw_data[i].moving_speed);
    }
    if(moving_speed_index == 0)
      moving_speed_index = 1;
    else
      moving_speed_index = 0;
  } else {
    DEBUG_SERIAL.print("[SyncWrite] Fail, Lib error code: ");
    DEBUG_SERIAL.print(dxl.getLastLibErrCode());
  }

  DEBUG_SERIAL.println("=======================================================");

  digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
  delay(3000);
}