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

.

    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

One comment on “C# Dynamixel reading and writing example

%d bloggers like this: