Using a Microcontroller as an Intermediary

You need the openCM microcontrolleropenCM microcontroller, a power supply, and two servos with separate IDs for this tutorial. You do not need the U2D2. Also, the example code is for Matlab 2021b or later and requires the Communications Toolbox. Python has similar serial communication capabilities.

Why would I use the OpenCM9.04 instead of the U2D2?

So far we have sent serial signals from a laptop or desktop computer directly to a servo. There are some reasons why this may not be ideal:

1. An operating system technically is not real-time (i.e., it cannot guarantee that tasks take place within a given amount of time), which may lead to delays in your control loop. Sufficiently long delays destabilize otherwise stable control loops.

2. Control architecture may not require that the main computer get constant info from the servo, lengthing each control loop's duration.

3. Serial messages may take a long time to send and decode.

4. A high-power computer may rapidly deplete the robot's battery.

5. Interfacing with sensors that are not Dynamixel servos would require extra serial ports, which might complicate and/or slow down communication between the computer and robot.

Thus, we may wish to distribute the robot's control system between a full computer (laptop, desktop, even a single-board like Raspberry Pi) and a microcontroller. The microcontroller has the following benefits:

1. It can guarantee real-time operation (i.e., it can guarantee that tasks take place within a given amount of time), which should let you set things up without unpredictable delays in your control loop.

2. It may be able to run a control loop very rapidly, as long as it requires only intermittently checking in with the main computer.

3. Sending messages only intermittently will reduce the time spent sending and decoding serial messages.

4. The microcontroller may run on its own for a long time because it only requires 5V and a tiny bit of current to operate. If the controller can operate without the main computer, this could significantly extend the battery life of your robot.

5. The microcontroller may simplify serial communication by collecting all the sensory data from the robot, neatly packaging it up, then sending it to the main computer, all over one serial port.

Sending data from a PC to the OpenCM9.04, which runs a "passthrough" sketch that passes data along to the servos

By now you have learned to program servo motions with the DynamixelSDK and how to program servo motions with a personal computer. Both these methods ultimately do the same thing: construct a serial packet in a format that the servo can understand (e.g., protocol 1, protocol 2) and broadcast it to the servos over a serial port.

If you want to write to actuators or read from sensors that are not Dynamixel servos, then you will need some other way to distribute or collect that information from the rest of the robot.

Here's the basic plan:

