Control servos with the low level interface¶

Description¶

This tutorial assumes that you have already been able to compile and install the library. If not, please refer to the related instructions in the building/installation instructions.

We have a robot that uses 4 dynamixel actuators: two MX106 ones and two MX28 ones. We would like to develop a program that connects to the actuators, enables them, sets their maximum speed so that they are slower than usual and sets their position at \(\pi\) radians (180 degrees). To do so, we are going to use the low level interface of the libdynamixel library. Our program will have the following usage

./low_level_control port

It requires the port name which is the absolute path to the linux device interfacing with the Dynamixel bus.

Depending on the interface device you use, the device name would look like ttyACMx or ttyUSBx with x being an integer

Warning

To be able to access this interface, you usually need special rights. On ubuntu, you either have to belong to the dialout group or to run the generated programs as superuser.

C++ Code with comments¶

We begin with including the required files:

#include <iostream>
#include <dynamixel/dynamixel_core.hpp>
#include <dynamixel/servos/mx106.hpp>
#include <dynamixel/servos/mx28.hpp>

We are expecting the port name as a command line parameter:

int main(int argc, char** argv)
{
    // We need to know to which port/device our servos are connected
    if (argc != 2) {
        std::cout << "Usage: " << argv[0] << " device/port" << std::endl;
        return -1;
    }

We first create a Usb2Dynamixel object with default parameters:

// Create a new Usb2Dynamixel controller with default parameters
dynamixel::controllers::Usb2Dynamixel controller;

Next, we want to establish a connection with the desired port:

// Always use try-catch when connecting to a device/port
try {
    // connect to device/port
    controller.open_serial(argv[1]);
}
catch (const dynamixel::errors::Error& e) {
    // Something bad happened
    std::cerr << "Error (dynamixel): " << e.msg() << std::endl;
    return 0;
}

Note that we should always enclose the connection to the serial port with a try-catch, since connecting to serial devices could always have some unexpected errors. Similarly, we would like our Usb2Dynamixel controller to notify us for possible errors. To do this, we must enable it as follows:

// Enable throwing exceptions -- by default all errors are ignored/silenced
controller.set_report_bad_packet(true);

We create 2 arrays with the IDs of each type of actuators:

// We need to know the IDs and the type of actuators
std::vector<size_t> mx_106_ids = {1, 2}; // we have 2 MX106 with IDs 1 and 2
std::vector<size_t> mx_28_ids = {3, 4}; // we have 2 MX28 with IDs 3 and 4

Finally, for each actuator we enable it, set its maximum speed and set its position at \(\pi\) radians:

dynamixel::StatusPacket<protocol_t> status;

for (size_t i = 0; i < mx_106_ids.size(); i++) {
    using mx106_t = dynamixel::servos::Mx106;
    // Enable torque
    controller.send(mx106_t::set_torque_enable(mx_106_ids[i], 1));
    controller.recv(status);

    // Set the moving speed of the actuator
    controller.send(mx106_t::set_moving_speed(mx_106_ids[i], 50));
    controller.recv(status);

    // Write goal position in register of the actuator
    controller.send(mx106_t::reg_goal_position_angle(mx_106_ids[i], M_PI));
    controller.recv(status);
}

for (size_t i = 0; i < mx_28_ids.size(); i++) {
    using mx28_t = dynamixel::servos::Mx28;
    // Enable torque
    controller.send(mx28_t::set_torque_enable(mx_28_ids[i], 1));
    controller.recv(status);

    // Set the moving speed of the actuator
    controller.send(mx28_t::set_moving_speed(mx_28_ids[i], 50));
    controller.recv(status);

    // Write goal position in register of the actuator
    controller.send(mx28_t::reg_goal_position_angle(mx_28_ids[i], M_PI));
    controller.recv(status);
}

// Send broadcast action
controller.send(dynamixel::instructions::Action<protocol_t>(protocol_t::broadcast_id));

All these commands should be enclosed into a try-catch since we enabled the exception throwing.

Here’s the low_level_control.cpp file:

#include <iostream>
#include <dynamixel/dynamixel_core.hpp>
#include <dynamixel/servos/mx106.hpp>
#include <dynamixel/servos/mx28.hpp>

int main(int argc, char** argv)
{
    // We need to know to which port/device our servos are connected
    if (argc != 2) {
        std::cout << "Usage: " << argv[0] << " device/port" << std::endl;
        return -1;
    }

    // Create a new Usb2Dynamixel controller with default parameters
    dynamixel::controllers::Usb2Dynamixel controller;

    // Always use try-catch when connecting to a device/port
    try {
        // connect to device/port
        controller.open_serial(argv[1]);
    }
    catch (const dynamixel::errors::Error& e) {
        // Something bad happened
        std::cerr << "Error (dynamixel): " << e.msg() << std::endl;
        return 0;
    }

    // Enable throwing exceptions -- by default all errors are ignored/silenced
    controller.set_report_bad_packet(true);

    // We need to know the IDs and the type of actuators
    std::vector<size_t> mx_106_ids = {1, 2}; // we have 2 MX106 with IDs 1 and 2
    std::vector<size_t> mx_28_ids = {3, 4}; // we have 2 MX28 with IDs 3 and 4

    using protocol_t = dynamixel::protocols::Protocol1;

    // We want to catch exceptions
    try {
        dynamixel::StatusPacket<protocol_t> status;

        for (size_t i = 0; i < mx_106_ids.size(); i++) {
            using mx106_t = dynamixel::servos::Mx106;
            // Enable torque
            controller.send(mx106_t::set_torque_enable(mx_106_ids[i], 1));
            controller.recv(status);

            // Set the moving speed of the actuator
            controller.send(mx106_t::set_moving_speed(mx_106_ids[i], 50));
            controller.recv(status);

            // Write goal position in register of the actuator
            controller.send(mx106_t::reg_goal_position_angle(mx_106_ids[i], M_PI));
            controller.recv(status);
        }

        for (size_t i = 0; i < mx_28_ids.size(); i++) {
            using mx28_t = dynamixel::servos::Mx28;
            // Enable torque
            controller.send(mx28_t::set_torque_enable(mx_28_ids[i], 1));
            controller.recv(status);

            // Set the moving speed of the actuator
            controller.send(mx28_t::set_moving_speed(mx_28_ids[i], 50));
            controller.recv(status);

            // Write goal position in register of the actuator
            controller.send(mx28_t::reg_goal_position_angle(mx_28_ids[i], M_PI));
            controller.recv(status);
        }

        // Send broadcast action
        controller.send(dynamixel::instructions::Action<protocol_t>(protocol_t::broadcast_id));
    }
    catch (const dynamixel::errors::Error& e) {
        // Something bad happened
        std::cerr << "Error (dynamixel): " << e.msg() << std::endl;
    }

    return 0;
}

Buildind with Waf¶

Now we need to create a wscript file for our project to compile it with waf (see the compilation tutorial for details):

#!/usr/bin/env python

def options(opt):
    pass

def configure(conf):
  # Get locations where to search for libdynamixel's headers
  includes_check = ['/usr/include', '/usr/local/include']

  try:
    # Find the headers of libdynamixel
    conf.start_msg('Checking for libdynamixel includes')
    conf.find_file('dynamixel/dynamixel.hpp', includes_check)
    conf.end_msg('ok')

    conf.env.INCLUDES_LIBDYNAMIXEL = includes_check
  except:
    conf.end_msg('Not found', 'RED')
  return

def build(bld):
    libs = 'LIBDYNAMIXEL BOOST'

    obj = bld(features = 'cxx',
              source = 'low_level_control.cpp',
              includes = '.',
              target = 'low_level_control',
              uselib =  libs)