Programming Servo Motions with a Personal Computer

How to send commands from a computer to the servos; how to read status messages from the servos

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.

How is data transferred to and from the Dynamixels?

The short answer is through serial packages with a specific format. The Dynamixels have two different protocols for how to format serial packages, Protocol 1.0 and Protocol 2.0. This tutorial will focus on Protocol 1.0 because it is simpler. However, Protocol 2.0 has more useful features, so in practice, we will use that more often. That said, the main ideas we will explore with Protocol 1.0 apply to Protocol 2.0.

The long answer is that we must send single bytes of data sequentially (i.e., in "serial") to the Dynamixels that encode what we want them to do:

Introduction to Low Level Dynamixel Programming

Dynamixel servo are controlled by receiving instructional packets, and sending status (or return) packets. A more detailed description of how this works can be found here, but this section will attempt to highlight the relevant components for control with the OpenCM microcontroller.

This section is important for writing your own libraries later. For extremely basic control of Dynamixel servos, this section is not as relevant; but as soon as you want to do more complicated maneuvers with the servos this section is necessary.

1. Binary Data and Decimal Conversion

Dynamixel servos are commanded by receiving packets of binary instructions. You are controlling the servos by sending 0s and 1s to them, in a specific order; the servos will interpret the packets and perform what you command them. An analogy that I have found helpful when thinking of this communication is to think of each package of data as a sentence. One bit represents one 0 or 1, and this can be thought of as the letters that make up the words of data. One byte (8 bits) can be thought of as a full word. Each byte is made up of eight 0s and 1s in any combination that can be converted into integer data. This is important for understanding how to successfully utilize existing Dynamixel libraries.

If one bit is a letter, and one byte is a word, then a package can be thought of as a sentence, or a command. It is comprised of several bytes of data in machine language that the Dynamixel will understand and interpret. Dynamixel servos will only understand a specific format of packets, so you need to follow the proper syntax.

The technical term for a “sentence” of data, or a collection of bytes, is a packet of data. When working with Dynamixel servos, we will send instructional packets, and the servos will then send status packets.

Another thing to consider when working with Dynamixel servos is that we will almost always be working with positive numbers – more specifically positive integers. The term for this in computer science is an unsigned integer. An unsigned integer is always a non-negative number. A computer can interpret a wider range of positive numbers with less data when it knows to expect unsigned values; this is because the first bit in a signed integer is typically reserved for the sign (positive or negative). An unsigned integer does not have to reserve this bit for the sign, so it can utilize all bits of data to represent the number. More on signedness can be found at the website linked.

The maximum value of a single byte is 1111 1111. As an unsigned integer, this is represented in decimal as 255. Refer back to the conversion instructions to learn how to convert an 8 bit binary number to a decimal integer. The minimum value of a single byte is 0000 0000, which in decimal is just 0. What this means is that each word (or byte) that makes up our packet of data is limited between 0 and 255 (which is 2^8=256 possible values).

2. Hexadecimal Representation

Another way to represent binary data is through hexadecimal conversion. This is a base 16 numerical system rather than base 2 (binary) or base 10 (decimal). The advantages of hexadecimal are that it is much simpler to convert binary to hexadecimal, and one byte of data can always be represented by two hexadecimal characters. When converting binary to hexadecimal you can break up the binary number into 4 bit values, and convert each 4 bit value into its hexadecimal representation. When you have converted each 4 bit value, you just concatenate (put together) the converted values to get the resulting hexadecimal number.

This is convenient because the maximum decimal number you can get from 4 bits is 15, and in binary this is represented by 1111. The range for hexadecimal is 0 to 15, but 10, 11, 12, 13, 14, and 15 are represented using A – F. So 1111 in binary (15 in decimal) maps to F in hexadecimal. The result is that each 4 bit number can be represented by a single hexadecimal character (0 to F). So a one byte value (8 bits) can be represented by 2 hexadecimal values. A 2 byte (16 bit) binary value can be represented by 4 hexadecimal values, and so on. When writing libraries to send packets of instruction to Dynamixel servos, it is often convenient to use hexadecimal values.

3. Dynamixel Communication Overview

