Author: Norberto Torres-Reyes Email: [email protected]
Date: Last modified on 04/18/19
Keywords: kinematics, planar, robotic arm, 2 link, lego, NXC, NXT, mindstorm, dynamixel,

This tutorial is for anyone who wishes to control Dynamixel RX-28 servos using a LEGO NXT Brick. This tutorial will also reinforce theoretical knowledge about serial communication and servo control. Readers of this tutorial are recommended to have the following background and interests:

*Previous experience with LEGO Mindstorm/NXT
*Matlab and/or C-based programming experience
*Some knowledge in serial communcation
*Experience with using servos
The rest of the tutorial is presented as follows:

• Parts List
• Theoretical Background
• Preliminaries
• NXC Code
• Running and Analysis
• Conclusions

A few parts for controlling the Dynamixels will be needed along with an NXT Brick as well as some parts used to communicate between the NXT Brick and the Dynamixel.

Dynamixel RX28
https://www.trossenrobotics.com/dynamixel-rx-28-robot-actuator.aspx

Lego BreadBoard Adapter
http://www.mindsensors.com/ev3-and-nxt/58-breadboard-connector-kit-for-nxt-or-ev3

SMPS2Dynamixel
http://www.robotis.us/smps2dynamixel/

(Optional) HiLetgo USB Logic Analyzer
https://www.amazon.com/gp/product/B077LSG5P2/ref=ppx_yo_dt_b_asin_title_o00_s00?ie=UTF8&psc=1


Other Parts Needed:

• NXT Cable

• Dynamixel Connector Cables

• USB A-to-C cable

• 12V 2-Amp Power Supply

The theoretical background can be subdivided into two parts:

• RS-485 Serial Communication
• Dynamixel Communication

RS-485 Serial Communication

RS-485 is an EIA/TIA standard for serial communication. This standard is an improvement on the older RS-232 which will not be discussed here. Several improvements include faster speeds, the ability to transmit data at a length of about 4000 feet (1200 meters), the ability to connect up to 32 devices, and good noise immunity. The data rates depend on the length of the cable and can range from 100 kbits/sec at 1200 meters to 10 Mbits/sec at 10 meters.

The good noise immunity comes from the differential signal between the two data lines. Using this method, any noise or ground level shifts can be avoided, reducing the chance of data corruption.



In this tutorial we will be using half-duplex communication although full-duplex can also be used. Half-duplex communication consists of sending and receiving data in only one direction at a time. Specifically, a serial communication protocol known as UART (Universal Asynchronous Receiver/Transmitter) is used. This method is called asynchronous because instead of having a clock signal to synchronize the transmitter and receiver, a preset data rate is agreed upon by both devices. The image above shows how a signal would look like for transmitting the hexadecimal value 0xCB (decimal 203, binary 11001011).

The signal consists of several different sets of bits. The first bit is the start bit “1” and signifies the beginning of an incoming signal. The subsequent 8 bits are the actual data bits; In our case “11001011”. An optional parity bit can go next, otherwise a stop bit “1” is used to signify the end of the transmission.

Dynamixel Communication

Communication with the Dynamixel RX-28 servos require several parameters. A tutorial about NXT communication with the RX-28 servos can be found here. The RX-28 datasheet is also very useful reference that should be looked at as well.

Serial communication with a Dynamixel consists of sending and receiving packets. There are two kinds of packets, an Instruction packet and a Status packet. The Instruction packet sends information to the Dynamixel and the Status packet sends information to the controller from the Dynamixel. Each packet has it's own different format but only the Instruction packet will be looked at in this tutorial.



0xFF: These two bytes are used to signal an incoming packet.
ID: This byte is the Dynamixel ID, which can be an individual ID or if 0xFE is used, the packet is sent to all Dynamixels.
LENGTH: This is the message length which is calculated by the number of parameters(N) + 2.
INSTRUCTION: The figure below shows the different set of instructions that can be sent, in this case it's “Write Data”.

PARAMETERS: The first parameter is the specific command, the remaining parameters are the bytes associated with that parameter. Some parameters require two bytes for one value, for example, for the base 10 number 950 the low (L) byte would be “10110110” and the high (H) byte would be “00000011”. The datasheet has the complete list of all parameters.
CHECKSUM: The checksum is determined by ~(ID + Length + Instruction + Parameter1 + … + ParameterN). The “~” symbol is the Not Bit Operator.

Writing Dynamixel ID's
Some simple tests can first be completed with the NXT and the Dynamixel servos before running the finished program. Occasionally, it may be required to change the ID's of the servos. The factory default ID for the RX-28's is 0x01. It is necessary to provide separate ID's for each servo or else communication with multiple servos will not work. The following code provides a simple way to change a Dynamixel ID with the NXT. The header file “Dynamixel2.h” must be located in the same folder as the code

