CONTROLLING Dynamixel MX106R Servo with Dynamixel shield

[BakaniumKgopolo]

Im struggling to get the Dynamixel servos to respond to the data sent via serial port, but when I manually generate random angles inside the loop the motor responds:

#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

//Please see eManual Control Table section of your DYNAMIXEL.
//This example is written for DYNAMIXEL AX & MX series with Protocol 1.0.
//For MX 2.0 with Protocol 2.0, refer to write_x.ino example.
#define CW_ANGLE_LIMIT_ADDR 6
#define CCW_ANGLE_LIMIT_ADDR 8
#define ANGLE_LIMIT_ADDR_LEN 2
#define OPERATING_MODE_ADDR_LEN 2
#define TORQUE_ENABLE_ADDR 24
#define TORQUE_ENABLE_ADDR_LEN 1
#define LED_ADDR 25
#define LED_ADDR_LEN 1
#define GOAL_POSITION_ADDR 30
#define GOAL_POSITION_ADDR_LEN 2
#define PRESENT_POSITION_ADDR 36
#define PRESENT_POSITION_ADDR_LEN 2
#define TIMEOUT 10 //default communication timeout 10ms

uint8_t turn_on = 1;
uint8_t turn_off = 0;

const uint8_t DXL_ID = 3;
const float DXL_PROTOCOL_VERSION = 1.0;

uint16_t goalPosition1 = 512;
uint16_t goalPosition2 = 1023;

DynamixelShield dxl;

void setup() {
// put your setup code here, to run once:

// For Uno, Nano, Mini, and Mega, use UART port of DYNAMIXEL Shield to debug.
DEBUG_SERIAL.begin(115200); //Set debugging port baudrate to 115200bps
while(!DEBUG_SERIAL); //Wait until the serial port for terminal is opened

// Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
dxl.begin(57600);
// Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);

// Turn off torque when configuring items in EEPROM area
if(dxl.write(DXL_ID, TORQUE_ENABLE_ADDR, (uint8_t*)&turn_off , TORQUE_ENABLE_ADDR_LEN, TIMEOUT))
DEBUG_SERIAL.println("DYNAMIXEL Torque off");
else
DEBUG_SERIAL.println("Error: Torque off failed");

// Set to Joint Mode
if(dxl.write(DXL_ID, CW_ANGLE_LIMIT_ADDR, (uint8_t*)&goalPosition1, ANGLE_LIMIT_ADDR_LEN, TIMEOUT)
&& dxl.write(DXL_ID, CCW_ANGLE_LIMIT_ADDR, (uint8_t*)&goalPosition2, ANGLE_LIMIT_ADDR_LEN, TIMEOUT))
DEBUG_SERIAL.println("Set operating mode");
else
DEBUG_SERIAL.println("Error: Set operating mode failed");

// Turn on torque
if(dxl.write(DXL_ID, TORQUE_ENABLE_ADDR, (uint8_t*)&turn_on, TORQUE_ENABLE_ADDR_LEN, TIMEOUT))
DEBUG_SERIAL.println("Torque on");
else
DEBUG_SERIAL.println("Error: Torque on failed");


dxl.write(DXL_ID, GOAL_POSITION_ADDR, (uint8_t*)&goalPosition1, GOAL_POSITION_ADDR_LEN, TIMEOUT);

}



void loop() {
// put your main code here, to run repeatedly:


int goalPosition1 = random(0, 181);

dxl.write(DXL_ID, GOAL_POSITION_ADDR, (uint8_t*)&goalPosition1, GOAL_POSITION_ADDR_LEN, TIMEOUT);
delay(500);


}

but when I try to get those numbers or continuously changing angles from my python code via serial port the motors dont respond even though Arduino shows that data is being received, the motors are connected to Dynamixel shield ,using 4pin RS485 cables ,which is stacked onto on an Arduino board:

/*******************************************************************************
* Copyright 2016 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.
*******************************************************************************/

#include <DynamixelShield.h>
#include <Dynamixel2Arduino.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

DynamixelShield dxl;

const uint8_t DXL_ID = 3;
const float DXL_PROTOCOL_VERSION = 1.0;


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

void setup() {
  // put your setup code here, to run once:
  
  // For Uno, Nano, Mini, and Mega, use UART port of DYNAMIXEL Shield to debug.
  DEBUG_SERIAL.begin(115200);

  Serial.begin(57600);
  // Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
  dxl.begin(57600);
  // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
  // Get DYNAMIXEL information
  dxl.ping(DXL_ID);

  // Turn off torque when configuring items in EEPROM area
  dxl.torqueOff(DXL_ID);
  dxl.setOperatingMode(DXL_ID, OP_POSITION);
  dxl.torqueOn(DXL_ID);
}

void loop() {
  // put your main code here, to run repeatedly:
   if (Serial.available()> 0) {
     
      int goalPosition1 = Serial.parseFloat();
  // Please refer to e-Manual(http://emanual.robotis.com/docs/en/parts/interface/dynamixel_shield/) for available range of value. 
  // Set Goal Position in RAW value
//  dxl.setGoalPosition(DXL_ID, 512);
  //delay(1000);
  // Print present position in raw value
  //DEBUG_SERIAL.print("Present Position(raw) : ");
  //DEBUG_SERIAL.println(dxl.getPresentPosition(DXL_ID));
  //delay(1000);

  // Set Goal Position in DEGREE value
  dxl.setGoalPosition(DXL_ID,goalPosition1, UNIT_DEGREE);
   }
  // Print present position in degree value
  //DEBUG_SERIAL.print("Present Position(degree) : ");
  //DEBUG_SERIAL.println(dxl.getPresentPosition(DXL_ID, UNIT_DEGREE));
  //delay(1000);
}

Please help, what may be the problem for regarding the serial communication?

[Jonathon]

On the Arduino Uno the serial port that you are using isn’t the one connected through the Arduino’s USB port. The available serial port is the one that’s made available through the 4 pin header on the top of the DYNAMXEL Shield. You’ll need to use an LN101 or comparable serial adapter to communicate over this port.

[BakaniumKgopolo]

Is it a must to use the 4 pin header to send the information from the Arduino’s USB port to the dynamixel servos for control? or is there another method to do it?

The Arduino shows that the data is arriving , so I need o get that data through the Dynamixel shield to the Servos

[roboteer]

@BakaniumKgopolo

First, you need to help me getting the big picture of what you are trying to do:

1. You are using an UNO R3 or below (basically an AVR controller) to control the MX-106R via the DXL Hardware Serial (Pin 0 and Pin 1) on the Dynamixel Shield.

2. You are also running another PC or SBC running Python code which figures out the Goal Position values that each MX-106 should be set at. And you want the MX-106 to pick up this type of information DIRECTLY (via SoftwareSerial (Pins 7 & 8) without having to use the Dynamixel2Arduino library, i.e. without having to use the Function dxl.setGoalPosition(DXL_ID,goalPosition1, UNIT_DEGREE)? Is this what you want to do? The short answer is - THIS IS NOT POSSIBLE.

What is possible is that you can use RC100 protocol from the PC/SBC running on Python to send an RC100 packet (which is not the same as the DXL packet used by the Function setGoalPosition()). This RC100 packet has information about the DXL’s ID and wanted Goal Position for this ID (one set per MX-106), which the UNO can pick up then decodes this RC100 packet and use this information to set up the proper call to Function setGoalPosition() to send a DXL packet to the actual MX-106s (one at a time) or you can prepare a SyncWrite process too.

By the way what kind of COM Port do you have on the PC/SBC side?

Some years back, I made this type of project between a PC running on Python and a CM-550 and reported it in this YouTube video. Please review it and let me know if that is what you want to do “concept-wise”. And then we’ll see what we can do to adapt these concepts to your specific conditions.

OpenCV (Python) + PySerial Project with CM-550

[roboteer]

@Jonathon

So far, I only read the specs for the UNO+DynamixelShield combo and I have not actually used this combo yet for my projects, so I may be mistaken here:

On the DXL Shield Pins 7 and 8 are UART Pins using 5V right? And the LN-101’s USART Pins are set at 3.3 V right? So shouldn’t we need a Logic Level Shifter module between the LN-101 and the DXL Shield?

[Jonathon]

The DYNAMIXEL Shield uses the hardware UART on pins 0 and 1 for DYNAMIXEL control. The LN-101 port connected to pins 7 and 8 the DYNAMIXEL Shield is actually a software serial port, rather than a hardware one since the board only has the one dedicated hardware UART. The board does contain the required level shifter for safe operation though.

[BakaniumKgopolo]

Yes what you said is true in 1.) and 2) but I want to use the setGoalPosition or any that you recommend that could work, and also I’m open to using the Dynamixel2Arduino library, How should I use the Dynamixel2Arduino library? Can I also do this without having to use the SoftwareSerial when using the Dynamixel2Arduino library.
Please note Im still a novice in using Dynamixel servos and the shield