Now that we understand how binary, decimal, and hexadecimal numerical representations can be converted, we can look at how we physically communicate with Dynamixel servos. We have already discovered that in order to control Dynamixel servos at the lowest level, the data must be sent in byte packets. Typically when building our packets of data, we will use hexadecimal representation. Again, the advantage of using hexadecimal to write out our data is that each byte takes up only two characters.

The user must assemble an instruction packet using syntax the servors can interpret. This is explained in detail below. The controller then broadcasts the packet to the servo(s) with the ID specified in the instruction packet. There is a generic broadcasting ID (0xFE or 254) that will send the command to all connected servos. Each servo has a portion of memory called its "buffer". Packets sent to the servo are stored in buffer in a first-in-first-out fashion.Once the servo(s) receives the packet, it executes the command that is specified in the packet. A status (or return) packet is then assembled and sent back to the controller (either your laptop or the openCM).

4. Dynamixel Instructional Packets

An instruction packet is command data that is sent to the main controller, which then sends it to Dynamixel servos. The structure of an instruction packet is shown on the Protocol 1.0 page, but is also described below.

The first two bytes that will make up our instructional packet of data will always be 0XFF 0XFF. I know in the previous section I said bytes represented in hexadecimal can always be represented by two characters, but that was only partially true. The physical value is represented by two characters (in this case FF); the 0X tells the microcontroller to expect a hexadecimal value following it. The FF in hexadecimal is equal to 255 in decimal, or 1111 1111 in binary. What the 0XFF 0XFF does is notifies the controller that it is the beginning of the packet. The binary equivalent of 0XFF 0XFF is 1111 1111 1111 1111. You can see that the binary representation is lengthy; this is why hexadecimal is the preferred numeric representation.

The next number in the packet is the ID. Dynamixel servos have a default ID value of 1 (out of the box), and can be set to anything between 0 and 253 (0X00 – 0XFD in hexadecimal). You can use a value of 254 (0XFE) to execute the command of instruction to all linked Dynamixel servos. This is known as the broadcasting ID. What this byte does is tells the controller which servo (or servos if using the broadcasting ID) to send the instructions to.

The next byte of data in the packet signifies the length of packet, which is going to be equal to the number of parameters + 2. The +2 comes from the form of the instruction (one byte), and from the checksum at the end of the packet (one byte). This lets the controller know how many additional bytes of data to expect as it executes the packet.

The following byte is reserved for the type of instruction to send. A table of the instruction types that can be sent can be found at the Robotis link above, or in the table below. We will get more into the specifics of the instruction types in an example.

Table 4.1 - Instruction Data

Byte Value	Name	Function	Number of Parameters
0x01	PING	No execution. It is used when controller is ready to receive Status Packet	0
0x02	READ_DATA	This command reads data from Dynamixel	2
0x03	WRITE_DATA	This command writes data to Dynamixel	2 or more
0x04	REG WRITE	It is similar to WRITE_DATA, but it remains in the standby state without being executed until the ACTION command arrives.	2 or more
0x05	ACTION	This command initiates motions registered with REG WRITE	0
0x06	RESET	This command restores the state of Dynamixel to the factory default setting.	0
0x83	SYNC WRITE	This command is used to control several Dynamixels simultaneously at a time.	4 or more

The next N bytes of data to be sent are the parameters of the instruction. For example, if we were to write data to a servo, the parameters would have to include at least two things: first, we would have to send data that represents the property of the servo we want to modify (goal position, maximum torque output, ID number, etc…), and second, we would have to send the value that we want that property to contain. So if we wanted to change a servo’s ID value, the first parameter would be the memory address that stores the ID property, and the second parameter would be our new ID value. Each Dynamixel servo has a similar core mapping of properties, but different series vary. You can find the tables that show each readable and writeable property for each series at the following link (navigate to the model of interest).

The last byte of data is the checksum. The checksum checks to see if the packet was corrupted upon transmission to the controller. It adds the values of all of the previous bytes of data and does some post-processing to avoid errors in most cases. The checksum does not include the first two bytes (0XFF 0XFF). Obviously, the checksum value could be (and will often be) greater than 255 (or 0XFF). If this is the case, then it is not possible to store this value in one byte, so the checksum does not just add the value of all previous bytes of data. It adds them all up, takes the lower byte of this added value (in hexadecimal it will always be the last two characters), and takes the opposite value. The way the opposite value is taken is by using the not bit operator (“~” in Matlab, "!" in other languages) which essentially takes the binary value, and every time it encounters a 0 it replaces it with a 1, and every time it encounters a 1 it replaces it with a 0. We will examine this in an example in the next section. It is not 100% effective because of the nature of sending data in individual bytes, but it is effective most of the time.