//Norberto Torres-Reyes
//04/06/2019
//Writes ID to a Dynamixel RX-28 with NXT

#include "Dynamixel2.h"
#define ID 90  //ID that you want to write to servo



task main()
{
 UseRS485();
 RS485Enable();
 RS485Uart(HS_BAUD_57600, HS_MODE_8N1);    //57600 baud, 8bit, 1stop, no parity
 Wait(100);
 
 while(!ButtonPressed(BTNCENTER, FALSE))   //display msg until center button pressed
 {
  TextOut(0,LCD_LINE1, "Press Orange" );
  TextOut(0,LCD_LINE2, "Button" );
  TextOut(0,LCD_LINE3, "To Begin:" );
  Wait(10);
 }
 
 setID(ID);
 Wait(100);
 
 ClearScreen();
 TextOut(0,LCD_LINE3, FormatNum("ID NEW: %d" ,ID));
 Wait(2000);
 }

Each Dynamixel ID can be changed individually. The ID's can range from 0x01 to 0xFD, where 0xFE is a universal ID that will communicate to all servos at once.

Testing Rotation

Each servo should be tested for proper operation. The following code will rotate a servo at 100 degree intervals from 0 to 300 degrees by pushing the left and right arrow buttons. Once again, “Dynamixel2.h” must be included in the same folder when running the code.

//Dynamixel RX-28 Servo Control
//Created by: Norberto Torres-Reyes
//Date: 03/30/19
//Version 1.0
//Dynamixel2.h modified from
//http://www.daslhub.org/unlv/wiki/doku.php?id=lego_nxt_dynamixel&s[]=dynamixel

#include "Dynamixel2.h"
#define ID 0XFE
                   //Dynamixel ID
int angleRaw = 0;           //Raw 10-bit value for servo angle
float angleRes = 1023/300;   //10 bit resolution over 300 degrees of movement
int angleStep = 1023*100/300;  //10-degree step increment
float angleVal = 0;    //degree value converted from raw
int rotateSpeed = 50;
float offsetVal = 0;

task main()
{
 UseRS485();
 RS485Enable();
 RS485Uart(HS_BAUD_57600, HS_MODE_8N1);    //57600 baud, 8bit, 1stop, no parity
 Wait(100);

 while(!ButtonPressed(BTNCENTER, FALSE))   //display msg until center button pressed
 {
  TextOut(0,LCD_LINE1, "Press Orange" );
  TextOut(0,LCD_LINE2, "Button" );
  TextOut(0,LCD_LINE3, "To Begin:" );
  Wait(10);
 }
 setMode(ID,SERVO,0,1023);        //servo mode with angle limits of
                                  //0 to 300(1023).
 while(true){
 ClearLine(LCD_LINE5);
 TextOut(0,LCD_LINE1,"Angle Control");
 TextOut(0,LCD_LINE3,"Right CW");
 TextOut(0,LCD_LINE4,"Left CCW");
 angleVal = (angleRaw/angleRes - offsetVal);      //maps 0-1023 to 0-300 with offset
 TextOut(0,LCD_LINE5,NumToStr((angleVal)));  //displays angle on screen
 Wait(200);

 if(ButtonPressed(BTNRIGHT,FALSE))      //increments angle CW by 10 degrees
 {                                      //if right button is pressed
  angleRaw = (angleRaw + angleStep);
  if(angleRaw >= 1023){                //limits max CW value to 1023
    angleRaw = 1023;
    }
  servo(ID,angleRaw,rotateSpeed);
  angleVal = (angleRaw/angleRes - offsetVal);
  TextOut(0,LCD_LINE5, NumToStr((angleVal)));
  Wait(200);
 }
 
  if(ButtonPressed(BTNLEFT,FALSE))    //increments angle CCW by 10 degrees
 {                                    //if left button is pressed
  angleRaw = (angleRaw - angleStep);
  if(angleRaw <= 0){                 //Limits CCW min value to 0
    angleRaw = 0;
    }
  servo(ID,angleRaw,rotateSpeed);
  angleVal = (angleRaw/angleRes - offsetVal);
  TextOut(0,LCD_LINE5, NumToStr((angleVal)));
  Wait(200);
 }
 }
}

The NXC code files as well as the header file is included below in a .zip file:
nxt_code.zip
Controlling Multiple Dynamixels
When controlling multiple Dynamixels, only a few more steps are required. First, each Dynamixel should have its own ID as described in the preliminary section.

#define ID1 90
#define ID2 91

Next, each Dynamixel should be initialized with the “setMode()” function.

setMode(ID1, SERVO, minAngle1, maxAngle1);
setMode(ID2, SERVO, minAngle2, maxAngle2);

Finally, when using the “servo()” function, make sure the correct ID for the correct Dynamixel is used.
The following code is an example of how two Dynamixels can be controlled by the NXT:

//2_Dynamixel_Control
//By: Norberto Torres-Reyes
//Date: 04/18/19
//Controls two different Dynamixels using the left and right buttons.

#include "Dynamixel2.h"
#define ID1 90
#define ID2 91
                   //Dynamixel ID
int angleRaw = 0;           //Raw 10-bit value for servo angle
float angleRes = 1023/300;   //10 bit resolution over 300 degrees of movement
int angleStep = 1023*100/300;  //10-degree step increment
float angleVal = 0;    //degree value converted from raw
int rotateSpeed = 50;
float offsetVal = 0;

task main()
{
 UseRS485();
 RS485Enable();
 RS485Uart(HS_BAUD_57600, HS_MODE_8N1);    //57600 baud, 8bit, 1stop, no parity
 Wait(100);

 while(!ButtonPressed(BTNCENTER, FALSE))   //display msg until center button pressed
 {
  TextOut(0,LCD_LINE1, "Press Orange" );
  TextOut(0,LCD_LINE2, "Button" );
  TextOut(0,LCD_LINE3, "To Begin:" );
  Wait(10);
 }
 setMode(ID1,SERVO,0,1023);        //servo mode with angle limits of
 setMode(ID2,SERVO,0,1023);        //0 to 300(1023).

 while(true){
 ClearLine(LCD_LINE5);
 TextOut(0,LCD_LINE1,"Angle Control");
 TextOut(0,LCD_LINE3,"Right CW");
 TextOut(0,LCD_LINE4,"Left CCW");
 angleVal = (angleRaw/angleRes - offsetVal);      //maps 0-1023 to 0-300 with offset
 TextOut(0,LCD_LINE5,NumToStr((angleVal)));  //displays angle on screen
 Wait(200);

 if(ButtonPressed(BTNRIGHT,FALSE))      //increments angle CW by 10 degrees
 {                                      //if right button is pressed
  angleRaw = (angleRaw + angleStep);
  if(angleRaw >= 1023){                //limits max CW value to 1023
    angleRaw = 0;
    }
  servo(ID1,angleRaw,rotateSpeed);
  angleVal = (angleRaw/angleRes - offsetVal);
  TextOut(0,LCD_LINE5, NumToStr((angleVal)));
  Wait(200);
 }
 
  if(ButtonPressed(BTNLEFT,FALSE))    //increments angle CCW by 10 degrees
 {                                    //if left button is pressed
  angleRaw = (angleRaw + angleStep);
  if(angleRaw >= 1023){                 //Limits CCW min value to 0
    angleRaw = 0;
    }
  servo(ID2,angleRaw,rotateSpeed);
  angleVal = (angleRaw/angleRes - offsetVal);
  TextOut(0,LCD_LINE5, NumToStr((angleVal)));
  Wait(200);
 }
 }
}

Running the code is straight forward. Ensure that the NXT is connected to the computer via the USB cable and the "Bricx Command Center" is running. Make sure that the code can compile properly without any errors. Connect the NXT cable to the “S4” port and connect the other end to breadboard adapter.
For a single servo control, connect one of the Dynamixel cables to the power supply and another to the breadboard adapter. If using two or more, one cable goes from the breadboard adapter to the first Dynamixel. Another cable is connected from the first servo to the next servo and so on. The last servo connects to the power supply. The 12v 2 amp adapter is then connected to the power converter.
Video:

Logic Analyzer (OPTIONAL)
The logic analyzer can be connected to the +5V, GND, SDL, SDA pins on the breadboard adapter before connecting to a dynamixel. Instruction on using the logic analyzer with your computer can be found here. When running the single servo control code, pressing the left or right arrow should give you a signal similar to the one shown below:

The differential nature of the signal can be seen from data lines D0 and D1. The logic analyzer software will automatically decode the signal received.
This tool is useful in debugging any errors that may arise when communicating with the Dynamixels that may otherwise be impossible to catch.

In Conclusion, this tutorial has provided a brief introduction into RX-485 serial communication, UART communication protocols, Dynamixel communication, and NXT control of single and multiple Dynamixels. The NXT code provided allows for ID reconfiguration of a Dynamixel and accurate angle control of multiple Dynamixels. The hope is that the reader can use the information and examples provided to apply to more advanced tasks. Such tasks can be combined with LEGO structures to create various useful mechanisms. It is also possible to use the information in this tutorial and expand the Dynamixel controls for use with other micro-controllers.