(I) Reading sensors connected to Robotis CM-510

Using the Dynamixel SDK instead of RoboPlus Tasks is not possible to query sensors connected to the CM-5xx controller. But, of course, using standard programming languages, like C++, and tools, line QT Creator or Eclipse, with its full featured IDEs and debuggers is a hugue gain.

So I created a firmware for the CM-510 which can be queried and receive commands to itself, including its sensors, or to Dynamyxel devices.

The idea is very simple:

This program running in the CM-510 receives:

- commands or queries to IDs of Dynamixel devices. These are not processed, only redirected to the Dynamixel bus only if it was received by serial port ( serial cable or zigbee). If it was received from the Dynamixel bus nothing is done.

- commands or queries to the CM-510 ID (I chose ID=200), like beep, or to the sensors connected to it. This commands and queries are processed in the CM-510, basically querying the sensors.

In both cases the answer is sent to the connection from which the query or command was received.

After power on the CM-510, you can select the mode with the 4 cursor keys as showed in a terminal connected to its serial port:

"For 'Toss mode' press (Up), for 'Device mode' (Down), for 'Device debug mode' (Left),to start press (Right)"

In the Device mode:

all the receptions and sends are through the Dynamixel bus, the CM-510 is simply another device.

In the Toss mode:

- what is received from the serial connection is sent to the Dynamixel bus or processed in the CM-510 (If sent to its ID)

-what is received from the Dynamixel bus is sent to the serial connection

Finally, the Debug mode:

is like the Device mode, but all the debug messages included in the CM-510 are sent to the serial connection.

A complete sequence with code snippets from the CM-510 program and from the code running in the other computer:

Some C++ code snippets from this example: (C# in the next post)


[sourcecode language="cpp"]
enum AX12Address //and functions implemented in the CM-510 program, like
{
ReadCM510SensorRaw = 1,
Beep = 8,
ReadCM510SensorFiltered = 4,
SetSensorValuesToFilter = 5,
...
}
[/sourcecode]

[sourcecode language="cpp"]
void doBeep()
{
cout << "Beep" << endl;
mySystem.dynamixelCommunication.sendOrder(100, AXS1_Buzzer, (byte) DO, (short) 500);
usleep (200000);
mySystem.dynamixelCommunication.sendOrder(200,MyCommunications::Beep short)5);
}
[/sourcecode]

Querying sensor:

[sourcecode language="cpp"]
void doQuerySensor()
{
int sensorPort=getSensorPort();
int value=mySystem.dynamixelCommunication.readSensorValue (200,ReadCM510SensorRaw, sensorPort);
cout << "the sensor reads: [" << value << "] " << endl << endl << endl;
}
[/sourcecode]

These command and query are processed in the CM-510:

Getting the sensor value:

[sourcecode language="c"]

int executeCM510Function()
{
...
case F_GET_SENSOR_VALUE_RAW:
values[1] = getSensorValueRaw(parameters[1]);
break;

case F_GET_SENSOR_VALUE_FILTERED:
values[1] = getSensorValueFiltered(parameters[1], sensorValuesToFilterDefined);
break;

case F_GET_TWO_DMS_SENSOR_VALUES:
parametersReceived=3;
getTwoDMSSensorsValues();
break;

case F_GET_MULTIPLE_SENSOR_VALUE:
getMultipleSensorsValues();
break;

case F_DO_SENSOR_SCAN:
values[1]= sensorScan(parameters[1]);
break;

case F_SET_VALUE_DMS1 : //set default value DMS1
DMS1=parameters[1];
break;

case F_SET_VALUE_DMS2 : //set default value DMS1
DMS2=parameters[1];
break;

case F_BEEP:
if (debugMode)
printf("executeCM510Function beep\n");
beep();
break;

case F_SET_SENSOR_VALUES_TO_FILTER:
sensorValuesToFilterDefined=parameters[1];
break;
}

return function;
}

}
...
int getSensorValueRaw(unsigned char portId)
{
ADCSRA = (1 << ADEN) | (1 << ADPS2) | (1 << ADPS1);

setPort(portId);
//PORTA &= ~0x80;
//PORTA &= ~0x20;

//_delay_us(12); // Short Delay for rising sensor signal
_delay_us(24);
ADCSRA |= (1 << ADIF); // AD-Conversion Interrupt Flag Clear
ADCSRA |= (1 << ADSC); // AD-Conversion Start

while( !(ADCSRA & (1 << ADIF)) ); // Wait until AD-Conversion complete

PORTA = 0xFC; // IR-LED Off

//printf( "%d\r\n", ADC); // Print Value on USART

//_delay_ms(50);
_delay_ms(ReadSensorDelayMS);

return ADC;
}

[/sourcecode]

 

[sourcecode language="c"]

void beep()
{
buzzOn(100);
buzzOff();
}
[/sourcecode]

But we can do a little more with the CM-510 processor, we can do some filtering to the sensor values.

The readings from the DMS are usually somewhat erratic, so we can simply:

- discard the minimum and maximum values:

