The Open Academic Robot Kit

This page is read only. You can view the source, but not change it. Ask your administrator if you think this is wrong.
==== Connecting a Raspberry Pi to Dynamixel servos via the OpenCM 9.04 board ====

The OpenCM 9.04 board is a microcontroller board made by Robotis. It has all of the circuitry and software to reliably communicate with the Dynamixel AX-12 servos. It is also Arduino compatible although, as it uses a very different chip to "normal" Arduinos, many libraries have issues with it. 

Further info about the OpenCM board is available from http://support.robotis.com/en/product/auxdevice/controller/opencm9.04.htm

Note that the OpenCM 9.04 board cannot be programmed using the normal Arduino IDE. Robotis has their own version of the IDE available from http://support.robotis.com/en/software/robotis_opencm.htm . 

Be aware that the OpenCM 9.04 board can be somewhat sensitive to low quality USB cables. Ideally stick to the cable that it ships with. If you keep getting errors relating to the serial port already being open when you attempt to program it, try changing the USB cable. 

The OpenCM 9.04 board can be powered directly from the AX-12A bus so simply connect it somewhere in the servo daisy chain. 

Connect the ground, Tx and Rx lines to those on the Raspberry Pi. Note that the Tx and Rx lines need to switch (Tx on the Raspberry Pi connects to Rx on the OpenCM 9.04 and vice versa). 

On the OpenCM 9.04, ground is marked as GND, Tx is marked as A4, Rx is marked as A5. See http://www.robotsource.org/bs/bd.php?bt=forum_CM9DeveloperWorld&bt_id=482 . 

On the Raspberry Pi, if you hold it so that the HDMI port is facing towards you, ground is the top row, 3rd from the left, followed by Tx and Rx. 

Note of course that this means if you connect the Raspberry Pi to the OpenCM 9.04 via ribbon cable, at one end the Tx/Rx lines will result in a twist in the cable (because in both cases, the Tx line is closest to ground - but you need to connect Tx to Rx and Rx to Tx). 

Also note that it is normal for the power light on the OpenCM 9.04 to glow slightly when the Raspberry Pi is turned on but the AX-12A bus is unpowered. Basically what's happening is that the signals from the Raspberry Pi are going into the (dead) OpenCM 9.04 and slightly powering it up. Not an ideal situation but it doesn't seem to cause much of a problem. 

Here is a very simple program that allows the Raspberry Pi (or anything else that talks serial, for that matter) to talk to the AX-12 servos. This is a very rough, quick program. Efforts to improve it are most welcome. :-) 

<code c>
/*

A thin shim translating Dynamixel commands from Serial 2 to Dynamixel. 
Very much a work-in-progress. For example, we can't read from the servos yet. 

Note that all values in <> are ASCII decimal integers. 
<pos> is a value from 0 to 1023. 
<vel> is a velocity, 0 to 1023 is forward, 1024-2047 is negative. 

Possible commands: 

"Wj<ID>" to make motor <ID> enter joint mode. 
"Wp<ID>,<pos>,<vel>" to make motor <ID> move to position <pos> with velocity <vel>. 
"Ww<ID>,<add>,<val>" to write <val> to register <add> of motor <ID>. 

*/

Dynamixel Dxl(1); 

void setup() 
{
  Serial2.begin(57600); 
  Dxl.begin(3); 
}

byte blockingRead()
{
  while (!Serial2.available()) delay(10); 
  return Serial2.read(); 
}

void processWrite(byte cmd, byte id)
{
          switch (cmd)
        {
          case 'j':
            Dxl.jointMode(id); 
            break; 
          case 'p':
            int pos, vel; 
            if (!(readInt(&pos) && readInt(&vel))) break; 
            Dxl.setPosition(id, pos, vel); 
            break; 
          case 'w':
            int add, val; 
            if (!(readInt(&add) && readInt(&val))) break; 
            Dxl.writeWord(id, add, val); 
            break; 
        }
}

void loop() 
{
  while (Serial2.available())
  {
    // Wait for a start byte, "W" for write or "R" for read. 
    byte inByte = blockingRead();
    switch (inByte)
    {
      case 'W':
        byte cmd = blockingRead();
        byte id = blockingRead();
        processWrite(cmd, id); 
        break; 
//      case 'R':
//      FILL THIS IN!
//        break; 
    }    
    
  }
}

boolean readInt(int * outVal)
{
  byte inBuffer[sizeof(short)]; 
  int i; 
  for (i = 0; i < sizeof(short); i ++)
  {
    inBuffer[i] = blockingRead();
  }
  *outVal = *((short*)inBuffer); 
  return true; 
}
</code>

The following Python code on the Raspberry Pi will allow you to control the robot via a Playstation 3 Sixaxis bluetooth controller once you have: 

  * [[designs:setup:pi:initial|Set up the Raspberry Pi]]. In particular pay attention to the bit about enabling the serial port. 
  * [[designs:interfacing:pi:ps3controller|Connected a PS3 Sixaxis controller]]. Otherwise I'm sure you can pull out the bits that'll let you drive the robot around. ;-) 

