Sorry it has been several years since I played with the DXL servos.
Back then I created a Serial port handler for the DXL servos.
No idea if this still works or not, but here is a sketch I had back then
#include <Dynamixel2Arduino.h>
//====================================================================================================
// Quick dirty test to find servos and track them...
// teensy version
//
//====================================================================================================
//============================================================================
// Global Include files
//=============================================================================
//=============================================================================
// Options...
//=============================================================================
typedef struct {
uint8_t protocol_index;
uint8_t first_servo_found;
uint8_t last_servo_found;
} PortList;
class TeensySerialPortHandler : public DYNAMIXEL::SerialPortHandler
{
public:
TeensySerialPortHandler(HardwareSerial& port, const int dir_pin = -1)
: SerialPortHandler(port, dir_pin), port_(port), dir_pin_(dir_pin) {}
virtual void begin(unsigned long baud) override
{
baud_ = baud;
if (dir_pin_ != -1) {
port_.begin(baud_);
port_.transmitterEnable(dir_pin_);
} else {
port_.begin(baud_, SERIAL_HALF_DUPLEX);
}
setOpenState(true);
}
virtual void end(void) override
{
port_.end();
setOpenState(false);
}
private:
HardwareSerial& port_;
const int dir_pin_;
uint32_t baud_;
};
#define DXL_SERIAL Serial1
#define DXL_DIR_PIN 2
#define DXL_BAUDRATE 1000000
#define DXL_SERVO_ENABLE_PIN 3
Dynamixel2Arduino dxl;
TeensySerialPortHandler dxl_port(DXL_SERIAL, DXL_DIR_PIN);
#define DEBUG_TOGGLE_PIN 13
#ifdef DEBUG_TOGGLE_PIN
#define DBGTogglePin(pin) digitalWrite(pin, !digitalRead(pin));
#else
#define DBGTogglePin(pin)
#endif
PortList portlist[] = {
{1},
{2},
};
#define COUNT_PORT_LIST (sizeof(portlist)/sizeof(portlist[0]))
//=============================================================================
// Define different robots..
//=============================================================================
// Protocol version
#define PROTOCOL_VERSION 1.0 // See which protocol version is used in the Dynamixel
#define PROTOCOL_VERSION2 2.0 // See which protocol version is used in the Dynamixel
#define DEVICENAME "3" // Check which port is being used on your controller
/** EEPROM AREA **/
#define AX_PRESENT_POSITION_L 36
//=============================================================
// Defines for X series (XL430)
//=============================================================================
#define DXL_X_MODEL_NUMBER 0 // 2 (R)
#define DXL_X_PRESENT_POSITION 132 // 4 (R)
//=============================================================================
// Globals
//=============================================================================
// Global objects
// Handle to port handler and packet handler;
uint8_t g_servo_port_index[254];
int g_servo_positions[254];
uint32_t g_baud_rate = DXL_BAUDRATE;
bool find_servos = true;
void wait_keyboard() {
Serial.println("Hit any key to continue"); Serial.flush();
while (!Serial.available()) ;
while (Serial.read() != -1) ;
}
//====================================================================================================
// Setup
//====================================================================================================
void setup() {
while (!Serial && (millis() < 3000)) ; // Give time for Teensy and other USB arduinos to create serial port
Serial.begin(38400); // start off the serial port.
if (CrashReport) {
Serial.print(CrashReport);
Serial.println("Press any key to continue");
while (Serial.read() == -1);
while (Serial.read() != -1);
}
Serial.println("\nDyn2Arduino Track Servos program");
// wait_keyboard();
#ifdef DXL_SERVO_ENABLE_PIN
pinMode(DXL_SERVO_ENABLE_PIN, OUTPUT);
digitalWrite(DXL_SERVO_ENABLE_PIN, HIGH);
#endif
#ifdef DEBUG_TOGGLE_PIN
pinMode(DEBUG_TOGGLE_PIN, OUTPUT);
#endif
dxl.setPort(dxl_port);
SetBaudRate(DXL_BAUDRATE);
find_servos = true;
}
//====================================================================================================
// Loop
//====================================================================================================
void loop() {
// Output a prompt
#ifdef USBHOST_SERIAL
myusb.Task();
#endif
if (find_servos) {
FindServos();
find_servos = false;
// lets toss any charcters that are in the input queue
Serial.println("\nFind Again, enter new baud or just hit enter");
Serial.println("Or move servos to track their movement");
Serial.flush(); // make sure the complete set of prompts has been output...
while (Serial.read() != -1) ; // Remove any stuff still in serial buffer.
}
if (Serial.available()) {
// Get a command
uint32_t new_baud = CheckForNewBaud();
if (new_baud) {
SetBaudRate(new_baud);
} else {
wait_keyboard();
}
find_servos = true;
} else {
TrackServos();
}
}
//=======================================================================================
XelInfoFromPing_t ping_info[32];
void FindServos(void) {
Serial.print("\nSearch for all servos at baud rate: ");
Serial.println(g_baud_rate, DEC);
// Initialize to no servos...
for (int i = 0; i < 254; i++) {
g_servo_port_index[i] = 0xff; // not found
}
for (uint8_t port_index = 0; port_index < COUNT_PORT_LIST; port_index++) {
DBGTogglePin(DEBUG_TOGGLE_PIN);
Serial.print("Begin Searching");
portlist[port_index].first_servo_found = 0xff;
portlist[port_index].last_servo_found = 0xff;
dxl.setPortProtocolVersionUsingIndex(portlist[port_index].protocol_index);
Serial.println(dxl.getPortProtocolVersion(), 2);
// wait_keyboard();
if (portlist[port_index].protocol_index == 1) {
Serial.println(" Begin Protocol 1: ");
for (int i = 0; i < 254; i++) {
//Serial.print(".");
if (dxl.ping(i)) {
if (g_servo_port_index[i] != 0xff) {
Serial.println("Multiple servos found with same ID");
}
g_servo_port_index[i] = port_index;
portlist[port_index].last_servo_found = i;
if (portlist[port_index].first_servo_found == 0xff) portlist[port_index].first_servo_found = i;
g_servo_positions[i] = static_cast<int>(dxl.getPresentPosition(i));
Serial.printf(" %d Type:%d Position:%d(%3.2f)\n", i,
dxl.getModelNumber(i), g_servo_positions[i],
dxl.getPresentPosition(i, UNIT_DEGREE)) ;
}
}
Serial.println(" Done");
} else {
Serial.println(" Begin Protocol 2 Simple Ping: ");
for (uint8_t i = 0; i < 254; i++) {
//Serial.print("-");
if (dxl.ping(i)) {
if (g_servo_port_index[i] != 0xff) {
Serial.println("Multiple servos found with same ID");
}
g_servo_port_index[i] = port_index;
Serial.printf(" %d Type:%d Position:%d\n", i,
dxl.getModelNumber(i), static_cast<int>(dxl.getPresentPosition(i))) ;
}
}
Serial.println(" Begin Protocol 2 ping data: ");
for (uint8_t i = 0; i < 254; i++) {
if (dxl.ping(i, ping_info, 1)) {
// if (((DYNAMIXEL::Master)dxl).ping(i, ping_info, 1)) {
if (g_servo_port_index[i] == 1) {
Serial.println("Multiple servos found with same ID");
}
g_servo_port_index[i] = 2;
portlist[port_index].last_servo_found = i;
if (portlist[port_index].first_servo_found == 0xff) portlist[port_index].first_servo_found = i;
Serial.printf(" %d Type:%x Ver:%x Position:%d \n", i,
ping_info[0].model_number, ping_info[0].firmware_version,
static_cast<int>(dxl.getPresentPosition(i))) ;
}
}
/*
Serial.println(" Try Protocol 2 - broadcast ping: ");
Serial.flush(); // flush it as ping may take awhile...
if (uint8_t count_pinged = dxl.ping(DXL_BROADCAST_ID, ping_info,
sizeof(ping_info) / sizeof(ping_info[0]))) {
Serial.print("Detected Dynamixel : \n");
for (int i = 0; i < count_pinged; i++)
{
Serial.print(" ");
Serial.print(ping_info[i].id, DEC);
Serial.print(", Model:");
Serial.print(ping_info[i].model_number, HEX);
Serial.print(", Ver:");
Serial.println(ping_info[i].firmware_version, HEX);
g_servo_port_index[i] = 2;
}
} else Serial.printf("Broadcast returned no items(%x)\n", dxl.getLastLibErrCode());
*/
Serial.println(" Done");
}
}
}
//=======================================================================================
// TrackServos
//=======================================================================================
#define TRACK_FUDGE 3
void TrackServos() {
for (uint8_t port_index = 0; port_index < COUNT_PORT_LIST; port_index++) {
//Serial.printf("%u %u - %u\n", port_index, portlist[port_index].first_servo_found, portlist[port_index].last_servo_found);
if (portlist[port_index].first_servo_found != 0xff) {
dxl.setPortProtocolVersionUsingIndex(portlist[port_index].protocol_index);
for (uint8_t i = portlist[port_index].first_servo_found; i <= portlist[port_index].last_servo_found; i++) {
if (g_servo_port_index[i] == port_index) {
int servo_pos = static_cast<int>(dxl.getPresentPosition(i));
if (abs(servo_pos - g_servo_positions[i]) > TRACK_FUDGE) {
Serial.print(i, DEC);
Serial.print("(");
Serial.print(portlist[port_index].protocol_index, DEC);
Serial.print("):");
Serial.print(servo_pos, DEC);
Serial.print("(");
float servo_angle = dxl.getPresentPosition(i, UNIT_DEGREE);
Serial.print(servo_angle, 3);
Serial.println(")");
g_servo_positions[i] = servo_pos;
}
}
}
}
}
}
//====================================================================================================
// Process command line, optionally return new baud rate
uint32_t CheckForNewBaud(void) {
int ch;
uint32_t new_baud = 0;
for (;;) {
// throw away any thing less than CR character...
ch = Serial.read();
if (ch != -1) {
if ((ch >= '0') && (ch <= '9')) {
new_baud = new_baud * 10 + (uint32_t)(ch - '0');
} else {
break;
}
}
}
while (Serial.read() != -1) ; // remove any trailing stuff.
return new_baud;
}
//=======================================================================================
void SetBaudRate(uint32_t new_baud)
{
Serial.print("Setting Baud to: ");
Serial.println(new_baud);
dxl.begin(new_baud);
for (uint8_t i = 0; i < COUNT_PORT_LIST; i++) {
DBGTogglePin(DEBUG_TOGGLE_PIN);
Serial.printf(" Index: %d\n", i);
delay(100);
dxl.setPortProtocolVersionUsingIndex(portlist[i].protocol_index);
if (portlist[i].protocol_index == 2) {
for (uint8_t id = 0; id < 254; id++) dxl.reboot(id);
}
}
g_baud_rate = new_baud;
Serial.println("Finished Setting Baud");
}