1. Formulate serial packets on your PC in Matlab (or whatever language you'd like)

2. Send those packets to the OpenCM9.04 over a serial port

3. Receive those packets on the OpenCM9.04, then immediately broadcast them to the servos

4. OpenCM9.04 reads servo states

5. OpenCM9.04 sends those packets to the PC

6. Interpret packets in Matlab

In Matlab, we create a serial port with which to communicate with the OpenCM9.04. If you aren't sure which serial port this should be, you can use your Arduino IDE to figure it out. In Matlab, make a script to

if exist("port") %#ok<EXIST> 
    %Do not make a new serialport object, just flush this one.
    port.flush();
else
    %Make a new serialport object.
    comport = "COM4";
    baud = double(1000000);
    port = serialport(comport,baud);
end

%Define some servo IDs and goal positions.
ID1 = 0;
goalPos1 = 1000;

setMXpositionPassthrough(ID1,goalPos1,port);

port.delete();
clear port

function success = setMXpositionPassthrough(ID,goalPosition,port)
    mxGoalPositionAddr = 30;
    toWrite = 3;
    packetLength = 5; %length of the message after this: 1 for toWrite, 1 for address, 2 for command, 1 for checksum.
    
    lowByte = mod(goalPosition,256);
    highByte = floor(goalPosition/256);
    
    packet = [255,255,ID,packetLength,toWrite,mxGoalPositionAddr,lowByte,highByte];
    preChecksum = sum(packet(3:end));
    preChecksumBinary = int2bit(preChecksum,8);
    checksum = bit2int(~preChecksumBinary,8);
    packet(end+1) = checksum;
    
    port.write(packet,"uint8");

    success = true;
end

This code will construct a packet for the servo and send it to the OpenCM9.04. Now, we must write a sketch for the OpenCM9.04 that will receive this message and relay it to the servos. In the Arduino IDE:

/*
 * Dynamixel : MX-series with Protocol 1.0
 * Controller : OpenCM9.04C + OpenCM 485 EXP
 * Power Source : SMPS 12V 5A
 * 
 * AX-Series are connected to Dynamixel BUS on OpenCM 485 EXP board or DXL TTL connectors on OpenCM9.04
 * http://emanual.robotis.com/docs/en/parts/controller/opencm485exp/#layout
 * 
 * This example will test only one Dynamixel at a time.
*/

#include <DynamixelSDK.h>

// AX-series Control table address
#define ADDR_AX_TORQUE_ENABLE           24                 // Control table address is different in Dynamixel model
#define DXL_ID                          1

// Protocol version
#define PROTOCOL_VERSION                1.0                 // See which protocol version is used in the Dynamixel

// Default setting
#define BAUDRATE                        1000000 //1000000
#define DEVICENAME                      "1"                 //DEVICENAME "1" -> Serial1(OpenCM9.04 DXL TTL Ports)
                                                            //DEVICENAME "2" -> Serial2
                                                            //DEVICENAME "3" -> Serial3(OpenCM 485 EXP)
#define TORQUE_ENABLE                   1                   // Value for enabling the torque

#define ESC_ASCII_VALUE                 0x1b

void setup() {
  // Open the serial port with the PC:
  Serial.begin(1000000);

  // Initialize PortHandler instance
  // Set the port path
  // Get methods and members of PortHandlerLinux or PortHandlerWindows
  dynamixel::PortHandler *portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);

  // Initialize PacketHandler instance
  // Set the protocol version
  // Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
  dynamixel::PacketHandler *packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);

  int dxl_comm_result = COMM_TX_FAIL;             // Communication result
  uint8_t dxl_error = 0;                          // Dynamixel error

  // Open port
  portHandler->openPort();
  
  // Set port baudrate
  portHandler->setBaudRate(BAUDRATE);

  // Enable Dynamixel Torque
  dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_AX_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    packetHandler->getTxRxResult(dxl_comm_result);
  }
  else if (dxl_error != 0)
  {
    packetHandler->getRxPacketError(dxl_error);
  }
  else
  {
    //Dynamixel detected and torque is enabled
  }

  //Create a buffer for incoming serial messages, "packet".
  uint8_t packet[100];
  int bytesAvailable;

  while(true)
  {
    //First, read bytes coming over the serial port and send them to the servos
    bytesAvailable = Serial.available();
    if(bytesAvailable)
    {
      Serial.readBytes(packet,bytesAvailable);
      portHandler->writePort(packet,bytesAvailable);
    }
  }  
}

void loop() {
  // do nothing

}

Running this program on your OpenCM9.04, then running the script in Matlab, should cause the servo to rotate to the position you command in Matlab. Note a couple things about this Arduino sketch:

• The meat of the program, lines 72-82, simply read what has been sent to them and write it to the serial port to which the servos are connected. It is literally taking in bytes and handing them off.

• We use the "portHandler" class to write data to the servos (line 79), but we do not need the "packetHandler" class, because the data received by the microcontroller is already formatted in packets that the servos understand.

  • This means we know what the packetHandler has been doing the whole time: it just takes in the parameters we provide (e.g., servo ID, memory address, values to write to memory), formats a packet that the servos can interpret, and sends it using the portHandler.

We can try a fancier example in which we generate a sinusoidal trajectory for the servo to follow, then send "breadcrumbs" (i.e., specific angles) spaced out over time. The OpenCM9.04's program should not need to change. However, the Matlab script should be updated:

if exist("port") %#ok<EXIST> 
    %Do not make a new serialport object, just flush this one.
    port.flush();
else
    %Make a new serialport object.
    comport = "COM4";
    baud = double(1000000);
    port = serialport(comport,baud);
end

%Define some servo IDs and goal positions.
ID1 = 0;
goalPos1 = 1000;
goalPos2 = 2000;

timeDuration = 6; %second
numCommands = 200;
dt = timeDuration/numCommands;
T = 2; %period, seconds
omega = 2*pi/T;
t = dt*(1:numCommands);
goalPositions = round(goalPos1 + (goalPos2-goalPos1)*(1/2)*(1-cos(omega*t)));

%Move servo to starting position, then wait half a second
setMXpositionPassthrough(ID1,goalPos1,port);
pause(0.5);

%Run through the trajectory, ensuring that each loop has duration "dt".
for i=1:numCommands
    loopTimer = tic;
    
    setMXpositionPassthrough(ID1,goalPositions(i),port);

    while toc(loopTimer) < dt
        %do nothing
    end
end

port.delete();
clear port