Note that it isn't particularly good code and it does have some glitches. But it does kinda work enough to show you what you need to do. :-) 

<code python EmuMini2-Sixaxis-demo.py>
import serial
import sixaxis
import sys


import time



sixaxis.init("/dev/input/js0")

# important AX-12 constants
AX_WRITE_DATA = 3
AX_READ_DATA = 4

s = serial.Serial()			   # create a serial port object
s.baudrate = 57600			  # baud rate, in bits/second
s.port = "/dev/ttyAMA0"		   # this is whatever port your are using
s.timeout = 3.0
s.open()


DXL_REG_CCW_Angle_Limit = 8 #to change control mode
DXL_REG_Goal_Postion = 30
DXL_REG_Moving_Speed = 32


def writeShort(val):
	s.write(chr(int(val)%256))
	s.write(chr(int(val)>>8))

def writeWord(ID, addr, val):
	s.write('W'+'w'+chr(ID))
	writeShort(addr)
	writeShort(val)

def jointMode(ID):
	s.write('W'+'j'+chr(ID))

def setPosition(ID, pos, vel):
	s.write('W'+'p'+chr(ID))
	writeShort(pos)
	writeShort(vel)




def moveToDxAngle(ID,dxlPosition,dxlSpeed):
	setPosition(ID,dxlPosition,dxlSpeed)

def moveToDegAngle(ID, degPosition, pcSpeed): 
	while degPosition > 180:
		degPosition = degPosition - 360
	while degPosition < -180:
		degPosition = degPosition + 360

	if (degPosition < -150):
		degPosition = -150
	if (degPosition > 150):
		degPosition = 150
	moveToDxAngle(ID, int(float(degPosition)*3.41+511.5), int(float(pcSpeed)*10.23))

def spinAtDxSpeed(ID,dxlSpeed):
	writeWord(ID,DXL_REG_Moving_Speed,dxlSpeed)

# Spins at a certain percent of full speed. 
def spinAtPcSpeed(ID,pcSpeed): 
	if pcSpeed >= 0:
		spinAtDxSpeed(ID,int(float(pcSpeed)*10.23))
	else:
		spinAtDxSpeed(ID,1024+int(float(-pcSpeed)*10.23))

def throttleSteeringToLeftRight(inThrottle, inSteering):
	left = min(100, max(-100, inThrottle - inSteering)); 
	right = min(100, max(-100, inThrottle + inSteering)); 
	return (left, right)



# Purge the first value
joystate = sixaxis.get_state()
time.sleep(0.5) 
print("Press [triangle] to exit.") 
shoulderPos = -45
tiltPos = 0
panPos = 0

# Set wheel and joint modes. 
writeWord(1, DXL_REG_CCW_Angle_Limit, 0)
writeWord(2, DXL_REG_CCW_Angle_Limit, 0)
writeWord(3, DXL_REG_CCW_Angle_Limit, 0)
writeWord(4, DXL_REG_CCW_Angle_Limit, 0)

jointMode(5)
jointMode(6)
jointMode(7)

while(1): 
	joystate = sixaxis.get_state()
	if (joystate['triangle'] == True): 
		break
	
	(left, right) = throttleSteeringToLeftRight(joystate['lefty'], joystate['leftx'])

	spinAtPcSpeed(1, -left)
	spinAtPcSpeed(2, right)
	spinAtPcSpeed(3, right)
	spinAtPcSpeed(4, -left)

	tiltSpeed = 100
	panSpeed = 100
	shoulderSpeed = 0

	if joystate['trig3'] == True: 
		shoulderPos = shoulderPos + 3
		shoulderSpeed = 20
	if joystate['trig1'] == True: 
		shoulderPos = shoulderPos - 3
		shoulderSpeed = 20
	# Shoulder is limited to -90 and 150. Note that this will hit the ground (which could be desired). 
	shoulderPos = max(-90, min(150, shoulderPos))


	# Cross resets. 
	if joystate['cross'] == True: 
		tiltPos = 0
		panPos = 0
	else: 
		tiltPos = tiltPos + float(joystate['righty'])*0.05
		tiltSpeed = max(abs(joystate['righty'])*0.5, shoulderSpeed)
		panPos = panPos - float(joystate['rightx'])*0.05
		panSpeed = abs(joystate['rightx'])*0.5
	
	tiltcmd = tiltPos + shoulderPos
	pancmd = panPos

	# Tilt is limited to 90 degrees, pan to 150. 
	tiltcmd = max(-90, min(90, tiltcmd))
	pancmd = max(-150, min(150, pancmd))

	moveToDegAngle(5, shoulderPos,shoulderSpeed)
	moveToDegAngle(6, tiltcmd, max(10, tiltSpeed))
	moveToDegAngle(7, pancmd, max(10, panSpeed))
	
	time.sleep(0.05) 

sixaxis.shutdown()
print("Exiting.") 


</code>
The Open Academic Robot Kit

User Tools

Site Tools

Page Tools