The checksum is calculated both upon sending and receiving of the packet, and then the two values are compared. The value that is calculated prior to sending the packet is the last physical byte of the packet. Upon receiving the packet, it is calculated again and compared to that last byte value.

5. Dynamixel Status (Return) Packets

One of the really powerful things that Dynamixel servos do after executing the commands specified in the instruction packet is send a packet of their current status back to the controller. The format of the status packet is nearly identical to that of the instruction packet:

0xFF 0xFF ID LENGTH ERROR PARAMETER1 PARAMETER2 ... PARAMETERN CHECKSUM

The only difference is instead of the “Instruction” byte, the status packet sends back an “Error” byte. More detailed information can be found on the Robotis site.

Again, the 0XFF 0XFF indicates the beginning of the packet, and the ID byte indicates which servo is transferring the status packet. The length byte is again the length of the packet, and can be calculated as N (the number of parameters) + 2. The +2 comes from the error byte and the checksum byte; the main difference between the instruction packet and the return packet is the instruction byte versus the error byte.

The next byte is the error byte. There are 7 potential causes of error in the Dynamixel servo, summarized below in Table 4.2, and also found on the Robotis support site.

Table 4.1 – Example Status Packet Error Byte Definition

Bit	Name	Contents
Bit 7	0	-
Bit 6	Instruction Error	In case of sending an undefined instruction or delivering the action command without the reg_write command, it is set as 1.
Bit 5	Overload Error	When the current load cannot be controlled by the set Torque, it is set as 1.
Bit 4	Checksum Error	When the Checksum of the transmitted Instruction Packet is incorrect, it is set as 1.
Bit 3	Range Error	When a command is out of the range for use, it is set as 1.
Bit 2	Overheating Error	When internal temperature of Dynamixel is out of the range of operating temperature set in the Control table, it is set as 1.
Bit 1	Angle Limit Error	When Goal Position is written out of the range from CW Angle Limit to CCW Angle Limit, it is set as 1.
Bit 0	Input Voltage Error	When the applied voltage is out of the range of operating voltage set in the Control table, it is as 1.

The error byte is capable of identifying any of these potential causes of error using only one byte of data. For example if I had an error due to overheating, my error byte would look like 0010 0000. In this case, all bits are set to 0, except bit 2 (starting from bit 0) which signifies an overheating error. Note: This table represents an example of what each bit represents in the error byte, and this may be slightly different for different models. To determine exactly how to interpret the error byte for your model, reference the manual specific to the model you are using.

The parameter bytes represent the data that the servo is sending back to the controller. This would be used if you wanted to determine the current position of a servo. You could send an instruction packet command that would be to read the data of the position and this data would be sent back in the status packet using the parameter bytes.

The checksum byte works the same way in the status packet as it does in the instruction packet.

6. Dynamixel Packet Examples

Additional examples can be found here, but we will go through two examples in this text before getting into higher level control. These examples are slightly simplified; in reality you are sending this data into the serial buffer of the robocontroller which will ultimately be sent directly to the servos, but the following demonstrates the format of the data that is being passed back and forth. Some more information about the buffer can be found here.

Example 1: Write Data

How would we change the ID of an AX-12A servo from 1 to 5? Let’s work through the packet of data we would have to send to write the new value of the ID (5) to the servo in the correct EEPROM address. Refer to Appendix 3 for the detailed difference between EEPROM and RAM.

Step 1: Beginning of packet

We know each packet will begin with the two bytes 0XFF 0XFF to signify that it is indeed the beginning of the packet. Our example is no different. So the beginning of our packet will just be 0XFF 0XFF.

Packet so far: 0XFF 0XFF

Step 2: ID

The next byte of data that will make up our packet is the ID of the servo that we are writing data to. In this case, we are sending data to the servo with ID #1, so the value of this byte should just be 1. For consistency, we will represent all bytes of data in hexadecimal format. Our ID byte will be 0X01.

