Controlling Dynamixel servomotors

Page last updated 25 Oct 2015, by Dan Gongora. 0 replies dynamixel, RS485

Wiring

The code was tested with the following circuit: /media/uploads/dmgongora/schematic.png

Connection test

mbed sends a Ping packet and waits for the response from the Dynamixel.

Dynamixel to mbed connection test

#include "mbed.h"
// From Dynamixel instruction set
const unsigned char PING = 0x01;
void pingDynamixel(uint8_t);
// Half duplex Asynchronouse serial communication
// 8 bit, 1 stop, No parity (mbed default config)
Serial dx116(p13, p14);
// Control pin for the half duplex communication
DigitalOut TxEnable(p15);
Serial pc(USBTX, USBRX);
int main()
{
    TxEnable = 0;   // Disable Tx/Enable Rx
    const uint8_t motorID = 0x01;     // Dynamixel ID
    const int dynamixelBR = 57600;
    dx116.baud(dynamixelBR);
    pc.baud(9600);
    
    pc.printf("Request status packet from ID #%x\n with %d bps", motorID, dynamixelBR);
    pingDynamixel(motorID);
    while(1) {
        if (dx116.readable()) {
            pc.printf("0x%x\n", dx116.getc());  // Read status packet if available
            wait_ms(100);
        }
        wait_ms(5);
    }
}

void pingDynamixel(uint8_t myID)
{
    uint8_t elements = 6;
    uint8_t packetBuffer[elements];
    if (dx116.writeable() ) {
        packetBuffer[0] = 0xff;
        packetBuffer[1] = 0xff;
        packetBuffer[2] = myID;   // ID
        packetBuffer[3] = 0x02;   // Length
        packetBuffer[4] = PING;   // Instruction
        packetBuffer[5] = ~(packetBuffer[2] + packetBuffer[3] + packetBuffer[4]) & 0xFF;   // Check sum
        // Send instruction packet
        TxEnable = 1;       // Enable Tx / Disable Rx
        for (int i = 0; i<= elements; i++) {
            dx116.putc(packetBuffer[i]);
        }
        wait_ms(1);         // fix this!!!
        TxEnable = 0;       // Disable Tx / Enable Rx
    } else {
        pc.printf("Dynamixel not writeable\n");
    }
}

Import programdynamixel_pingTest

mbed to Dynamixel ping test.

Last commit 18 Aug 2015 by Dan Gongora

The coding conventions of the library might not be appropriate and many methods are missing but it is usable.

Example using the library

#include "mbed.h"
#include "Dynamixel.h"

int main()
{
    Dynamixel DX116(p13, p14, p15, 9, 57600);
    
    DX116.toggleLED(1);
    wait(0.5);
    DX116.toggleLED(0);
    DX116.move(0);
    wait(1);
    DX116.move(1023);
}

Import programdynamixel_test

Tested on RX-24F and DX116.

Last commit 19 Aug 2015 by Dan Gongora

Import programdynamixel_pingTest

mbed to Dynamixel ping test.

Last commit 18 Aug 2015 by Dan Gongora

The coding conventions of the library might not be appropriate and many methods are missing but it is usable.

Example using the wheel mode

/*****************************************************
- Description: mbed to Dynamixel connection test using
    the library
- Requirements:
    Dynamixel (i.e. DX116, RX28)
    MAX3088/MAX485 (RS485 transceiver)
- Connections:
    MAX3088     --  mbed
    ======================
    Pin 1       --  Pin 14
    Pin 2       --  Pin 15
    Pin 4       --  Pin 13

- Comments:
    See schematic for wiring details and class
    documentation for available methods.
*****************************************************/
#include "mbed.h"
#include "Dynamixel.h"

DigitalOut debug1(LED1);
DigitalOut debug2(LED2);
DigitalOut debug3(LED3);
DigitalOut debug4(LED4);
int main()
{
    debug1 = 0;
    debug2 = 0;
    debug3 = 0;
    debug4 = 0;
    Dynamixel motor2(p13, p14, p15, 1, 57600); // TX, RX, TX_ENABLE, MOTOR_ID, BD

    /**************************
    *** Check communication ***
    **************************/
    motor2.toggleLED(1);
    wait(1);
    motor2.toggleLED(0);
    /**************************
    ***** Set max angles  *****
    **************************/
    motor2.setAngleLimit(0, ADDRESS_CW_ANGLE_LIMIT);
    wait(0.5);
    motor2.setAngleLimit(0, ADDRESS_CCW_ANGLE_LIMIT);
    wait(0.5);
    /**************************
    ******** Set speed ********
    **************************/
    motor2.setSpeed(50); // 0-1023 CW
    debug1 = 1;
    wait(2);
    motor2.setSpeed(2000); // 1024-2047 CCW
    wait(2);
    debug2 = 1;
    motor2.setSpeed(0);
}

Import programdynamixel_wheel_test

Tested on MX-28

Last commit 25 Oct 2015 by Dan Gongora

Troubleshooting

If the servos do not respond, make sure you're using the right ID and baudrate values.

Please log in to post comments.