Humanoid Notes

CM-2 Plus

1. In Linux, needed a udev rule to allow permissions to read/write the USB Serial port:
   1. /etc/udev/rules.d/10-ftdi.rules:
   2. MODE="0777"
2. Put the CM-2+ in Manager Mode (1st blinking light)
3. Open the serial port at 57600 baud
   1. import serial
   2. ser = serial.Serial("/dev/ttyUSB0", 57600)
4. The CM-2+ will use its own UART to talk to the Dynamixel Bus at a Baud rate of 1000000
5. send it a "t" and return and newline to put into "toss mode"
   1. ser.setTimeout(.1)
   2. ser.write("t\r")
   3. ser.read(10000)

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

where:

1. 0xFF 0xFF begins each packet
2. 0x01 is ID
3. 0x02 is the LENGTH of what comes next
4. 0x01 is the INSTRUCTION code
5. 0xFB is the CHECKSUM

Us it:

>>> write(0xff, 0xff, 0x01, 0x02, 0x00, 0xfc)

The returned Status Packet is as the following

Status Packet : 0XFF 0XFF 0X01 0X02 0X00 0XFC

where:

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

Dynamixel Commands

Name	Meaning	Code
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, [1])
    """
    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[0]) - 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)

Links

• Hardware
  • Dynamixel Links
  • Dynamixel details
    • RX-28 Manual
  • CM2+ Manual
  • Related to a Pass-though mode
  • USB 2 Dynamixel
  • Sample Python Controller
• Simulation
  • Media:ObstacleRunAAAI2010.pdf