C# Dynamixel reading and writing example

Three C# classes for a simple example for reading and writing the position of a AX-12 Dynamixel servo. You can use it with the USB2Dynamixel or using only the serial port: in "Manage mode" send "T" to put firmware in "Toss Mode".

Here you can find several combinations of hardware, firmware and programming tools.

The main

.

[sourcecode language="csharp"]
class Program
{
static void Main(string[] args)
{
int error=0;
int idAX12=15;

SerialPort2Dynamixel serialPort = new SerialPort2Dynamixel();
Dynamixel dynamixel = new Dynamixel();

if (serialPort.open("COM1")==false) {
dynamixel.sendTossModeCommand(serialPort);

int pos=dynamixel.getPosition(serialPort, idAX12);

if (pos>250 && pos dynamixel.setPosition(serialPort, idAX12, pos-100);
else
Console.Out.WriteLine("nPosition under 250 or over 1023", pos);

serialPort.close();
}
else {
Console.Out.WriteLine("nCan't open serial port");
error=-1;
}
}
}
[/sourcecode]

Serial Port

[sourcecode language="csharp"]
public class SerialPort2Dynamixel
{
const int HeaderSize = 4;
const int PacketLengthByteInx = 3;
const int BufferSize = 1024;
const int MaximuTimesTrying = 250;
const int waitTime = 5;

private string portName = "COM3";

private byte[] buffer = new byte[BufferSize];

private SerialPort serialPort = new SerialPort();

public bool open(String com)
{
bool error = false;
portName = com;

serialPort.PortName = com;
serialPort.BaudRate = 57600;
serialPort.DataBits = 8;
serialPort.Parity = Parity.None;
serialPort.StopBits = StopBits.One;

try
{
if (serialPort.IsOpen)
Console.WriteLine("Serial port is already open");
else
serialPort.Open();
}
catch (Exception exc)
{
Console.WriteLine(exc.Message);
error = true;
}

return error;
}

public void close()
{
if (serialPort.IsOpen)
serialPort.Close();
Console.WriteLine("Conecction closed!");
}

private void cleanConnection()
{
serialPort.DiscardInBuffer();
}

public byte[] query(byte[] buffer, int pos)
{
byte[] outBuffer = null;
try
{
serialPort.Write(buffer, 0, pos);
System.Threading.Thread.Sleep(waitTime);
outBuffer = rawRead();
}
catch (Exception exc)
{
Console.Out.WriteLine(exc.Message);
}

return outBuffer;
}

public void rawWrite(byte[] buffer, int pos)
{
try
{
serialPort.Write(buffer, 0, pos);
}
catch (Exception exc)
{
Console.Out.WriteLine(exc.Message);
}
}

public byte[] rawRead()
{
byte[] localbuffer = null;
int n = serialPort.BytesToRead;
if (n != 0)
{
localbuffer = new byte[n];
try
{
serialPort.Read(localbuffer, 0, n);
}
catch (Exception exc)
{
Console.Out.WriteLine(exc.Message);
}
}
return localbuffer;
}
}<code>
[/sourcecode]

Dynamixel

[sourcecode language="csharp"]
class Dynamixel
{
protected static int MaxBufferSize = 1024;
protected byte[] buffer = new byte[MaxBufferSize];

private static byte checkSumatory(byte[] data, int length)
{
uint cs = 0;
for (int i = 2; i < length; i++)
{
cs += data[i];
}
cs = ~cs;
return (byte)(cs & 0x0FF);
}

public static void toHexHLConversion(int pos, out byte hexH, out byte hexL)
{
ushort uPos = (ushort)pos;
hexH = (byte)(uPos >> 8);
hexL = (byte)uPos;
}

public static ushort fromHexHLConversion(byte hexH, byte hexL)
{
return (ushort)((hexL << 8 ) + hexH);
}

public static short fromHexHLConversionToShort(byte hexH, byte hexL)
{
return (short)((hexL << 8 ) + hexH);
}

public static void toHexHLConversion(int pos, out string hexH, out string hexL)
{
string hex;
int lon, start;
hex = String.Format("{0:X4}", pos);
lon = hex.Length;
if (lon < 2)
{
hexL = hex;
hexH = "0";
}
else
{
start = lon - 2;// lon = 4, start = 2; lon=3, start=1
hexL = hex.Substring(start);
hexH = hex.Substring(0, start);
}
}

public void sendTossModeCommand(SerialPort2Dynamixel sp2d)
{
byte[] buffer = { (byte)'t', (byte)'r' };
sp2d.rawWrite(buffer, 2);
System.Threading.Thread.Sleep(100);
sp2d.rawRead();
}

private static int getReadPositionCommand(byte[] buffer, byte id)
{
//OXFF 0XFF ID LENGTH INSTRUCTION PARAMETER1 …PARAMETER N CHECK SUM
int pos = 0;
buffer[pos++] = 0xff;
buffer[pos++] = 0xff;
buffer[pos++] = id;
buffer[pos++] = 4; // bodyLength = 4
buffer[pos++] = 2; //the instruction, rawRead => 2

// pos registers 36 and 37
buffer[pos++] = 0x24;

//bytes to rawRead
buffer[pos++] = 2;

byte checksum = checkSumatory(buffer, pos);
buffer[pos++] = checksum;

return pos;
}

private static int getSetPositionCommand(byte[] buffer, byte id, short goal)
{
int pos = 0;
byte numberOfParameters = 0;
//OXFF 0XFF ID LENGTH INSTRUCTION PARAMETER1 …PARAMETER N CHECK SUM

buffer[pos++] = 0xff;
buffer[pos++] = 0xff;
buffer[pos++] = id;

// bodyLength
buffer[pos++] = 0; //place holder

//the instruction, query => 3
buffer[pos++] = 3;

// goal registers 30 and 31
buffer[pos++] = 0x1E;// 30;

//bytes to write
byte hexH = 0;
byte hexL = 0;
toHexHLConversion(goal, out hexH, out hexL);
buffer[pos++] = hexL;
numberOfParameters++;
buffer[pos++] = hexH;
numberOfParameters++;

// bodyLength
buffer[3] = (byte)(numberOfParameters + 3);

byte checksum = checkSumatory(buffer, pos);
buffer[pos++] = checksum;

return pos;
}

public short getPosition(SerialPort2Dynamixel sp2d, int id)
{
//byte[] localbuffer = new byte[MaxBufferSize];
int size = getReadPositionCommand(buffer, (byte)id);
byte[] res = sp2d.query(buffer, size);

short position = -1;

if (res != null)
{
int length = res.Length;
if (res != null && length > 4 && res[4] == 0)
{
byte l = 0;
byte h = res[5];
if (length > 6)
{
l = res[6];
}

position = fromHexHLConversionToShort(h, l);
}
}

return position;
}

public bool setPosition(SerialPort2Dynamixel sp2d, int id, int goal)
{
bool couldSet = false;

short position = (short)goal;
int size = getSetPositionCommand(buffer, (byte)id, (short)goal);
byte[] res = sp2d.query(buffer, size);

//ushort value = 1;
if (res != null && res.Length > 4 && res[4] == 0)
couldSet = true;

return couldSet;
}
}
[/sourcecode]

The zip with the full example for Visual Studio 2008