DASL Wiki

This page is read only. You can view the source, but not change it. Ask your administrator if you think this is wrong.
====== Dynamixel XL-320 Control with LEGO NXT ======

**Author:** Norberto Torres-Reyes Email: torresre@unlv.nevada.edu 
\\
**Date:** Last modified on 05/16/19
\\
**Keywords:** kinematics, planar, robotic arm, 2 link, lego, NXC, NXT, mindstorm, dynamixel,
\\

=====Motivation and Audience=====



This tutorial is for anyone who wishes to control Dynamixel XL-320 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:

<fc blue>
  *Previous experience with LEGO Mindstorm/NXT
\\
  *Matlab and/or C-based programming experience
\\
  *Some knowledge in serial communcation
\\
  *Experience with using servos
</fc>
\\
The rest of the tutorial is presented as follows:
  *[[lego_dynamixel_control#parts list|Parts List]]
  *[[lego_dynamixel_control#theoretical background|Theoretical Background]]
  *[[lego_dynamixel_control#preliminaries|Preliminaries]]
  *[[lego_dynamixel_control#nxt brick code|NXC Code]]
  *[[lego_dynamixel_control#running and analysis|Running and Analysis]]
  *[[lego_dynamixel_control#conclusions|Conclusions]]


====Parts List====

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 XL-320** 
\\
https://www.trossenrobotics.com/dynamixel-xl-320-robot-actuator
\\
{{:torres:tutorials:nxt_dynamixel:image1.jpg?300|}}
\\
**Lego BreadBoard Adapter**
\\
http://www.mindsensors.com/ev3-and-nxt/58-breadboard-connector-kit-for-nxt-or-ev3
\\
{{:torres:tutorials:nxt_dynamixel:capture5.png?nolink&300|}}
\\
**U2D2**
\\
http://www.robotis.us/u2d2/
\\
{{:torres:tutorials:image3.jpg?300|}}
\\
**(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
\\
{{:torres:tutorials:nxt_dynamixel:capture7.jpg?nolink&300|PHOTO}}
\\
\\
Other Parts Needed:
   *NXT Cable
\\
{{:torres:tutorials:nxt_dynamixel:capture8.jpg?nolink&300|}}
\\
   *Dynamixel Connector Cables
\\
{{:torres:tutorials:nxt_dynamixel:image2.jpg?300|}}
\\
   *USB A-to-C cable
\\
{{:torres:tutorials:nxt_dynamixel:capture10.png?nolink&300|}}
\\
   *External Power Supply
\\
{{:torres:tutorials:nxt_dynamixel:capture11.jpg?nolink&300|}}
\\
====Theoretical Background====

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.
\\
\\
{{:torres:tutorials:nxt_dynamixel:capture12.png?nolink&500|}}
\\
\\
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 [[https://en.wikipedia.org/wiki/Parity_bit|parity bit]] can go next, otherwise a stop bit "1" is used to signify the end of the transmission.
\\
\\
**Dynamixel Communication**
\\
{{:torres:tutorials:nxt_dynamixel:capture1.png?700|}}
\\
Communication with the Dynamixel XL-320 servos require several parameters. A tutorial about NXT communication with the RX-28 servos (similar, but a different protocol is used) can be found [[lego_nxt_dynamixel|here]]. The XL-320 [[http://emanual.robotis.com/docs/en/dxl/x/xl320/#control-table-of-eeprom-area|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.
\\
\\
{{:torres:tutorials:image5.jpg?800|}}
\\
\\
**Header/Reserved:** These four bytes are used to signal an incoming packet; they always remain the same.
\\
**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 two bit 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".
\\
{{:torres:tutorials:image4.jpg?800|}}
\\
**PARAMETERS:** The first one or two parameters are 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.
\\
**CRC:** These two bits are the calculated cyclic redundancy check value. This is a more complicated recursive algorithm than the previously used checksum. The CRC is used to detect any message errors that could have occurred during transmission.




====Preliminaries====
**Setting Default Baud Rate**
\\
The default baud rate for the XL-320 is 1 mbps. The NXT is not able to communicate at these speeds. To be able to reduce the default baud rate, one must use the Dynamixel Wizard software found within the [[http://www.robotis.us/roboplus1/|RoboPlus 1.0 suite]] as well as the U2D2 communication converter. Follow the instructions [[http://emanual.robotis.com/docs/en/software/rplus1/dynamixel_wizard/|here]] for communicating with the Dynamixel. You will have to use jumper cables and a breadboard to connect the U2D2 TTL channel to the XL-320 since the servo cable is too small. Change the default baud rate to 57600 baud. You can also change other parameters if desired.
\\
\\
**RS485 to TTL**
\\
The XL-320 uses TTL logic levels (0 to 5v) for communication. Since the NXT uses RS485 communication, an RS485-to-TTL converter must be used. This can be done using a [[https://www.google.com/url?sa=t&rct=j&q=&esrc=s&source=web&cd=7&ved=2ahUKEwip-ILdzdbiAhWQup4KHYefCGwQFjAGegQIBhAC&url=https%3A%2F%2Fdatasheets.maximintegrated.com%2Fen%2Fds%2FMAX1487-MAX491.pdf&usg=AOvVaw3BFaoKg2WHoZayW5LHwFD5|MAX485 chip]], as seen below or with an off the shelf converter such as [[https://www.amazon.com/DROK-Adapter-Module-Converter-Indicator/dp/B075V2NMV8/ref=sr_1_3?keywords=rs485+to+ttl&qid=1558067804&s=gateway&sr=8-3|this one]].
\\
\\
{{:torres:image6.jpg?300|}}
\\
\\
The MAX485 chip offers some simplicity over using an off the shelf converter although many higher level features are not included. Nevertheless, it is definitely enough for this tutorial at a fraction of the cost and zero extra added components. The pin layout is as follows:
\\
   *1 - TTL Output
   *2 - LOW (GND)
   *3 - LOW (GND)
   *4 - Not Connected
   *5 - GND
   *6 - SDA (From NXT)
   *7 - SCL (From NXT)
   *8 - 5 volts
**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 XL-320'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 "DynamixelXL320.h" must be located in the same folder as the code
\\
<code>
//Norberto Torres-Reyes
//05/16/2019
//Writes ID to a Dynamixel XL-320 with NXT

#include "DynamixelXL320.h"
#define ID 02  //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);
 }
</code>
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 to a certain angle and back in a loop. Once again, "DynamixelXL320.h" must be included in the same folder when running the code.
<code>
//Dynamixel XL-320 Servo Control
//Created by: Norberto Torres-Reyes
//Date: 05/16/19
//Version 1.0

#include "DynamixelXL320.h"
#define ID 0XFE

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

 while(true){
 Wait(1000);
 Servo(ID,500,100);
 Wait(1000);
 Servo(ID,0,100);
 }
}
</code>

====NXC Code====
The NXC code files as well as the header file is included below in a .zip file:
\\
{{:torres:xl320_codev2.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.
<code>
#define ID1 01
#define ID2 02
</code>
\\
\\
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:
<code>
//2_Dynamixel_Control
//By: Norberto Torres-Reyes
//Date: 05/16/19
//Controls two different Dynamixels using the left and right buttons.

#include "DynamixelXL320.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);
 }

 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);
 }
 }
}
</code>

====Running and Analysis====
Running the code is straight forward. Ensure that the NXT is connected to the computer via the USB cable and the [[http://bricxcc.sourceforge.net/|"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. The data lines from the adapter should then be connected to the RS485-to-TTL converter.
\\
For a single servo control, connect one of the Dynamixel cables to the TTL output. If using two or more, one cable goes from the TTL output to the first Dynamixel. Another cable is connected from the first servo to the next servo and so on.
\\
**Video:**
\\
{{youtube>ODi75v3P31c?medium}}
\\
\\
**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 [[https://www.youtube.com/watch?v=Gx0Qo5n-x_4|here]]. Below is an example of an instruction packet as seen from the logic analyzer. 
\\
{{:torres:tutorials:nxt_dynamixel:logiccapture_instructionpacketservo_trimmed.png?nolink&900|}}
\\
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.
====Conclusions====
In Conclusion, this tutorial has provided a brief introduction into RS-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.