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
.
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; } } }
Serial Port
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>
Dynamixel
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; } }
The zip with the full example for Visual Studio 2008