Packet so far: 0XFF 0XFF 0X01

Step 3: Length

The next byte of data will tell our servo with ID #1 how many more bytes of data to expect. In this case, we will need two parameters (one to tell the servo that we are changing the ID, and one to tell the servo the value we are changing it to); we will also need one byte for the instruction, and one byte for the checksum. So our length byte will be 4 (in decimal) or 0X04 in hexadecimal.

Packet so far: 0XFF 0XFF 0X01 0X04

Step 4: Instruction

Our instruction byte is to tell the servos what we are doing – it will signify whether we are writing data, reading data, simply pinging the system to inform the controller of system status, etc… All possible instructions for Dynamixel servos are listed in Table 4.1. If you refer back to the table you will see WRITE_DATA is signified by the value 3 (0X03), and this is the value we will want to use to write a new ID.

Packet so far: 0XFF 0XFF 0X01 0X04 0X03

Step 5: Parameters

Now we need our two parameters. I mentioned in step 3 three that the parameters would map to the address of the ID value in the EEPROM. In this case, we want to refer to Appendix 3 to see what that value is for the AX-12A servo. In this case it maps to address 3 (0X03) so our first parameter byte will be 0X03. Our next parameter is the value that we want to be stored in that address. Our goal is to change the ID of the servo to 5, so we want to write the value 5 to the address that represents ID, which we just discovered was 3. So our second parameter (5) will be 0X05.

Packet so far: 0XFF 0XFF 0X01 0X04 0X03 0X03 0X05

Step 6: Checksum

The last byte we have to send is the value of the checksum. This is probably the most difficult value to think about physically, but programming it is not as hard as it would seem. The checksum is equal to the opposite value of the low byte of the sum of all of the previous bytes. Let’s go carefully through this example.

Step 6a: Add up all data bytes (not the 0XFF 0XFF):

0X01 + 0X04 + 0X03 + 0X03 + 0X05 = 1 + 4 + 3 + 3 + 5 = 16

Step 6b: Convert to binary

16 = 2^4 = 0001 0000.

Step 6c: Take the opposite value

In computer science there is a not bit operator that reverses the individual bits of a byte. In C++ it is designated as ~ (not). What this will do is change each 0 in the byte to a 1, and each 1 in the byte to a 0. So we will take our binary sum that we just found and apply the not bit operator to it.

~(0001 0000) = 1110 1111.

Step 6d: Convert to hexadecimal and compile the final byte

1110 in decimal is 14, in hexadecimal that maps to E. 1111 in decimal is 15, in hexadecimal that maps to F. So 1110 1111 = 0XEF.

Final Instruction Packet: 0XFF 0XFF 0X01 0X04 0X03 0X03 0X05 0XEF

The checksum byte can be recalculated by the controller and compared the value that is sent. If they are mismatched, that means some data was lost in communication and the data may be corrupted.

Example 2: Read Data

For our read data example, let’s say we want to read the present voltage that the servo with ID 1 is receiving. Again, we will assume our servo is an AX-12A.

Step 1: Beginning of packet

It is still just 0XFF 0XFF; all packets will start this way when working with Dynamixel servos.

Packet so far: 0XFF 0XFF

Step 2: ID

The ID of the servo in which we are reading data is just 1, so our ID byte is 0X01

Packet so far: 0XFF 0XFF 0X01

Step 3: Length

In this case we will have two parameters, plus the instruction and the checksum. Our length will be 0X04. The two parameters are the address of the voltage, and the length of the data that we are reading (1 byte). We will get more into this in step 5.

Packet so far: 0XFF 0XFF 0X01 0X04

Step 4: Instruction

Since we are reading data now, we can refer back to Table 4.1 and determine what value represents reading data. If you look back at the table you will see that read data is designated by 0X02, so our byte for this instruction is 0X02.

Packet so far: 0XFF 0XFF 0X01 0X04 0X02

Step 5: Parameters

The first parameter as I mentioned in step 3 is the address of data to be read. This will always be the start address, so if reading from multiple addresses (for instance if you wanted to read both bytes that signify position), the first parameter will always be the starting address. In this case, we are reading voltage being supplied to the servo, and our address (found in Appendix 3) is 42 (or 0X2A). The second parameter will be the number of bytes of data that we are reading. Since voltage is represented with only one byte of data, our value for this byte is just 1 (or 0X01). If we wanted to read from multiple addresses, we would need an additional byte for each address.

Packet so far: 0XFF 0XFF 0X01 0X04 0X02 0X2A 0X01

Step 6: Checksum

Now we are going to want to calculate our checksum value and attach it to the end of our packet so that the controller can verify that no data was lost upon sending the packet. We will take the same steps that we did in example 1. Step 6a: Add up all the data bytes (excluding the 0XFF 0XFF beginning of packet)

0X01 + 0X04 + 0X02 + 0X2A + 0X01 = 1 + 4 + 2 + 42+ 1 = 50

Step 6b: Convert to Binary

50 = 0011 0010

Step 6c: Take the opposite value

~(0011 0010) = 1100 1101

Step 6d: Convert to hexadecimal and compile the final byte

1100 = 12 in decimal which maps to C in hexadecimal

1101 = 13 in decimal which maps to D in hexadecimal

Checksum byte = 0XCD

Final Packet: 0XFF 0XFF 0X01 0X04 0X02 0X2A 0X01 0XCD

If these examples are still difficult to comprehend, more examples can be found at the site listed at the beginning of the examples, which is reproduced here.

How do we automate this process?

You can write scripts that facilitate the sending and receiving of serial packets. The following code snippets will enable you to do is in Matlab.

First, you must open a serial port. Determine which port your U2D2 is utilizing and the baud rate of your servos. Using this information, you can instantiate a "serialport" object in Matlab:

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

Note that the serialport requires that the baud rate be specified as a double-precision integer.

Your serialport has many methods, e.g., write(), read(), that will help you easily send your serial packet, which you will see below.

Next, we will define a couple variables for convenience in the following code. I define the command values from the table above:

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

Then, I define the memory addresses of the values I am going to write and read:

mxGoalPositionAddr = 30;
mxPresentPositionAddr = 36;

Finally, I define the IDs of my two servos and a couple goal positions:

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

Now, we construct a serial packet to write to the servos. We will simply write goalPos1 to servo ID1. The packet must be a vector of values. Each scalar value in the vector must not be greater than 255 because we will eventually send them one-by-one as unsigned integer bytes.

Recall that our packet must read 0xFF, 0xFF, ID of the servo we are talking to, length of the packet, instruction (e.g., read, write), parameters (memory address + values to write), checksum. Oddly, the first thing we must specify is the length of the packet after the length byte. In this example, that is an instruction (1 byte), memory address (1 byte), value to write to that address (2 bytes for goal position), and checksum (1 byte), for a total of 5 bytes.

packetLength = 5; %length of the message after this: 1 for toWrite, 1 for address, 2 for command, 1 for checksum.

Note that some values only require 1 byte (e.g., ID), and that we may also choose to write as many bytes as we'd like in sequence. For example, because goal position is defined by two bytes starting at address 30 and movement speed is defined by two bytes starting at address 32, you could write 4 bytes in one packet, which would change the length.

Sending multiple instructions in one packet in this way would reduce the communication overhead by adding a couple bytes to this packet, instead of requiring we send two separate packets. This would also require only one Delay Return Time.

Next, because goal position may be larger than 2^8-1 = 255 (for MX series, 2^12-1 = 4095), we must split up the goal position into a low byte and a high byte. For example, the decimal goal position 1000 = 0000 0011 1110 1000 in binary, where the high byte is in bold, and the low byte is in italics. The low byte is 1000%256; it is simply the remainder when you divide 1000 by 256. The high byte is floor(1000/256), the rounded-down integer when you divide 1000 by 256.

The 1000%256 operation could also be represented as a "bitwise AND" operation. Logical AND is the same as multiplication, so to keep the low byte, we can create a "binary mask":

0000 0011 1110 1000 & 0000 0000 1111 1111 = 0000 0000 1110 1000

The floor(1000/256) operation could also be represented as a "bitwise shift", indicated by >>. floor(1000/(2^8)) is the same as 1000 >> 8: 0000 0011 1110 1000 >> 0000 1000 = 0000 0000 0000 0011

