Dynamixel servo motors can be controlled in different ways : by Wizard2.0 the Dynamixel software or Raspberry with pypot each requiring a USB2Dynamixel or by Raspberry or Arduino with Dynamixel shields. However, to set the fundamental parameters of the dynamixel servo, it is generally the Dynamixel software on the computer that is used. This is very restrictive and that is why I was led to develop a small embedded system to facilitate the setting of Dynamixel MX series servos.
Contenus de la page
Equipment At My Disposal
- An Arduino Uno as motherboard
- An Arduino 2.8 TFT LCD Shield touch screen as user interface
- One MX-106 servo motor and one MX-28 servo motor for testing my project
Simple Communication With The Servo Motor
To avoid having to purchase one of the above-mentioned connectors, the choice was made to control the servo via TTL serial communication from an Arduino Uno (i.e. using the Rx and Tx ports of the Arduino). However, reverting to the motor’s intrinsic communication protocol requires a good understanding of it and sending the right binary data packets. Another difficulty is that the servo data pin is a duplex for transmitted and received information. However, on the arduino the serial communication pins are separate.
A thorough review of the state of the art led me to note that a similar project had already been done on a proprietary card by JosueGutierrez from SavageElectronics. However, it is the work of Mahyar Abdeetedal that has contributed to the development of the following elements. Indeed, despite the fact that Dynamixel has developed an Arduino library in partnership with its community, it was designed for the use of Dynamixel shields and was not conclusive for the desired use. To avoid developing a new library, research led me to the work of J.Teda who created the Dynamixel_Serial library on Arduino.
Therefore, to communicate with the Dynamixel, the SN74LS241 3-state buffer is required, allowing half-duplex communication according to the following circuit:
Parameter Setting Function
Now able to communicate with the servo motor, it will be necessary to write the function to configure the servo motor whatever its current parameters.
//======Library======//
#include <Dynamixel_Serial.h> // Library needed to control Dynamixel servo
//======Servo parameters======//
#define SERVO_ControlPin 10 // Control pin of buffer chip
#define LED13 0x0D // Pin of Visual indication for runing "heart beat" using onboard LED
//======Variable======//
long Baud[9] = {9600, 19200, 57600, 115200, 200000, 250000, 400000, 500000, 1000000};
bool MODE=1; //ServoMode by default
int Torque = 100; //100% torque by default
unsigned int CWLimit=0x000*0.088; //converts the hexadecimal input to degrees
unsigned int CCWLimit=0xFFF*0.088;
void ResetServo(){
// Pin setup for Visual indication of runing (heart beat) program using onboard LED
pinMode(LED13, OUTPUT);
digitalWrite(LED13, HIGH);
for (int b=0; b<9; b++){ // This "for" loop will take about 20 Sec to compelet and is used to loop though all speeds that Dynamixel can be and send reset instuction
long Baudrate_BPS = 0;
Baudrate_BPS = Baud[b];
if(Baudrate_BPS==57600){
Dynamixel.begin(Baudrate_BPS ,SERVO_ControlPin); // Set Ardiuno Serial speed and control pin
Dynamixel.reset(0xFE); // Broadcast to all Dynamixel IDs(0xFE is the ID for all Dynamixel to responed) and Reset Dynamixel to factory default
}
else{
Dynamixel.begin(Baudrate_BPS ,SERVO_ControlPin); // Set Ardiuno Serial speed and control pin
for (int i=1; i<0xFF; i++){
Dynamixel.reset(i);
}
delay(5);
}
delay(100); // Time needed for Dynamixel to Broadcast
}
digitalWrite(LED13, LOW);
delay(3000); // Give time for Dynamixel to reset
}
The ResetServo() function has the role of « making the servo programmable ». In fact, let’s consider at the beginning that the servomotor is of unknown ID and Baudrate. In this case, it is necessary to be able to impose working parameters before imposing those desired by the user. This is what this function does by using the reset instruction, imposing 57600bps as the working frequency.
void ProgramBaudrateID(long SERVO_SET_Baudrate, int SERVO_ID,bool MODE,int Torque,unsigned int CWLimit,unsigned int CCWLimit){
// Now that the Dynamixel is reset to factory setting we will program its Baudrate and ID
Dynamixel.begin(57600,SERVO_ControlPin); // Set Ardiuno Serial speed to factory default speed of 57600
Dynamixel.setID(0xFE,SERVO_ID); // Broadcast to all Dynamixel IDs(0xFE) and set with new ID
delay(10); // Time needed for Dynamixel to set it's new ID before next instruction can be sent
Dynamixel.setStatusPaket(SERVO_ID,READ); // Tell Dynamixel to only return status packets when a "read" instruction is sent e.g. Dynamixel.readVoltage();
delay(30);
Dynamixel.setBaudRate(SERVO_ID,SERVO_SET_Baudrate); // Set Dynamixel to new serial speed
delay(30); // Time needed for Dynamixel to set it's new Baudrate
Dynamixel.begin(SERVO_SET_Baudrate,SERVO_ControlPin); // We now need to set Ardiuno to the new Baudrate speed
Dynamixel.ledState(SERVO_ID, ON); // Turn Dynamixel LED on
delay(5);
Dynamixel.setMode(SERVO_ID, MODE,int(CWLimit*11.375),int(CCWLimit*11.375)); // Turn mode to SERVO, must be WHEEL if using wheel mode
delay(30);
Dynamixel.setMaxTorque(SERVO_ID,int(Torque*7,67)); // Set Dynamixel to max torque limit
}
Now programmable, user parameters are applied to the servomotor : Baudrate, ID, Operating mode, torque, min and max angle.
User Interface
Now that the major function is done, the user must be able to impose its parameters. To do this, it was chosen to use the 2.8 TFT LCD Shield touch screen. The GFX library from Adafruit was used to create the graphical interface and is based on the following idea :
//======Library======//
#include <Adafruit_TFTLCD.h>
#include <Adafruit_GFX.h>
#include <TouchScreen.h>
//======GPIO======//
[...]
//======Screen Calibration======//
[...]
//======Colors======//
[...]
//======tft object declaration======//
Adafruit_TFTLCD tft(LCD_CS, LCD_CD, LCD_WR, LCD_RD, LCD_RESET);
TouchScreen ts = TouchScreen(XP, YP, XM, YM, 300);
//======Variable======//
char currentPage;
void setup() {
//Initial LCD setup//
tft.reset();
tft.begin(0x9341);
tft.setRotation(3);
DrawHomeScreen();
currentPage = '0'; // Indicates that we are at Home Screen
}
The setup displays the wallpaper once on switch-on after initializing it.
void loop() {
if(currentPage=='0'){
TSPoint p = ts.getPoint(); //Get touch point
if (p.z > ts.pressureThreshhold) {
p.x = map(p.x, TS_MAXX, TS_MINX, 0, 320);
p.y = map(p.y, TS_MAXY, TS_MINY, 0, 240);
if(p.x>30 && p.x<285 && p.y>130 && p.y<200){ // The user has pressed inside the red rectangle
//This is important, because the libraries are sharing pins
pinMode(XM, OUTPUT);
pinMode(YP, OUTPUT);
currentPage='1';
DrawBaudRateSetup();
}
}
}
[...]
}
The loop is responsible for checking at any time whether there has been an interaction on the screen by looping over « ifs » and, if necessary, displaying a new feature.
void DrawHomeScreen(){
//Backgroud in black//
tft.fillScreen(BLACK);
//Draw white frame
tft.drawRect(0,0,319,240,WHITE);
//Print "Dynamixel" Text
tft.setCursor(110,30);
tft.setTextColor(WHITE);
tft.setTextSize(2);
tft.print("Dynamixel");
//Print "Configuration of MX servo" Text
tft.setCursor(10,50);
tft.setTextColor(WHITE);
tft.setTextSize(2);
tft.print("Configuration of MX servo");
//Create Red Button
tft.fillRect(30,130, 260, 70, DRED);
tft.drawRect(30,130, 260, 70,WHITE);
tft.setCursor(118,138);
tft.setTextColor(WHITE);
tft.setTextSize(3);
tft.print("Start");
tft.setCursor(40,168);
tft.setTextColor(WHITE);
tft.setTextSize(3);
tft.print("configuration!");
}
To draw, loop and setup independent functions are created for each page being displayed that need to be called at the desired time as shown above.
Implementation Of The Project
Almost finished, it is only a question of making the electrical circuit and its box.
Electrical Circuit
Enclosure
Designed to be powered by the PARKSIDE 20V, 2 Ah battery, the base has been made to be clipped on without problem: