- In Linux, needed a udev rule to allow permissions to read/write the USB Serial port:
- Put the CM-2+ in Manager Mode (1st blinking light)
- Open the serial port at 57600 baud
- import serial
- ser = serial.Serial("/dev/ttyUSB0", 57600)
- The CM-2+ will use its own UART to talk to the Dynamixel Bus at a Baud rate of 1000000
- send it a "t" and return and newline to put into "toss mode"
Now you are ready to send it byte commands.
Example: Ping Instruction
First, let's write a little Python function that will construct a string from a series of bytes:
def write(*s): ser.write("".join(map(chr, s)))
This assumes that there is a serial port called "ser" already opened and ready to use. The *s allows any number of arguments, which will be put into the list named "s". The "".join() joins the list together with nothing between them; the map(chr, s) will turn each number into a character.
Now, let's open a serial port, and use the write function to send a command. Looking up an example, we see that "0xff 0xff 0x01 0x02 0x00 0xfc" will query Dynamixel 1. From the manual:
Obtaining the status packet of the Dynamixel actuator with an ID of 1
Send Instruction Packet : 0XFF 0XFF 0X01 0X02 0X01 0XFB
- 0xFF 0xFF begins each packet
- 0x01 is ID
- 0x02 is the LENGTH of what comes next
- 0x01 is the INSTRUCTION code
- 0xFB is the CHECKSUM
>>> write(0xff, 0xff, 0x01, 0x02, 0x00, 0xfc)
The returned Status Packet is as the following
Status Packet : 0XFF 0XFF 0X01 0X02 0X00 0XFC
0x01 is the ID 0x02 is the LENGTH of what comes next 0x00 is the ERROR code 0xFC is the CHECKSUM
Let's try it. We read the data:
>>> ser.read(6) '\xff\xff\x01\x02@\xbc'
Regardless of whether the Broadcasting ID is used (0xFE) or the Status Return Level (Address 16) is 0, a Status Packet is always returned by the PING instruction.
We should probably read byte by byte:
>>> write(0xff, 0xff, 0x01, 0x02, 0x00, 0xfc) >>> b1 = ser.read(1) >>> b1 == 0xff >>> b2 = ser.read(1) >>> b2 == 0xff
Ok so far.
>>> id = ser.read(1) >>> id == 0x01
And then let's get the data:
>>> data_len = ser.read(1) >>> data_len == 2 >>> error = ser.read(1) >>> checksum = ser.read(1)
Shouldn't be anything else to read:
>>> leftover = ser.read(1) >>> len(leftover) == 0
|PING||No action. Used for obtaining a Status Packet||0x01|
|READ DATA||Reading values in the Control Table||0x02|
|WRITE DATA||Writing values to the Control Table||0x03|
|REG WRITE||Similar to WRITE_DATA, but stays in standby||0x04|
|ACTION||Triggers the action registered by the REG_WRITE instruction||0x05|
|RESET||Dynamixel actuator to the Factory Default Value||0x06|
|SYNC WRITE||Used for controlling many Dynamixel actuators at the same time||0x83|
Sample Python Code
import serial, time ADDR_GOAL_POSITION = 0x1E ADDR_TORQUE = 0x18 ADDR_LED = 0x19 PING = 0x01 READ = 0x02 WRITE = 0x03 SYNC_WRITE = 0x83 BROADCAST = 0xFE def bytes(num): return (num % 256, num / 256) def write_data(ID, address, values): """ write_data(ID, ADDR_TORQUE, [1, 1]) # torque on, LED on write_data(8, ADDR_LED, ) """ header = [0xFF, 0xFF] length = len(values) + 3 data = [ID, length, WRITE, address] + values check = [checksum(data)] packet = "".join( map(chr, header + data + check)) print "send:", map(hex, map(ord, packet)) ser.write(packet) print "receive:", map(hex, map(ord, ser.read(1000))) def sync_write_data(address, *tuples): """ sync_write_data(ADDR_TORQUE, (ID, 1, 1), (ID, 1, 1)) # torque on, LED on sync_write_data(ADDR_LED, (0, 1), (1, 1), (2, 1)...) """ header = [0xFF, 0xFF] L = len(tuples) - 1 # ((ID, v1, v2, ...) N = len(tuples) # number of servos commands length = (L + 1) * N + 4 data = [BROADCAST, length, SYNC_WRITE, address, L] for values in tuples: data += values check = [checksum(data)] packet = "".join( map(chr, header + data + check)) print "send:", map(hex, map(ord, packet)) ser.write(packet) def move_to(*tuples): """ move_to((ID, position, speed), (ID, position, speed), ...) """ header = [0xFF, 0xFF] L = 4 # 2 for position, 2 for speed N = len(tuples) # number of servos commands length = (L + 1) * N + 4 data = [BROADCAST, length, SYNC_WRITE, ADDR_GOAL_POSITION, L] for ID, position, speed in tuples: p0, p1 = bytes(position) s0, s1 = bytes(speed) data += [ID, p0, p1, s0, s1] check = [checksum(data)] packet = "".join( map(chr, header + data + check)) print "send:", map(hex, map(ord, packet)) ser.write(packet) def checksum(s): """ Return the low bits of the the bitwise NOT of the sum. """ return 0x00FF & ~sum(s)