Workshop: Dynamixel communications with C#

[Next post: Workshop: USB, serial and remote communications with C#]

As I wrote in the previous post, I am not using Robotis Dynamixel SDK because it only works with the  USB2Dynamixel, and I need that it also should work with the serial port and with zigbee or bluetooth (really all 4 use the serial connection). Also I want to query sensors connected to the CM-510.

[caption id="attachment_1053" align="alignright" width="150"] Zigbee[/caption]

Using the CM-510 and computer serial port (or USB to serial) connection you are free to use any wired or wireless device. Really there are a lot of possibilities.

We will start connecting to the Dynamixel bus and sending commands and queries. These classes do the work:

- DynamixelCommunication

- SerialPort2Dynamixel

- RCDataReader

But there are other classes that offer to them some additional services, like Configuration, Utils, Hex and several enumeration types.

I will use the Class-Responsability-Collaboration template to present the classes.

DynamixelCommunication

The main responsibility of this class is sending commands and queries to any Dynamixel device, including the sensors, sound and other capabilities of the CM-510 controller.

Collaborator classes:

- SerialPort2Dynamixel,  that offers operations to use the serial port encapsulating .Net SerialPort class

- Three enums for easy use and avoid errors, using an specific type is safer that using simple integers.

    public enum AXS1_IRSensor { Left, Center, Right, None };
    public enum AXS1_SoundNote { LA, LA_, SI, DO, DO_, RE }; //Only the first six 
    public enum DynamixelFunction, with all the Dynamixel protocols codes and some that I added for the CM-510.

- Configuration class, that reads a file where are stored basic configuration parameters. like:

        private static string ParameterSerialPortName
        private static string ParameterSerialPortBaudRate
        private static string ParameterWaitTime_ms
        private static string ParameterWaitTimeForSensors_ms

Operations:

The public operations are the interface that other classes will use, like:

- short readValue(int id, DynamixelFunction address), reads the value of any AX-12 parameter (or other Dynamixels)


- bool sendOrder(int id, DynamixelFunction address, int value), send commands, like position, speed or torque.


And the private that do internal work supporting the public interface, like:

-  static int getReadWordCommand(byte[] buffer, byte id, DynamixelFunction address), create the Dynamixel hexadecimal sequence (FF FF 0F 05 03 1E CB 01 FE)


- static short getQueryResult(byte[] res), once the query or command is sent it gets the result.


Let's see readValue and two other called functions:

[sourcecode language="csharp"]

public short readValue(int id, DynamixelFunction address)
{
mutex.WaitOne();
short position = -1;

try
{
int size = getReadWordCommand(buffer, (byte)id, address);
byte[] res = serialPort.query(buffer, size, WaitTimeReadSensor);

position = getQueryResult(res);
if (position < 0)
Debug.show("DynamixelCommunication.readValue", position);

}
catch (Exception e)
{
Debug.show("DynamixelCommunication.readValue", e.Message);
}

mutex.ReleaseMutex();

return position;
}

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

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

// bodyLength = 4
buffer[pos++] = 4;

//the instruction, read => 2
buffer[pos++] = 2;

// AX12 register
buffer[pos++] = (byte)address;

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

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

return pos;
}

private static short getQueryResult(byte[] res)
{
short value = -1;

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

value = Hex.fromHexHLConversionToShort(h, l);
}
}
return value;
}

[/sourcecode]

Notes:

To avoid concurrency problems all the operations that use the Dynamixel bus are protected with a Mutex object that avoids that two or more concurrent objects use DynamixelCommunication simultaneously entering the same operation or using the same resources, like variables, objects or the Dynamixel bus.

All the operations use the same buffer, but being protected with the Mutex object I think that is the better option, although in a previous version I used a very different approach where there were AX12 objects with their own buffer.

[Next post: Workshop: USB, serial and remote communications with C#]