Yes I’ll Iook at video and give you a review. Thanks

[BakaniumKgopolo]

hey man i tried using this code to check if the motor is receiving the command, I use this code to turn on the LED on the motor if serial.available > 0:

/*******************************************************************************
* Copyright 2016 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.
*******************************************************************************/

#include <Dynamixel2Arduino.h>
//#include <DynamixelShield.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

//Please see eManual Control Table section of your DYNAMIXEL.
//This example is written for DYNAMIXEL AX & MX series with Protocol 1.0.
//For MX 2.0 with Protocol 2.0, refer to write_x.ino example.
#define CW_ANGLE_LIMIT_ADDR         6
#define CCW_ANGLE_LIMIT_ADDR        8
#define ANGLE_LIMIT_ADDR_LEN        2
#define OPERATING_MODE_ADDR_LEN     2
#define TORQUE_ENABLE_ADDR          24
#define TORQUE_ENABLE_ADDR_LEN      1
#define LED_ADDR                    25
#define LED_ADDR_LEN                1
#define GOAL_POSITION_ADDR          30
#define GOAL_POSITION_ADDR_LEN      2
#define PRESENT_POSITION_ADDR       36
#define PRESENT_POSITION_ADDR_LEN   2
#define TIMEOUT 10    //default communication timeout 10ms

uint8_t turn_on = 1;
uint8_t turn_off = 0;

const uint8_t DXL_ID = 3;
const float DXL_PROTOCOL_VERSION = 1.0;

uint16_t goalPosition1 = 90;
//uint16_t goalPosition2 = 1023;

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);

void read_command() {
    // Set DXL_DIR_PIN to HIGH for transmission mode
    
    // Read incoming command from the serial port and parse it
    if (Serial.available() >= 4) {
        // Assuming the command format is [CMD, ARG1, ARG2, ARG3]
        // Read and store the command parameters
        dxl.write(DXL_ID, LED_ADDR, (uint8_t*)&turn_on, LED_ADDR_LEN, TIMEOUT);
        //delay(500); // Delay for LED visibility

        uint8_t cmd = Serial.read();
        uint16_t arg1 = Serial.read();
        uint16_t arg2 = Serial.read();
        uint16_t arg3 = Serial.read();


        digitalWrite(DXL_DIR_PIN, HIGH);

        // Process the command as needed
        // For simplicity, you can directly actuate the Dynamixel motors here
        // Example: Set goal positions
        if (cmd == 1) {
            uint16_t goalPosition1 = arg1;
            //goalPosition2 = arg2;
        }
    }
}

void execute_command() {
    // Actuate the Dynamixel motors based on the received command
    // For simplicity, this function can directly set goal positions
    dxl.write(DXL_ID, GOAL_POSITION_ADDR, (uint8_t*)&goalPosition1, GOAL_POSITION_ADDR_LEN, TIMEOUT);
    delay(1000); // Delay for stability

    // Example: Turn on LED
    //dxl.write(DXL_ID, LED_ADDR, (uint8_t*)&turn_on, LED_ADDR_LEN, TIMEOUT);
    //delay(500); // Delay for LED visibility

    // Example: Set a different goal position
   // dxl.write(DXL_ID, GOAL_POSITION_ADDR, (uint8_t*)&goalPosition2, GOAL_POSITION_ADDR_LEN, TIMEOUT);
    //delay(1000); // Delay for stability

    // Example: Turn off LED
    //dxl.write(DXL_ID, LED_ADDR, (uint8_t*)&turn_off, LED_ADDR_LEN, TIMEOUT);
    //delay(500); // Delay for LED visibility
}

