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