DYNAMIXEL servos

Dynamixel motor control (MKR Zero)

• Product DYNAMIXEL Shield for Arduino MKR Series: https://store.arduino.cc/products/dynamixel-shield-for-arduino-mkr-series?srsltid=AfmBOoq7ptPLfwzykpUecfmQo3vii_8N3padSzJVAQ7RiUqJHuECxUOW
• Product Dynamixel XL-320 Servo: https://store.arduino.cc/products/dynamixel-xl-320

Dependencies

• Install the library DynamixelShield in the Arduino IDE
  • GitHub: https://github.com/ROBOTIS-GIT/DynamixelShield
  • Documentation: https://emanual.robotis.com/docs/en/parts/interface/mkr_shield/

Description

This script is based on the example code provided with the DynamixelShield Arduino library. Some modifications were required to make it work with the MKR Zero board, the MKR shield, and the Dynamixel motor used in this setup.

The main changes are:

• The shield communicates through Serial1 on the MKR Zero.
• The port baudrate had to be set to 1,000,000 because the motor did not respond when using the default baudrate in the original example.
• Debug output is sent to the Arduino Serial Monitor.

The script first checks if the motor is connected using ping(). If the motor responds, torque is enabled and the operating mode is set to position control.

In the loop the motor is commanded to move to different positions. The position is set once using a raw value and once using degrees, and the current motor position is printed to the Serial Monitor.

A raw position value corresponds to the internal encoder value of the motor, while UNIT_DEGREE allows the position to be set using degrees.

Source code

/*******************************************************************************
* ROBOTIS Example - Fixed for MKR Zero
*******************************************************************************/

#include <DynamixelShield.h>

#define DXL_SERIAL Serial1
#define DEBUG_SERIAL Serial

const uint8_t DXL_ID = 1;
const float DXL_PROTOCOL_VERSION = 2.0;

DynamixelShield dxl(DXL_SERIAL);

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

void setup() {

  DEBUG_SERIAL.begin(115200);
  while(!DEBUG_SERIAL);

  // Set Port baudrate (motor responded at 1,000,000)
  dxl.begin(1000000);

  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);

  DEBUG_SERIAL.println("Pinging motor...");

  if(dxl.ping(DXL_ID))
    DEBUG_SERIAL.println("Motor connected!");
  else
    DEBUG_SERIAL.println("Motor NOT detected!");

  dxl.torqueOff(DXL_ID);
  dxl.setOperatingMode(DXL_ID, OP_POSITION);
  dxl.torqueOn(DXL_ID);
}

void loop() {

  dxl.setGoalPosition(DXL_ID, 512);
  delay(1000);

  DEBUG_SERIAL.print("Present Position(raw) : ");
  DEBUG_SERIAL.println(dxl.getPresentPosition(DXL_ID));

  delay(1000);

  dxl.setGoalPosition(DXL_ID, 90, UNIT_DEGREE);
  delay(1000);

  DEBUG_SERIAL.print("Present Position(degree) : ");
  DEBUG_SERIAL.println(dxl.getPresentPosition(DXL_ID, UNIT_DEGREE));

  delay(2000);
}

Alternative simple test

The following script is a simpler test program that only moves the motor between two positions (0° and 90°). This can be useful to quickly verify that the motor and shield are working correctly.

#include <DynamixelShield.h>

#define DXL_SERIAL Serial1
#define DEBUG_SERIAL Serial

DynamixelShield dxl(DXL_SERIAL);

const uint8_t ID = 1;

void setup() {

  DEBUG_SERIAL.begin(115200);
  while(!DEBUG_SERIAL);

  dxl.begin(1000000);
  dxl.setPortProtocolVersion(2.0);

  DEBUG_SERIAL.println("Motor connected!");

  dxl.torqueOff(ID);
  dxl.setOperatingMode(ID, OP_POSITION);
  dxl.torqueOn(ID);
}

void loop() {

  DEBUG_SERIAL.println("Move to 90°");
  dxl.setGoalPosition(ID, 90, UNIT_DEGREE);
  delay(3000);

  DEBUG_SERIAL.println("Move to 0°");
  dxl.setGoalPosition(ID, 0, UNIT_DEGREE);
  delay(3000);

}

Find motor ID

If the motor ID is unknown, the following script can be used to scan for connected Dynamixel motors. The script tries to ping motors with ID values between 0 and 19 and prints the detected ID in the Serial Monitor.

This is useful if the motor does not respond to commands because the wrong ID is used in the control script.

#include <DynamixelShield.h>

#define DXL_SERIAL Serial1
#define DEBUG_SERIAL Serial

DynamixelShield dxl(DXL_SERIAL);

void setup() {

  DEBUG_SERIAL.begin(115200);
  while(!DEBUG_SERIAL);

  dxl.begin(1000000);
  dxl.setPortProtocolVersion(2.0);

  DEBUG_SERIAL.println("Scanning for motors...");

  for(int id=0; id<20; id++){
    if(dxl.ping(id)){
      DEBUG_SERIAL.print("Motor found with ID: ");
      DEBUG_SERIAL.println(id);
    }
  }
}

void loop(){}

Troubleshooting

Common issues when working with Dynamixel motors:

• The motor does not respond → check that the correct baudrate is used.
• The motor is not detected → verify the motor ID using the script above.
• No movement → make sure torque is enabled.
• Communication problems → confirm that the shield switch is set to TTL.
• Motor not powered → check the external power supply connected to the shield.