Archive for: December 2011

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

Writing Dynamixel AX-12+ position

This entry is part 3 of 5 in the series Bioloid C++ tutorial

The intent of this code is to be clear and easy to understand, it not pretend to be the best code to do the job.

This method create the command to write the AX-12+ position :

int Dynamixel::getSetAX12PositionCommand(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, &hexH, &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;
}

int Dynamixel::setPosition(SerialPort *serialPort, int idAX12, int position)
{
	int error=0;

	int n=getSetAX12PositionCommand(idAX12, position);
	long l=serialPort->sendArray(buffer,n);
	Sleep(10);

	memset(bufferIn,0,BufferSize);
	n=serialPort->getArray(bufferIn, 8);

	if (n>4 && bufferIn[4] == 0)
		printf("nid=<%i> set at pos=<%i>n", idAX12, position);
	else {
		error=-1;
		printf("nid=<%i> error: <%i>n", idAX12, bufferIn[4]);
		bf(bufferIn, n);
	}

	return error;
}

Simple C++ class example using serial port, USB, wireless…

This entry is part 2 of 2 in the series Serial communications

This post is part of the Practical C++ programming tutorial for Bioloid

Here you can find a post serie about using serial port communications with C/C++ and C#, for Windows, Linux and microcontrollers.
This code is for Windows and Visual Studio and can be used for serial cable communications, USB2Dynamixel and indeed Zigbee:

Header:

class SerialPort {
private:
HANDLE serialPortHandle;

public:
SerialPort();
~SerialPort();

int connect ();
int connect (wchar_t *device);
//int connect (char *deviceName, int baudRate, SerialParity parity);
void disconnect(void);

int sendArray(unsigned char *buffer, int len);
int getArray (unsigned char *buffer, int len);

void clear();
};

Body:


SerialPort::SerialPort() {
serialPortHandle = INVALID_HANDLE_VALUE;
}

SerialPort::~SerialPort() {
if (serialPortHandle!=INVALID_HANDLE_VALUE)
CloseHandle(serialPortHandle);

serialPortHandle = INVALID_HANDLE_VALUE;
}

int SerialPort::connect() {
return connect(L"COM1");
}

int SerialPort::connect( wchar_t* device) {
int error=0;
DCB dcb;

memset(&dcb,0,sizeof(dcb));

dcb.DCBlength = sizeof(dcb);

dcb.BaudRate = 57600;
dcb.Parity = NOPARITY;
dcb.fParity = 0;
dcb.StopBits = ONESTOPBIT;
dcb.ByteSize = 8;

serialPortHandle = CreateFile(device, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, NULL, NULL);

if (serialPortHandle != INVALID_HANDLE_VALUE) {
if(!SetCommState(serialPortHandle,&dcb))
error=2;
}
else {
error=1;
}

if (error!=0) {
disconnect();
}
else {
clear();
}

return error;
}

void SerialPort::disconnect(void) {
CloseHandle(serialPortHandle);
serialPortHandle = INVALID_HANDLE_VALUE;

//printf("Port 1 has been CLOSED and %d is the file descriptionn", fileDescriptor);
}

int SerialPort::sendArray(unsigned char *buffer, int len) {
unsigned long result;

if (serialPortHandle!=INVALID_HANDLE_VALUE)
WriteFile(serialPortHandle, buffer, len, &result, NULL);

return result;
}

int SerialPort::getArray (unsigned char *buffer, int len) {
unsigned long read_nbr;

read_nbr = 0;
if (serialPortHandle!=INVALID_HANDLE_VALUE)
{
ReadFile(serialPortHandle, buffer, len, &read_nbr, NULL);
}

return((int) read_nbr);
}

void SerialPort::clear() {
PurgeComm (serialPortHandle, PURGE_RXCLEAR | PURGE_TXCLEAR);
}

Bioloid “Mars Curiosity” (6×6 and rocker-bogie suspension)

This is the first drive of the first version of an 6×6 rocker-bogie suspension inspired in Mars Curiosity rover. It has been built using a Bioloid Comprehensive/Premium kit plus a few parts.

A tougher test:

%d bloggers like this: