(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...