Detect and control servos with the high 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 would like to develop a program that auto-detects all the servos connected to a port, sets their position at \(\pi\) radians (180 degrees) and then reads the position that they have to verify that they got to the desired one. To do so, we are going to use the high level interface of the libdynamixel library. Our program will have the following usage

./high_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.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 select which protocol our servos are utilizing by using the C++ preprocessor:

// Define protocol
#ifdef PROTOCOL1
    using protocol_t = dynamixel::protocols::Protocol1;
#else
    using protocol_t = dynamixel::protocols::Protocol2;
#endif

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);

Next, we try to detect all the connected servos:

// auto-detect scans all possible IDs and returns the connected servos
std::vector<std::shared_ptr<dynamixel::servos::BaseServo<protocol_t>>> servos
    = dynamixel::auto_detect<protocol_t>(controller);

Finally, for each of the dected servos we enable it and set its position at \(\pi\) radians:

// For each servo detected
for (auto servo : servos) {
    // Print the name and id of the servo
    std::cout << "Detected an " << servo->model_name()
              << " with ID " << servo->id() << std::endl;

    dynamixel::StatusPacket<protocol_t> st;
    // Enable torque
    controller.send(servo->set_torque_enable(1));
    controller.recv(st);

    // Put it in position of pi
    controller.send(servo->set_goal_position_angle(M_PI));
    controller.recv(st);

    // Sleep for a bit to allow the servo to reach the position
    sleep(1);

    // Request the current position of the servo
    controller.send(servo->get_present_position_angle());
    controller.recv(st);

    // Parse the position from the packet received
    if (st.valid()) // check if valid response
    {
        double a = servo->parse_present_position_angle(st);
        std::cout << "It went to the angle: " << a << ". "
                  << "Error: " << std::sqrt((a - M_PI) * (a - M_PI)) << std::endl;
    }
    else
        std::cout << "Could not read the angle of the servo." << std::endl;
}

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

Here’s the high_level_control.cpp file:

#include <iostream>
#include <dynamixel/dynamixel.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;
    }

// Define protocol
#ifdef PROTOCOL1
    using protocol_t = dynamixel::protocols::Protocol1;
#else
    using protocol_t = dynamixel::protocols::Protocol2;
#endif

    // 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 want to catch exceptions
    try {
        // auto-detect scans all possible IDs and returns the connected servos
        std::vector<std::shared_ptr<dynamixel::servos::BaseServo<protocol_t>>> servos
            = dynamixel::auto_detect<protocol_t>(controller);

        // For each servo detected
        for (auto servo : servos) {
            // Print the name and id of the servo
            std::cout << "Detected an " << servo->model_name()
                      << " with ID " << servo->id() << std::endl;

            dynamixel::StatusPacket<protocol_t> st;
            // Enable torque
            controller.send(servo->set_torque_enable(1));
            controller.recv(st);

            // Put it in position of pi
            controller.send(servo->set_goal_position_angle(M_PI));
            controller.recv(st);

            // Sleep for a bit to allow the servo to reach the position
            sleep(1);

            // Request the current position of the servo
            controller.send(servo->get_present_position_angle());
            controller.recv(st);

            // Parse the position from the packet received
            if (st.valid()) // check if valid response
            {
                double a = servo->parse_present_position_angle(st);
                std::cout << "It went to the angle: " << a << ". "
                          << "Error: " << std::sqrt((a - M_PI) * (a - M_PI)) << std::endl;
            }
            else
                std::cout << "Could not read the angle of the servo." << std::endl;
        }
    }
    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 = 'high_level_control.cpp',
              includes = '.',
              target = 'high_level_control',
              uselib =  libs,
              defines = ['PROTOCOL1']) # change this to PROTOCOL2 if you want to use servos that operate with PROTOCOL2