Reading Servo Motions with a Personal Computer

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

Why would I want to read information from the Dynamixel smart servos?

Robotics has traditionally focused on calculating and following "trajectories", that is, sequences of robot postures over time. However, an important part of the work we do in this lab is to better understand how animals process sensory feedback as a part of control and how robot performance could be enhanced by such feedback. Therefore, it is important that our robots measure things such as joint angles, angular velocities, joint torques, foot forces, segment bending, and other body states.

How does the servo send information to the main computer?

Reciprocal to the previous tutorial, the servo sends information to the computer, which is temporarily stored in a serial buffer and then read byte-by-byte. The servo sends data in two different contexts:

1. Dynamixel status packets, as explained previously, and

2. In response to an instruction packet that contains a "Read" command.

1. Dynamixel status packets

These were explained in the last tutorial, but they are mentioned here briefly to correct a lie by omission. Specifically, every time we called u2d2.write(), the servo sent 6 bytes into the computer's serial port buffer, which we never read. You may want your control program to make use of this information (e.g., if the temperature of a servo becomes too high, set off an alarm or alter the controller to use that servo less), or you may want to clear the buffer so that it does not interfere with later messages you wish to receive (recall that data is read from the buffer first-in-first-out). Here are some options:

If you want to read the packets, use serialport.read(6,"uint8") to take in the message:

comport = "COM3";
baud = double(1000000);
u2d2 = serialport(comport,baud);

%construct a packet to send to the servo

u2d2.write(packet,"uint8");

while u2d2.NumBytesAvailable < 6
    %do nothing while we wait for the buffer to fill
end

returnPacket = u2d2.read(u2d2.NumBytesAvailable,"uint8");

If you do not wish to read the packets, use serialport.flush() to empty the buffer:

comport = "COM3";
baud = double(1000000);
u2d2 = serialport(comport,baud);

%construct a packet to send to the servo

u2d2.write(packet,"uint8");

while u2d2.NumBytesAvailable < 6
    %do nothing while we wait for the buffer to fill
end

u2d2.flush();

However, this could also delete the info you wanted to read. Thus, the best option may be to read the status return packet every time you send a command, even if you do not use it for anything.

Here's a basic program that does everything:

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

toRead = 2;
toWrite = 3;
toSyncWrite = 131; %0x83

mxGoalPositionAddr = 30;
mxPresentPositionAddr = 36;

ID1 = 1;
ID2 = 2;
goalPos1 = 1000;
goalPos2 = 1000;

%Write data to the MX
%Sentence: 0xFF,0xFF,ID,Length,Instruction,Parameters...,Check Sum
packetLength = 5; %length of the message after this: 1 for toWrite, 1 for address, 2 for command, 1 for checksum.
lowByte = mod(goalPos1,256);
highByte = floor(goalPos1/256);
packet = [255,255,ID1,packetLength,toWrite,mxGoalPositionAddr,lowByte,highByte];
% preChecksum = sum(packet(3:end));
% preChecksumBinary = int2bit(preChecksum,8);
% checksum = bit2int(~preChecksumBinary,8);
% packet(end+1) = checksum;
[~,packet(end+1)] = checksumMatch(packet,NaN);

u2d2.write(packet,"uint8");

waitForReturn = tic;
while u2d2.NumBytesAvailable < 6
    %do nothing
end
toc(waitForReturn)

u2d2.read(u2d2.NumBytesAvailable,"uint8")

%Write data to both MX simultaneously
L = 2; %Writing a position takes 2 bytes
N = 2; %There are two dynamixels in this example
packetLength = ((L + 1) * N) + 4;
IDbroadcast = 254;
lowByte1 = mod(goalPos1,256);
highByte1 = floor(goalPos1/256);
lowByte2 = mod(goalPos2,256);
highByte2 = floor(goalPos2/256);
packet = [255,255,IDbroadcast,packetLength,toSyncWrite,mxGoalPositionAddr,2,ID1,lowByte1,highByte1,ID2,lowByte2,highByte2];

preChecksum = sum(packet(3:end));
preChecksumBinary = int2bit(preChecksum,8);
checksum = bit2int(~preChecksumBinary,8);
packet(end+1) = checksum;

u2d2.write(packet,"uint8");

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


for i=1:numCommands
    loopTimer = tic;
    
    setMXposition(ID1,goalPositions(i),u2d2);

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

pause(1)

%Read data from the MX

%Flush the buffer, so we are not confused by reading old data.
% fprintf('serial port has %i bytes available.\n',u2d2.NumBytesAvailable)
% u2d2.flush();

%Construct the instruction packet to read from the servo.
packetLength = 4;
numBytesPresPos = 2;
packet = [255,255,ID1,packetLength,toRead,mxPresentPositionAddr,numBytesPresPos];
preChecksum = sum(packet(3:end));
preChecksumBinary = int2bit(preChecksum,8);
checksum = bit2int(~preChecksumBinary,8);
packet(end+1) = checksum;

%Write the instruction packet to read from the servo.
u2d2.write(packet,"uint8");

while u2d2.NumBytesAvailable == 0
    %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 u2d2's buffer.
data = u2d2.read(u2d2.NumBytesAvailable,"uint8");

%Calculate the checksum to ensure the data was not corrupted.
preChecksum = sum(data(3:end-1));
preChecksumBinary = int2bit(preChecksum,8);
checksum = bit2int(~preChecksumBinary,8);

%If the checksums match, take in the data and construct the actual read
%value from the low and high bytes. Else, throw an error.
if data(end) == checksum
    ID = data(3);
    lowByte = data(end-2);
    highByte = data(end-1);
    presentPosition = highByte*256+lowByte;
else
    error('Error in reading position of servo %i.',ID1)
end

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

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

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

u2d2.delete();
clear u2d2

Here is a function for reading the position, packaged up all together:

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

Here's a function for setting the position:

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

Here's a function for calculating the checksum:

function [match,checksum] = checksumMatch(packet,checksumIndex)
    %Provide the packet and which index in the packet is the checksum for
    %comparison.
    %
    %Calculate the checksum. If they match, return true.
    %
    %You can use this function to simply calculate the checksum without
    %comparing it to anything by passing checksumIndex = NaN or [].
    
    if length(packet) <= 2
        error('Input "packet" must have length of 3 or more.')
    end

    %justCalc is a boolean that determines if we are supposed to just calculate the 
    % checksum value for this packet (true), or if we must also compare
    % the calculated value to the provided packet (false).
    %our calculated checksum to the checksum in the packet
    justCalc = (isnan(checksumIndex) || isempty(checksumIndex));

    if justCalc
        preChecksum = sum(packet(3:end));
        preChecksumBinary = int2bit(preChecksum,8);
        checksum = bit2int(~preChecksumBinary,8);
        match = NaN;
    else
        %Save the provided checksum, then remove that index from the
        %packet.
        providedChecksum = packet(checksumIndex);

        packet(checksumIndex) = [];
        preChecksum = sum(packet(3:end));
        preChecksumBinary = int2bit(preChecksum,8);
        checksum = bit2int(~preChecksumBinary,8);

        if providedChecksum == checksum
            match = true;
        else
            match = false;
        end
    end
end