void setup() {
  // put your setup code here, to run once:
  pinMode(DXL_DIR_PIN, OUTPUT); // Set DXL_DIR_PIN as an output pin
  // For Uno, Nano, Mini, and Mega, use UART port of DYNAMIXEL Shield to debug.
  DEBUG_SERIAL.begin(115200);   //Set debugging port baudrate to 115200bps
  while(!DEBUG_SERIAL);         //Wait until the serial port for terminal is opened
  
  // Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
  dxl.begin(57600);
  // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);

  // Turn off torque when configuring items in EEPROM area
  if(dxl.write(DXL_ID, TORQUE_ENABLE_ADDR, (uint8_t*)&turn_off , TORQUE_ENABLE_ADDR_LEN, TIMEOUT))
    DEBUG_SERIAL.println("DYNAMIXEL Torque off");
  else
    DEBUG_SERIAL.println("Error: Torque off failed");

  // Set to Joint Mode
  if(dxl.write(DXL_ID, CW_ANGLE_LIMIT_ADDR, (uint8_t*)&goalPosition1, ANGLE_LIMIT_ADDR_LEN, TIMEOUT))
    DEBUG_SERIAL.println("Set operating mode");
  else
    DEBUG_SERIAL.println("Error: Set operating mode failed");

  // Turn on torque
  if(dxl.write(DXL_ID, TORQUE_ENABLE_ADDR, (uint8_t*)&turn_on, TORQUE_ENABLE_ADDR_LEN, TIMEOUT))
    DEBUG_SERIAL.println("Torque on");
  else
    DEBUG_SERIAL.println("Error: Torque on failed");
}

void loop() {
    // Read incoming command and execute it
    read_command();
    execute_command();
}

and the LED is not turning on, but when I put : dxl.write(DXL_ID, LED_ADDR, (uint8_t*)&turn_on, LED_ADDR_LEN, TIMEOUT); outside the if(Serial.available>0) the LED turns on

[Jonathon]

The serial port that you are trying to read from is the same port that you’re using to control the DYNAMIXEL actuators.

There’s only one hardware UART available on the Arduino Uno board, so you won’t be able to take serial input over the USB port at the same time as you are controlling the DYNAMIXEL actuators.

[roboteer]

I agree with @Jonathon. For your needs of DXL control and inter-controller communication at the same time you have to forget about the UNO R3 and earlier. You’ll need at least 2 hardware UARTs. Unfortunately the UNO R4 is not fully operational at this point in time. You’ll have to look at MKR Zero or OpenRB-150 but they only have X3P connectors. So you’ll have to add a DXL communication bridge also to control your MX-106R.

Introduction

[BakaniumKgopolo]

I thought the Dynamixel shield has its own serial too, so cant we write the data to that serial after reading it from the USB port?

Thank you by the way

[roboteer]

It is only a software UART linked to Pins 7 & 8 and connected to that 4-pin connector for BT210/410 or LN-101 as Jonathon mentioned before. It may not have the performance needed

[BakaniumKgopolo]

Ok thanks man, I hope I’ll manage with information you’ve provided

By the way, One last quesion, Is it not possible to just control the servos using arduino without the Shield, can I not write the functions needed and then make them into a header file so that I can call it in Arduino to get the motors runnning

[roboteer]

No because the DXLs communicate at half-duplex while the UART is at full duplex. The Dir-pin D2 is used by robotis software to make the UART behaves like a half-duplex. Also there are specific circuitry on the Shield to do that job too.

As you are using Python on a host computer and if your robot can be tied to your host computer via USB cable. Then there is another different option whereas you use the U2D2 and DXL SDK to control the DXLs from the PC directly.

U2D2 Power Hub Board Set

U2D2

The DXL SDK has a Python version.

[BakaniumKgopolo]

Hello. Would it be possible to use the Arduino Due which has multiple hardware serial ports to try controlling the motors. Using a serial to usb adapter to receive incremental angles from the python code and write them to the Arduino(connecting it to one of the RX-17 and TX-16 pins on the Arduino), process them and then try controlling the motors using the dynamixel shield via RX-0 and TX-1.

[roboteer]

Personally I have not used the Due, so I can’t help you much here.

According to this Arduino post, the Arduino Due is not directly compatible with the Dynamixel Shield, because the Due uses 3.3V for its I/O pins and the Dynamixel Shield uses 5V for its I/O pins.

Issues with Dynamixel AX-18A and Arduino DUE board

related hardware and servos: AX-18A Dynamixel Shield (not MKR series) Arduino DUE software: Arduino IDE (ver 1.8.19) Mac I am working on a project that uses Dynamixel AX-18A and I want to use an Arduino Due board with a Dynamixel shield. While I am able to upload the code without errors or complications, the motor is unresponsive. The code I initially tried uploading to ensure everything was working as predicted with the velocity-mode and position-mode examples provided by the Dynamixel2Ar…

In the past I have used this Logic Level Shifter to interface between 3.3 V and 5 V and it worked great for me.

Pololu - Logic Level Shifter, 4-Channel, Bidirectional