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...
if you can't find it but you want to know the danger bug.