These representations are valuable if you are writing low-level code that needs to be very efficient, e.g., on a microcontroller.

Then, we must send the low byte first, which always bends my mind a bit. So here is how I would construct my sentence, minus the checksum:

lowByte = mod(goalPos1,256);
highByte = floor(goalPos1/256);
packet = [255,255,ID1,packetLength,toWrite,mxGoalPositionAddr,lowByte,highByte];

Now we must calculate the checksum: sum the packet excluding the first two bytes, 0xFF 0xFF; isolate the low byte of this sum; invert each bit from 0 to 1 or 1 to 0; concatenate this byte to the packet:

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

Finally, we send the command to the servo, specifying that the decimal values we put into our packet should be represented as unsigned 8-bit integers (i.e., single bytes):

u2d2.write(packet,"uint8"); 

Wow, that part was easy! What a wise person would do is write a function that does all the steps we just did automatically. Here is a simple function that would enable you to send many servo positions in sequence without rewriting a bunch of code:

function success = setMXposition(ID,goalPosition,port)
    mxGoalPositionAddr = 30; %memory address for the goal position of the MX servo
    toWrite = 3; %instruction to write a message
    packetLength = 5; %length of the message after this: 1 for toWrite, 1 for address, 2 for command, 1 for checksum.
    
    %Split the goal position into low and high bytes:
    lowByte = mod(goalPosition,256);
    highByte = floor(goalPosition/256);
    
    %Construct the packet:
    packet = [255,255,ID,packetLength,toWrite,mxGoalPositionAddr,lowByte,highByte];
    
    %Calculate the checksum:
    preChecksum = sum(packet(3:end));
    preChecksumBinary = int2bit(preChecksum,8);
    checksum = bit2int(~preChecksumBinary,8);
    
    %Append the checksum:
    packet(end+1) = checksum;
    
    %Write the packet over the port that was passed to this function:
    port.write(packet,"uint8");
    success = true;
end

Using this function, I can write any sequence of goal positions to generate a complete motion:

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

ID1 = 1;

timeDuration = 5; %seconds
numCommands = 100; %time duration will be split into this many samples
dt = timeDuration/numCommands; %Time between servo commands
T = 2; %period of a sine wave, seconds
omega = 2*pi/T; %frequency of a sine wave, radians per second
t = dt*(1:numCommands); %Vector of times at which to send servo commands
goalPositions = round(goalPos1 + (goalPos2-goalPos1)*(1/2)*(1-cos(omega*t))) %Vector of servo commands. Note that they must be integers, so I have rounded the values.

%Loop through the commands and send them at intervals of dt seconds.
for i=indices
    loopTimer = tic; %start the clock
    
    setMXposition(ID1,goalPositions(i),u2d2); %send the command

    %Wait around until dt has passed
    while toc(loopTimer) < dt
        %do nothing
    end
end

Sending one command at a time to each servo is a great way to learn, but would be impractical for running a robot with many actuators. For this reason, there is also a "sync write" command, with which we can write to a particular memory address in multiple servos, and give each servo a unique value, e.g., send a new goal position to every servo, with each servo receiving a unique goal position. Doing so requires that we modify the packet structure.

We start the packet with 0xFF, 0xFF as usual. But for the ID, we send 0xFE (254), which signals that all servos should listen to the incoming message. Then we send the length of the packet after the length byte (same as before), which is now much greater, for example:

L = 2; %Writing a position takes 2 bytes
N = 2; %There are two dynamixels in this example
packetLength = ((L + 1) * N) + 4;

Next, we add the instruction, which is now "sync write", 0x83 (which is 8*16+3=131 in decimal, not 83).

Then, we add the memory address we wish to write to for every single servo. In this example, I'll be writing to the goal position again.

Then, we add the number of bytes we will be writing to each servo. For goal position, this number is 2. If we wanted to write to multiple addresses at once, we could use a higher number. If we wanted to write to a single position, we could set this as 1.

Next, we add all the data we want to send: servo ID, bytes to write; servo ID, bytes to write; on and on until we have listed everything we wish to write.

Finally, we calculate and append the checksum as usual.

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

toWrite = 3;
toSyncWrite = 131; %0x83

mxGoalPositionAddr = 30;

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

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