- if we take 5 more than measures, then return the average if the are more than 3, if 3 or less it

Previously we should set how many readings should be done, if not, the default number of readings are 5:

[sourcecode language="c"]
int getSensorValueFiltered(unsigned char portId, int times)
{
...
switch(function)
{
case F_GET_SENSOR_VALUE_RAW:
values[1] = getSensorValueRaw(parameters[1]);
break;

case F_GET_SENSOR_VALUE_FILTERED:
values[1] = getSensorValueFiltered(parameters[1], sensorValuesToFilterDefined);
break;

case F_GET_TWO_DMS_SENSOR_VALUES:
parametersReceived=3;
getTwoDMSSensorsValues();
break;

case F_GET_MULTIPLE_SENSOR_VALUE:
getMultipleSensorsValues();
break;

case F_DO_SENSOR_SCAN:
values[1]= sensorScan(parameters[1]);
break;

case F_SET_VALUE_DMS1 : //set default value DMS1
DMS1=parameters[1];
break;

case F_SET_VALUE_DMS2 : //set default value DMS1
DMS2=parameters[1];
break;

case F_BEEP:
if (debugMode)
printf("executeCM510Function beep\n");
beep();
break;

case F_SET_SENSOR_VALUES_TO_FILTER:
sensorValuesToFilterDefined=parameters[1];
break;
}
...
[/sourcecode]

We also can take values from multiple sensors with one query, but It will be explained in the next post...

Workshop: Programming a Bioloid robot workbench using C# and C++

[Next post: Dynamixel communications with C#]

It would be a workshop using C# .Net and C++ with Qt 5. The code presented here is used in this two different robots and boards, a HP 214 Ipaq with Windows Mobile and a Raspberry Pi, using the Robotis CM-510 as the servo and sensors controller:

[youtube http://www.youtube.com/watch?v=mvaMTdlb48E&w=281&h=210][youtube http://www.youtube.com/watch?v=Yhv43H5Omfc&w=281&h=210]

These will be the first steps, using C# and .Net , here the code and the exe for the Workbench UI:

Using this enhaced Toss Mode that adds some new functions.  Some of them:

Choose hardware, firmware and language

There are a lot of possible combinations of hardware, firmware and languages for programming Bioloid. I think that the table below show the the main combinations.

You can choose from the easy but limited Robotis own tool (Roboplus Task) and only your CM-5 or CM-510 to a SBC or "embedded" PC like Roboard and any language which can manage a serial port connection, like C, C++, Java, Python,...

Linux C++ Dynamixel reading and writing example

C# Dynamixel reading and writing example

Practical C++ programming tutorial for Bioloid

[caption id="attachment_256" align="aligncenter" width="617"] Programming Bioloid: choose hardware, firmware and languages[/caption]

Robotis officially supports the programming solutions with the blue background:

• The dark blue, RoboPlus Tasks, is the only one in which you can create the motions with RoboPlus Motion and execute it in CM-5/CM-510 with the program create with RoboPlus Tasks, after downloading the generated executable into the CM-5/CM-510, of course.

• The clear blue, Embedded C SDK and Dynamixel SDK, are two distinct SDK offered and supported by Robotis, but currently there is no command to execute the motions created with RoboPlus Motion.

With these programming solutions you can use the Zigbee SDK to send and receive data between the CM5-/CM-510 and any computer, using Zig-110A or the new bluetooth BT-110, the Zig2Serial and the USB2Dynamixel. You can download example in Visual Basic .Net, C# and Visual C++

But there are more options!

Using a PC, SBC (Single Board Computer), PDA, Mobile or other light and battery powered computer

Using a serial port connection with your more beloved programming language:

• USB2Dynamixel

If you have any device with a USB host and a FTDI driver you can use USB2Dynamixel to command programatically your Dynamixel servos using the Dynamixel protocol. You only will need your CM-5 or CM-510 to connect your Dynamixel to the battery.

• Serial port cable

Same as the previous option but instead of using the USB2Dynamixel you only will need the serial cable and the "Toss Mode" launched with the 't' command from the "Manage Mode"

• Wireless control

Instead of only sending and receiving data, with the previous wireless connections you can command remotely your robot using the standard firmware and the "Toss Mode" launched with the 't' command from the "Manage Mode". You will need to command it using the Dynamixel protocol.

With these options and the CM-510 you will find a little problem... there is no way to read your sensor values! Well, you can use this firmware that offers a "Toss Mode" that supports reading CM-510 ports.

Start learning!

If you want to start learning how to program your CM-5 or CM-510 controller you will find interesting this post "Start programming  CM-5/CM-510 in C". But may be you prefer to control your robot from a PC, SBC or other computer in C++ or C#

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

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

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 can be used (of course) for serial cable communications, USB2Dynamixel and indeed Zigbee:

Header:

[sourcecode language="cpp"]
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();
};
[/sourcecode]

Body:

[sourcecode language="cpp"]

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);
}
[/sourcecode]

NXT Acer Explorer

NXT Acer Explorer