function success = setMXpositionPassthrough(ID,goalPosition,port)
    mxGoalPositionAddr = 30;
    toWrite = 3;
    packetLength = 5; %length of the message after this: 1 for toWrite, 1 for address, 2 for command, 1 for checksum.
    
    lowByte = mod(goalPosition,256);
    highByte = floor(goalPosition/256);
    
    packet = [255,255,ID,packetLength,toWrite,mxGoalPositionAddr,lowByte,highByte];
    preChecksum = sum(packet(3:end));
    preChecksumBinary = int2bit(preChecksum,8);
    checksum = bit2int(~preChecksumBinary,8);
    packet(end+1) = checksum;
    
    port.write(packet,"uint8");

    success = true;
end

Receiving data on a PC from the OpenCM9.04, which runs a "passthrough" sketch that passes data along from the servos

Let's expand our programs to ensure we can read information from the servo as it moves. This will require we change both the Matlab script and the Arduino sketch. Specifically:

• Matlab must listen for bytes returning along the serial port and read them to clear the buffer

• Matlab must interpret packets sent back from the OpenCM9.04 if they contain useful data

• Arduino must tell the servos to report their states

• Arduino must send the packets it receives back to Matlab

First, let's update the Matlab script. Two changes will need to be made:

1. As discussed in reading servo motions with a personal computer, the servo returns a 6-byte status packet every time we send a command. Thus, our "setMXposition" function will need to read 6 bytes from the buffer before continuing, to prevent the buffer from accumulating garbage

2. We will need to define and use a "getMXposition" function that will send the "read" instruction, listen for the response packet, and decode its contents

Here's a new script with each of these functions added/updated:

if exist("port") %#ok<EXIST> 
    %Do not make a new serialport object, just flush this one.
    port.flush();
else
    %Make a new serialport object.
    comport = "COM4";
    baud = double(1000000);
    port = serialport(comport,baud);
end

%Define some servo IDs and goal positions.
ID1 = 0;
goalPos1 = 1000;
goalPos2 = 2000;

timeDuration = 6; %second
numCommands = 200;
dt = timeDuration/numCommands;
T = 2; %period, seconds
omega = 2*pi/T;
t = dt*(1:numCommands);
goalPositions = round(goalPos1 + (goalPos2-goalPos1)*(1/2)*(1-cos(omega*t)));

%Move servo to starting position, then wait half a second
setMXpositionPassthrough(ID1,goalPos1,port);
pause(0.5);

%Continuously write and read positions
presPositions = NaN(size(goalPositions));

for i=1:numCommands
    loopTimer = tic;
    
    setMXposition(ID1,goalPositions(i),port);
    presPositions(i) = getMXposition(ID1,port);

    while toc(loopTimer) < dt
        %do nothing
    end
end

figure
plot(t,goalPositions)
hold on
plot(t,presPositions)
xlabel('time (s)')
ylabel('position (a.u.)')
legend('goal','read')

port.delete();
clear port

function success = setMXposition(ID,goalPosition,port)
    mxGoalPositionAddr = 30;
    toWrite = 3;
    packetLength = 5; %length of the message after this: 1 for toWrite, 1 for address, 2 for command, 1 for checksum.
    
    lowByte = mod(goalPosition,256);
    highByte = floor(goalPosition/256);
    
    packet = [255,255,ID,packetLength,toWrite,mxGoalPositionAddr,lowByte,highByte];
    preChecksum = sum(packet(3:end));
    preChecksumBinary = int2bit(preChecksum,8);
    checksum = bit2int(~preChecksumBinary,8);
    packet(end+1) = checksum;
    
    port.write(packet,"uint8");

    %Read the status packet
    while port.NumBytesAvailable < 6
        %do nothing
    end

    port.read(6,"uint8");

    success = true;
end

function [presentPosition, success] = getMXposition(ID,port)
    mxPresentPositionAddr = 36;
    toRead = 2;

    %Construct the instruction packet to read from the servo.
    packetLength = 4;
    numBytesPresPos = 2;
    packet = [255,255,ID,packetLength,toRead,mxPresentPositionAddr,numBytesPresPos];
    preChecksum = sum(packet(3:end));
    preChecksumBinary = int2bit(preChecksum,8);
    txChecksum = bit2int(~preChecksumBinary,8);
    packet(end+1) = txChecksum;
    
    %Write the instruction packet to read from the servo.
    port.write(packet,"uint8");
    
    while port.NumBytesAvailable < 8
        %Do nothing; we just wait for the servo to receive the command to send
        %us data and for the data to be transmitted.
    end
    
    %Read the available bytes from port's buffer.
    data = port.read(8,"uint8");
    
    %Calculate the checksum to ensure the data was not corrupted.
    preChecksum = sum(data(3:end-1));
    preChecksumBinary = int2bit(preChecksum,8);
    rxChecksum = bit2int(~preChecksumBinary,8);
    
    %If the checksums match, and if you get data from the intended servo,
    %take in the data and construct the actual read
    %value from the low and high bytes. Else, report success as false.
    if data(end) == rxChecksum && data(3) == ID
        lowByte = data(end-2);
        highByte = data(end-1);
        presentPosition = highByte*256+lowByte;
        success = true;
    else
        presentPosition = NaN;
        success = false;

    end
end

We must also update the sketch running on the OpenCM9.04, because in its current form, it does not request any info from the servos and does not send it back to the PC. Here is an updated sketch:

/*
 * Dynamixel : AX-series with Protocol 1.0
 * Controller : OpenCM9.04C + OpenCM 485 EXP
 * Power Source : SMPS 12V 5A
 * 
 * AX-Series are connected to Dynamixel BUS on OpenCM 485 EXP board or DXL TTL connectors on OpenCM9.04
 * http://emanual.robotis.com/docs/en/parts/controller/opencm485exp/#layout
 * 
 * This example will test only one Dynamixel at a time.
*/

#include <DynamixelSDK.h>

// AX-series Control table address
#define ADDR_AX_TORQUE_ENABLE           24                 // Control table address is different in Dynamixel model
#define DXL_ID                          1

// Protocol version
#define PROTOCOL_VERSION                1.0                 // See which protocol version is used in the Dynamixel

// Default setting
#define BAUDRATE                        1000000 //1000000
#define DEVICENAME                      "1"                 //DEVICENAME "1" -> Serial1(OpenCM9.04 DXL TTL Ports)
                                                            //DEVICENAME "2" -> Serial2
                                                            //DEVICENAME "3" -> Serial3(OpenCM 485 EXP)
#define TORQUE_ENABLE                   1                   // Value for enabling the torque

#define ESC_ASCII_VALUE                 0x1b

void setup() {
  // Open the serial port with the PC:
  Serial.begin(1000000);

  // Initialize PortHandler instance
  // Set the port path
  // Get methods and members of PortHandlerLinux or PortHandlerWindows
  dynamixel::PortHandler *portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);

  // Initialize PacketHandler instance
  // Set the protocol version
  // Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
  dynamixel::PacketHandler *packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);

  int dxl_comm_result = COMM_TX_FAIL;             // Communication result
  uint8_t dxl_error = 0;                          // Dynamixel error

  // Open port
  portHandler->openPort();
  
  // Set port baudrate
  portHandler->setBaudRate(BAUDRATE);

  // Enable Dynamixel Torque
  dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_AX_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    packetHandler->getTxRxResult(dxl_comm_result);
  }
  else if (dxl_error != 0)
  {
    packetHandler->getRxPacketError(dxl_error);
  }
  else
  {
    //Dynamixel detected and torque is enabled
  }

  //Create a buffer for incoming serial messages, "packet".
  uint8_t packet[100];
  int bytesAvailable;

  while(true)
  {
    //First, read bytes coming over the serial port and send them to the servos
    bytesAvailable = Serial.available();
    if(bytesAvailable)
    {
      Serial.readBytes(packet,bytesAvailable);
      portHandler->writePort(packet,bytesAvailable);
    }

    //Second, read any available info from the servos' buffers and send them back over serial
    bytesAvailable = portHandler->getBytesAvailable();
    if(bytesAvailable)
    {
      portHandler->readPort(packet,bytesAvailable);
      Serial.write(packet,bytesAvailable);
    }
  }  
}

void loop() {
  // put your main code here, to run repeatedly:

}

Now, the updated sketch will read from the servo in every loop and transmit what it reads back to the PC.

If our robot had other sensors (e.g., strain gauges, accelerometers, gyroscopes), we could read from those as well, then send their data to the PC.

Programs such as ROS are intended to help solve this problem of actuator and sensor integration. However,

1. ROS 1 was not real-time, and while ROS 2 claims to be real-time, I don't know what the limitations are.

2. Oftentimes tools that are designed to solve every problem are not great at solving any single problem. ROS makes it easier to connect actuators and sensors that don't normally talk to one another. However, our robots have MANY actuators and sensors. ROS's universality produces lots of overhead that slows down operation in a way that is not acceptable for us, because we already have trouble running programs quickly with so many actuators and sensors.

3. If you understand how this stuff works at the low level, it sets you up to be a real robotics expert. You will have the ability to solve and troubleshoot many different problems and produce a specialized solution for your particular problem. I think that's legit.