CM-510 firmware and post updated
Category: Robotis Dynamixel programming
(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)
enum AX12Address //and functions implemented in the CM-510 program, like
{
ReadCM510SensorRaw = 1,
Beep = 8,
ReadCM510SensorFiltered = 4,
SetSensorValuesToFilter = 5,
...
}
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);
}
Querying sensor:
void doQuerySensor()
{
int sensorPort=getSensorPort();
int value=mySystem.dynamixelCommunication.readSensorValue (200,ReadCM510SensorRaw, sensorPort);
cout << "the sensor reads: [" << value << "] " << endl << endl << endl;
}
These command and query are processed in the CM-510:
Getting the sensor value:
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;
}
void beep()
{
buzzOn(100);
buzzOff();
}
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:
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;
}
...
We also can take values from multiple sensors with one query, but It will be explained in the next post…
(II) C++ crash examples tutorial using Robotis CM-900 board and IDE (II)
In this post I will try to show one important C++ feature, virtual classes as interfaces, defining a rol that several classes can implement. Also
Here you can download the file with the source code.
// C++ crash tutorial with Robotis CM-900.
// First example: http://softwaresouls.com/softwaresouls/
/*
Classes inheritance and interface creation in C++
*/
// Abstract class as Interface for communication device classes http://www.cplusplus.com/doc/tutorial/polymorphism/
class CommunicationDevice
{
public:
virtual void setup(int baudrate=0)=0; // baudrate=0, default parameter in case is not passed any parameter.
//...)=0; is a pure virtual method that convert the class in abstract http://www.cplusplus.com/doc/tutorial/polymorphism/
// because is not straight usable , a derived classimpllementing the pure virtual methods must be created
virtual void send(char *message)=0; // default parameter in case is not passed any parameter.
virtual void send(int i)=0; //Method overloading http://www.cplusplus.com/doc/tutorial/functions2/ Same method name with different parameter types
};
class Zigbee : public CommunicationDevice //inherits the interface a should define the methos to be able to create objects.
{
public:
void setup(int baudrate=0);
void send (char *message);
void send(int i);
};
class USB : public CommunicationDevice //inherits the interface a should define the methos to be able to create objects.
{
public:
void setup(int baudrate=0);
void send (char *message);
void send(int i);
};
class Debug {
CommunicationDevice &commDevice;
public:
Debug(CommunicationDevice &aCommDevice) : commDevice(aCommDevice) {}; // member object commDevice is instantiated by reference to avoid object copy
void show(char *message)
{
commDevice.send(message); // the "commDevice" method called depends on the object passed to the constructor
}
void show(int i)
{
commDevice.send(i);
}
};
void Zigbee::setup(int baudrate)
{
if (baudrate==0)
baudrate=57600; //only changes local copy, not the variable passed as value parameter http://www.cplusplus.com/doc/tutorial/functions2/
Serial2.begin(baudrate);
}
void Zigbee::send(char *message)
{
Serial2.println(message);
}
void Zigbee::send(int i)
{
Serial2.println(i);
}
void USB::setup(int baudrate)
{
SerialUSB.begin();
}
void USB::send(char *message)
{
SerialUSB.println(message);
}
void USB::send(int i)
{
SerialUSB.println(i);
}
Zigbee zigbee;
USB usb;
const byte NumberOfCommsDevices=2;
CommunicationDevice *commsDevices[]={&usb, &zigbee};
//={usb, zigbee};
void setup()
{
// Individually:
// zigbee.setup();
// usb.setup();
// collectively. using the common Interface "CommunicationDevice"
for (int i=0;i<NumberOfCommsDevices;i++) commsDevices[i]->setup();
delay(2000); // waiting for 2 seconds, let the user activate the monitor/console window (Control+Shift+M).
}
int counter=0;
void loop()
{
++counter;
Debug debug1(usb); // Debug constructor accepts any class inhereting the Interface "CommunicationDevice", Debug(CommunicationDevice &aCommDevice)
debug1.show("Hola"); //debug object will call the method "send" implemented in the object passed to the constructor. In this case "usb.send"
//Showing the counter for each iteration until the 2000th iteration
if (counter <=2000)
debug1.show(counter);
else
{
// Showing the counter every 100 iterations and waiting for 2 seconds
if (counter%100==0) // Operator modulo http://www.cplusplus.com/doc/tutorial/operators/ (returns the remainder)
{
debug1.show(counter);
delay(2000);
}
}
}
Bioloid Workbench Windows C++ examples
This post presents the Windows version of two examples/utilities built, this time, with MS Visual C++ and QT Creator, a great open source IDE and framework.
The first utility has a comand line user interface, being an easy example with only two DLL dependencies to the Visual C++ runtimes.
The second example utility has a graphic user interface created with QT Creator.
Here you can find the bin executables and the projects with the source code.
This is the main function of the first example, the command line:
int _tmain(int argc, _TCHAR* argv[])
{
cout << "AXControl_v2_VS_CPP test v0" << endl;
bool quit=false;
MyBasicSystem::BasicSystem mySystem;
UI ui(mySystem);
do
{
ui.showMenu();
int selected=ui.selectOption();
if (selected==ui.OptionQuit)
quit=true;
ui.executeSelectedOption(selected);
}while (!quit);
mySystem.dynamixelCommunication.close();
return 0;
}
[Update] Yesterday I discovered a subtle but ugly bug. Can you spot it? Send an email to 
if you can’t find it but you want to know the danger bug.
It use two classes, the “BasicSystem” (in the library “VS_CPP_AXControl_lib.lib”) and a simple “UI” class that receives the input from the user and use the operations offered by the “BasicSystem” class.
For example:
int UI::getPosition()
{
int position=0;
do
{
cout << "Type a value between 0 and 1023 to set the AX-12 in that position" << endl; cin>>position;
}while(!validRange(position, 0, 1023));
cout << "Position:" << position << endl;
return position;
}
...
void UI::doSetPosition()
{
int ax12Id=getId();
int position=getPosition();
mySystem.dynamixelCommunication.sendOrder(ax12Id, MyCommunications::GoalPosition,(short) position);
}
And all the cource code:
/*------------------------------------------------------------------------------*\
* This source file is subject to the GPLv3 license that is bundled with this *
* package in the file COPYING. *
* It is also available through the world-wide-web at this URL: *
* http://www.gnu.org/licenses/gpl-3.0.txt *
* If you did not receive a copy of the license and are unable to obtain it *
* through the world-wide-web, please send an email to *
* siempre.aprendiendo@gmail.com so we can send you a copy immediately. *
* *
* @category Robotics *
* @copyright Copyright (c) 2011 Jose Cortes (http://www.siempreaprendiendo.es) *
* @license http://www.gnu.org/licenses/gpl-3.0.txt GNU v3 Licence *
* *
\*------------------------------------------------------------------------------*/
#include "stdafx.h"
#include
#include "BasicSystem.h"
#include "Util.h"
using namespace std;
class UI
{
public:
static const int OptionQuit=8;
UI(MyBasicSystem::BasicSystem& aMySystem) : mySystem(aMySystem) {}
void showMenu();
int selectOption();
bool executeSelectedOption(int selected);
void doBeep();
private:
MyBasicSystem::BasicSystem mySystem;
static const int OptionSetLEDOnOff=1;
static const int OptionQueryAX12=2;
static const int OptionMoveAX12=3;
static const int OptionTorqueAX12=4;
static const int OptionQuerySensor=5;
static const int OptionShowMenu=6;
static const int OptionBeep=7;
static const int MinOption=OptionSetLEDOnOff;
static const int MaxOption=OptionQuit;
bool validRange(int value, int MinValue, int MaxValue);
bool nonValidOption(int option);
int getPosition();
int getId();
int getSensorPort();
void doGetPosition();
void doSetPosition();
void doQuerySensor();
void doSetLEDOnOff();
void doSetTorqueOnOff();
};
void UI::showMenu()
{
cout << "V02" << endl;
cout << OptionSetLEDOnOff << ".- Toggle AX-12 LED" << endl;
cout << OptionQueryAX12 << ".- Query AX-12" << endl;
cout << OptionMoveAX12 << ".- Move AX-12" << endl;
cout << OptionTorqueAX12 << ".- Torque AX-12" << endl;
cout << OptionQuerySensor << ".- Query CM-510 sensor" << endl;
cout << OptionShowMenu << ".- Show menu" << endl;
cout << OptionBeep << ".- Beep" << endl;
cout << OptionQuit << ".- Quit" << endl;
}
bool UI::nonValidOption(int option)
{
return (optionMaxOption);
}
int UI::selectOption()
{
int option=-1;
while (nonValidOption(option))
{
cout << "Select option (" << MinOption << "-" << MaxOption << ")" << endl; cin >> option;
if (nonValidOption(option))
{
cout << endl;
cout << endl;
cout << "(" << option << ") is NOT a valid option" << endl; } } return option; } bool UI::validRange(int value, int MinValue, int MaxValue) { return (value>=MinValue && value<=MaxValue);
}
int UI::getPosition()
{
int position=0;
do
{
cout << "Type a value between 0 and 1023 to set the AX-12 in that position" << endl; cin>>position;
}while(!validRange(position, 0, 1023));
cout << "Position:" << position << endl; return position; } int UI::getId() { int ax12Id=1; do { puts ("Type the ID of the AX-12 to use, a value between 1 and 18, "); cin >> ax12Id;
}while(!validRange(ax12Id, 1, 18));
cout << "AX12 Id:" << ax12Id << endl; return ax12Id; } int UI::getSensorPort() { int sensorPort=1; do { puts ("Type the sensor Port to read, a value between 1 and 6, "); cin >> sensorPort;
}while(!validRange(sensorPort, 1, 6));
cout << "Sensor Port number:" << sensorPort << endl;
return sensorPort;
}
void UI::doBeep()
{
cout << "Beep" << endl;
mySystem.dynamixelCommunication.sendOrder(100, AXS1_Buzzer, (byte) DO, (short) 500);
Sleep (2);
mySystem.dynamixelCommunication.sendOrder(200, MyCommunications::Beep, (byte) 5);
}
void UI::doGetPosition()
{
int ax12Id=getId();
int position = mySystem.dynamixelCommunication.readValue(ax12Id, MyCommunications::PresentPosition);
cout << "the position is: [" << position << "] " << endl << endl << endl;
}
void UI::doSetPosition()
{
int ax12Id=getId();
int position=getPosition();
mySystem.dynamixelCommunication.sendOrder(ax12Id, MyCommunications::GoalPosition,(short) position);
}
void UI::doQuerySensor()
{
int sensorPort=getSensorPort();
int value=mySystem.dynamixelCommunication.readSensorValue (200, ReadCM510SensorRaw, sensorPort);
cout << "the sensor reads: [" << value << "] " << endl << endl << endl;
}
void UI::doSetLEDOnOff()
{
byte lByte=0, hByte=0;
int ledValue;
int ax12Id=getId();
ledValue=mySystem.dynamixelCommunication.readValue(ax12Id, MyCommunications::LED);
Hex::toHexHLConversion(ledValue, hByte, lByte);
bool onOff=false;
if (lByte!=0)
onOff=true;
cout << "The LED is: [" << (onOff?"on":"off") << "], putting it: [" << (onOff?"Off":"on") << "] " << endl << endl << endl;
mySystem.dynamixelCommunication.sendOrder(ax12Id, MyCommunications::LED, byte(onOff?0:1));
}
void UI::doSetTorqueOnOff()
{
byte lByte=0, hByte=0;
int ax12Id=getId();
int torque=mySystem.dynamixelCommunication.readValue(ax12Id, MyCommunications::Torque);
Hex::toHexHLConversion(torque, hByte, lByte);
bool onOff=(lByte!=0?true:false);
cout << "The Torque is: [" << (onOff?"on":"off") << "], putting it: [" << (onOff?"Off":"on") << "] " << endl << endl << endl;
mySystem.dynamixelCommunication.sendOrder(ax12Id, MyCommunications::Torque,(byte) (onOff?0:1));
}
bool UI::executeSelectedOption(int option)
{
bool isOk=true;
cout << endl;
cout << endl;
cout << "Selected option: [" << option << "] " << endl; switch(option) { case OptionSetLEDOnOff: doSetLEDOnOff(); break; case OptionQueryAX12: doGetPosition(); break; case OptionMoveAX12: doSetPosition(); break; case OptionTorqueAX12: doSetTorqueOnOff(); break; case OptionQuerySensor: doQuerySensor(); break; case OptionShowMenu: showMenu(); break; case OptionBeep: doBeep(); break; } return isOk; }
The code for the Qt version is more complex, but as an example, the same operation that the example for the command line:
int MainWindow::getAX12_1_Id() { // Get the value from the edit control for the first AX-12 QString qStr=ui->SB_AX12_1_ID->text();
int id=Util::convertToInt(qStr.toStdString());
return id;
}
...
void MainWindow::on_B_AX12_1_SET_POS_clicked()
{
int id = getAX12_1_Id();
// Get the target position from the UI edit control as string and a convert it to int
short position= ui->SB_AX12_1_POS->text().toInt();
// Send the order to the AX-12 with the "id" to go to "position"
pDynComm->sendOrder(id, MyCommunications::GoalPosition,position);
}
Robotis CM-900 in Toss Mode and querying connected sensors
CM-900 is a very cheap and tiny Robotis Dynamixel controller based in the STM32 ARM 32 bits microcontroller. Here you can find several links to documents.
I think it’s ideal as a controller managed with, for example, a Raspberry Pi or a Pandaboard, so I have created a program which can be managed externally to control Dynamixel items (AX-12, AX-S1), Toss Mode, and sensors connected to the CM-900 with a simple protocol similar but simpler than the Dynamixel protocol:
[Headers] [ID] [Command code] [Nº Params] [Param1] [Param2] [ParamN]
Toss Mode works like USB2Dynamixel, but with this program in addition you can get sensor vales.
Toss Mode was a software feature, at least since CM-5, which allows you to use the elements connected to the controller from another device, usually a more powerful computer. What it receives from the USB or serial is sent to the Dynamixel bus, and whatever it receives from Dynamyxel bus is sent to the serial or USB connection.
Example for blinking twice: { 0xFF, 0xFF, 90, 1, 1, 2 }
Headers: 0xFF 0xFF
ID: 90
Command code: 1
Number of parameters: 1
Parameter #1: 2
Example for reading ultrasonic sensor (HC-SR04) : { 0xFF, 0xFF, 90, 2, 0 }
void MainWindow::testCM900_Sensor_US()
{
const int size=5;
byte buffer2[size] = { 0xFF, 0xFF, 90, 2, 0 };
pDynComm->rawWrite(buffer2, size);
}
Ultrasonic sensor hc-sr04
And here the file with the code for Robotis CM-9 IDE version 0.9.9. You also need to copy Prof. Mason library for ultrasonic sensor to CM 900 IDE libraries folder. Althought it i still a beta version, I think it works ok. Asigning true to these variables, the messages for debugging are sent either to USB connection o Serial2 (connecting Zigbee is the easy option):
bool debugOutputSerial2On=true; bool debugOutputOn=false;
[UPDATED] A simple but pretty complete Linux C++ example for Bioloid
[Jun 12, 2013, Some source code typos fixed and code updated]:
.
.
.
.
.
bool nonValidOption(int option)
{
return (optionMaxOption);
}
This is a very simple but pretty complete example, trying to use the least possible dependencies. I have tested in a just installed Ubuntu and you only needed the AXControl library (I created and use it because then there were no Robotis library and because it can use serial port, Dynamixel bus, Zigbee, BT (Robotis or other adaptors)
It use the terminal, no QT, to avoid library dependencies. But you should add the AXControl_v2 lib files to your /usr/lib folder as administrator (sudo). Click here if you want one unique executable for Linux (tested with Linux Mint 14) using these sources files
When executed, it shows the values configured in the HexaWheels.conf file and any problem it founds, like SerialPort2Dynamixel.open(ss)=> [Serial port NOT opened]:
ubuntu@A3820 ~/projects/trunk/bioloid/Herramientas/CommandLine $ ./commandline
init to load configuration(ss)=> [./HexaWheels.conf]
Configuration file=> (ss)=> [./HexaWheels.conf]
Tiempo_Espera_ms(ss)=> [35]
Tiempo_Espera_Sensores_ms(ss)=> [60]
Nombre_Puerto_Serie(ss)=> [/dev/ttyUSB0]
Baudios_Puerto_Serie(ss)=> [1000000]
Motion_File_Name(ss)=> [HexaWheels_2.0.mtn]
UmbralContacto(ss)=> [550]
UmbralDeteccion(ss)=> [200]
loadConfigurationValues: (si)=> [134529476]
getIntValue: (ss)=> [Tiempo_Espera_ms]
getIntValue: (ss)=> [Tiempo_Espera_Sensores_ms]
getIntValue: (ss)=> [Baudios_Puerto_Serie]
getStringValue: (ss)=> [Nombre_Puerto_Serie]
Device /dev/ttyUSB0, Speed B1000000
SerialPort2Dynamixel.open(ss)=> [Serial port NOT opened]
SerialPort2Dynamixel.open, serialPortName(ss)=> [/dev/ttyUSB0]
SerialPort2Dynamixel.open, baudRate(si)=> [4104]
I can’t open DynamixelCommunication, name: (ss)=> [/dev/ttyUSB0]
terminate called after throwing an instance of ‘std::exception*’
Aborted
If it can connect an there are no problem you should see:
ubuntu@A3820 ~/projects/trunk/bioloid/Herramientas/CommandLine $ ./commandline
init to load configuration(ss)=> [./HexaWheels.conf]
Configuration file=> (ss)=> [./HexaWheels.conf]
Tiempo_Espera_ms(ss)=> [35]
Tiempo_Espera_Sensores_ms(ss)=> [60]
Nombre_Puerto_Serie(ss)=> [/dev/ttyUSB0]
Baudios_Puerto_Serie(ss)=> [1000000]
Motion_File_Name(ss)=> [HexaWheels_2.0.mtn]
UmbralContacto(ss)=> [550]
UmbralDeteccion(ss)=> [200]
loadConfigurationValues: (si)=> [134529476]
getIntValue: (ss)=> [Tiempo_Espera_ms]
getIntValue: (ss)=> [Tiempo_Espera_Sensores_ms]
getIntValue: (ss)=> [Baudios_Puerto_Serie]
getStringValue: (ss)=> [Nombre_Puerto_Serie]
Device /dev/ttyUSB0, Speed B1000000
SerialPort2Dynamixel.open, serialPortName(ss)=> [/dev/ttyUSB0]
SerialPort2Dynamixel.open, baudRate(si)=> [4104]
V01
2.- Toggle AX-12 LED
3.- Query AX-12
4.- Move AX-12
5.- Torque AX-12
6.- Query CM-510 sensor
7.- Show menu
8.- Beep
9.- Quit
Select option (2-9)
/*------------------------------------------------------------------------------*\
* This source file is subject to the GPLv3 license that is bundled with this *
* package in the file COPYING. *
* It is also available through the world-wide-web at this URL: *
* http://www.gnu.org/licenses/gpl-3.0.txt *
* If you did not receive a copy of the license and are unable to obtain it *
* through the world-wide-web, please send an email to *
* siempre.aprendiendo@gmail.com so we can send you a copy immediately. *
* *
* @category Robotics *
* @copyright Copyright (c) 2011 Jose Cortes (<a href="https://plus.google.com/105007891378677151287/about">https://plus.google.com/105007891378677151287/about</a>) *
* @license http://www.gnu.org/licenses/gpl-3.0.txt GNU v3 Licence *
* *
\*------------------------------------------------------------------------------*/
#include "stdafx.h"
#include
#include "BasicSystem.h"
#include "Util.h"
using namespace std;
class UI
{
public:
static const int OptionQuit=8;
UI(MyBasicSystem::BasicSystem& aMySystem) : mySystem(aMySystem) {}
void showMenu();
int selectOption();
bool executeSelectedOption(int selected);
void doBeep();
private:
MyBasicSystem::BasicSystem mySystem;
static const int OptionSetLEDOnOff=1;
static const int OptionQueryAX12=2;
static const int OptionMoveAX12=3;
static const int OptionTorqueAX12=4;
static const int OptionQuerySensor=5;
static const int OptionShowMenu=6;
static const int OptionBeep=7;
static const int MinOption=OptionSetLEDOnOff;
static const int MaxOption=OptionQuit;
bool validRange(int value, int MinValue, int MaxValue);
bool nonValidOption(int option);
int getPosition();
int getId();
int getSensorPort();
void doGetPosition();
void doSetPosition();
void doQuerySensor();
void doSetLEDOnOff();
void doSetTorqueOnOff();
};
void UI::showMenu()
{
cout << "V02" << endl;
cout << OptionSetLEDOnOff << ".- Toggle AX-12 LED" << endl;
cout << OptionQueryAX12 << ".- Query AX-12" << endl;
cout << OptionMoveAX12 << ".- Move AX-12" << endl;
cout << OptionTorqueAX12 << ".- Torque AX-12" << endl;
cout << OptionQuerySensor << ".- Query CM-510 sensor" << endl;
cout << OptionShowMenu << ".- Show menu" << endl;
cout << OptionBeep << ".- Beep" << endl;
cout << OptionQuit << ".- Quit" << endl;
}
bool UI::nonValidOption(int option)
{
return (optionMaxOption);
}
int UI::selectOption()
{
int option=-1;
while (nonValidOption(option))
{
cout << "Select option (" << MinOption << "-" << MaxOption << ")" << endl; cin >> option;
if (nonValidOption(option))
{
cout << endl;
cout << endl;
cout << "(" << option << ") is NOT a valid option" << endl; } } return option; } bool UI::validRange(int value, int MinValue, int MaxValue) { return (value>=MinValue && value<=MaxValue);
}
int UI::getPosition()
{
int position=0;
do
{
cout << "Type a value between 0 and 1023 to set the AX-12 in that position" << endl; cin>>position;
}while(!validRange(position, 0, 1023));
cout << "Position:" << position << endl; return position; } int UI::getId() { int ax12Id=1; do { puts ("Type the ID of the AX-12 to use, a value between 1 and 18, "); cin >> ax12Id;
}while(!validRange(ax12Id, 1, 18));
cout << "AX12 Id:" << ax12Id << endl; return ax12Id; } int UI::getSensorPort() { int sensorPort=1; do { puts ("Type the sensor Port to read, a value between 1 and 6, "); cin >> sensorPort;
}while(!validRange(sensorPort, 1, 6));
cout << "Sensor Port number:" << sensorPort << endl;
return sensorPort;
}
void UI::doBeep()
{
cout << "Beep" << endl;
mySystem.dynamixelCommunication.sendOrder(100, AXS1_Buzzer, (byte) DO, (short) 500);
Sleep (2);
mySystem.dynamixelCommunication.sendOrder(200, MyCommunications::Beep, (byte) 5);
}
void UI::doGetPosition()
{
int ax12Id=getId();
int position = mySystem.dynamixelCommunication.readValue(ax12Id, MyCommunications::PresentPosition);
cout << "the position is: [" << position << "] " << endl << endl << endl;
}
void UI::doSetPosition()
{
int ax12Id=getId();
int position=getPosition();
mySystem.dynamixelCommunication.sendOrder(ax12Id, MyCommunications::GoalPosition,(short) position);
}
void UI::doQuerySensor()
{
int sensorPort=getSensorPort();
int value=mySystem.dynamixelCommunication.readSensorValue (200, ReadCM510SensorRaw, sensorPort);
cout << "the sensor reads: [" << value << "] " << endl << endl << endl;
}
void UI::doSetLEDOnOff()
{
byte lByte=0, hByte=0;
int ledValue;
int ax12Id=getId();
ledValue=mySystem.dynamixelCommunication.readValue(ax12Id, MyCommunications::LED);
Hex::toHexHLConversion(ledValue, hByte, lByte);
bool onOff=false;
if (lByte!=0)
onOff=true;
cout << "The LED is: [" << (onOff?"on":"off") << "], putting it: [" << (onOff?"Off":"on") << "] " << endl << endl << endl;
mySystem.dynamixelCommunication.sendOrder(ax12Id, MyCommunications::LED, byte(onOff?0:1));
}
void UI::doSetTorqueOnOff()
{
byte lByte=0, hByte=0;
int ax12Id=getId();
int torque=mySystem.dynamixelCommunication.readValue(ax12Id, MyCommunications::Torque);
Hex::toHexHLConversion(torque, hByte, lByte);
bool onOff=(lByte!=0?true:false);
cout << "The Torque is: [" << (onOff?"on":"off") << "], putting it: [" << (onOff?"Off":"on") << "] " << endl << endl << endl;
mySystem.dynamixelCommunication.sendOrder(ax12Id, MyCommunications::Torque,(byte) (onOff?0:1));
}
bool UI::executeSelectedOption(int option)
{
bool isOk=true;
cout << endl;
cout << endl;
cout << "Selected option: [" << option << "] " << endl;
switch(option)
{
case OptionSetLEDOnOff:
doSetLEDOnOff();
break;
case OptionQueryAX12:
doGetPosition();
break;
case OptionMoveAX12:
doSetPosition();
break;
case OptionTorqueAX12:
doSetTorqueOnOff();
break;
case OptionQuerySensor:
doQuerySensor();
break;
case OptionShowMenu:
showMenu();
break;
case OptionBeep:
doBeep();
break;
}
return isOk;
}
int _tmain(int argc, _TCHAR* argv[])
{
cout << "AXControl_v2_VS_CPP test v0" << endl;
bool quit=false;
MyBasicSystem::BasicSystem mySystem;
UI ui(mySystem);
do
{
ui.showMenu();
int selected=ui.selectOption();
if (selected==ui.OptionQuit)
quit=true;
ui.executeSelectedOption(selected);
}while (!quit);
mySystem.dynamixelCommunication.close();
return 0;
}
C++, Bioloid and Raspberry Pi (v0.2)
[V.02 updates: AX C++ architecture, core classes diagram and HexaWheels scanning video]
Why C++, Bioloid and Raspberry Pi?
C++, specially with the great improvements of the last C++11 standard, joins together a great efficiency in performance and a low memory footprint with advanced high level language features, making C++ a great tool for embedding, robotics, programming.
If you want to know how to use C++ very efficiently these two guides will help you:
– The JSF air vehicle C++ coding standards ( F-35 fighter aircraft)
– ISO C++ committee’s report on performance
.
.
Bioloid Premium
Bioloid Premium is a wonderful kit for creating legged and wheeled robots, including (here full parts list):
– 18 powerful and versatile AX-12 servos
– an ATMega 2561 (CM-510) or, recently, an ARM STM32F103RE 32bits (CM-530), based controller. Also you can control the AX-12 with the USB2Dynamixel straight from your USB with a FTDI driver.
– And a lot of parts to create the structure of the robot
.
RaspberryPi
Raspberry Pi is the cheaper and more brilliant conceived SBC (more specifications here):
– Broadcom BCM2835 SoC full HD multimedia applications processor
– 700 MHz Low Power ARM1176JZ-F Applications Processor
– Dual Core VideoCore IV® Multimedia Co-Processor
– 256/512 MB SDRAM
One simple example:
Learning C++
C++ is a very powerful but complex programming language, so I think that the better approach is to start step by step, from the most easy features (yes, C++ could be used in an easy way) to the most advanced features it offers. What is C++? I will quote (I try to not explain anything that already is explained), Stroustrup, “his father”, from his book The C++ programming language 3th Edition:
“C++ is a general-purpose programming language with a bias towards systems programming that
– is a better C,
– supports data abstraction,
– supports object-oriented programming, and
– supports generic programming.”
And wikipedia:
C++ (pronounced “see plus plus”) is a statically typed, free-form, multi-paradigm, compiled, general-purpose programming language. It is regarded as an intermediate-level language, as it comprises a combination of both high-level and low-level language features.[3] Developed by Bjarne Stroustrup starting in 1979 at Bell Labs, it adds object oriented features, such as classes, and other enhancements to the C programming language.
- Learn C++, no previous knowledge required
- www.cplusplus.com Tutorials
- MIT Open Courseware introduction to C++
- C++ FAQ
- Google Code University
- C++ Reference
If you want more C++ links, these found at JUCE will help you.
Programming — Principles and Practice Using C++
Free books and documents:
- Thinking C++, download Vol1 and Vol2
- One of the best summarized explanations about C++11
- C++11 new standard, a lot of changes from the previous version of the standard, like:
– Maintain stability and compatibility with C++98 and possibly with C;
– Improve C++ to facilitate systems and library design, rather than to introduce new features useful only to specific applications;
– Increase type safety by providing safer alternatives to earlier unsafe techniques;
– Increase performance and the ability to work directly with hardware
Books:
- Programming principles and practices using C++, by Bjarne Stroustrup
- C++ without fear, by Brian Overland, a very easy way to learn C++
Advancing:
In robotics, and embedded programming in general, we will need some advanced knowledge and practices to reach our goals.
Free books and documents:
- Design principles and patterns, by Robert C. Martin, about developing a sound software architecture
- Design Patterns, to develop a depth understanding of Object Orientation and to improve our designs
- Concurrent programming, threading Our robots we will need to do several actions simultaneously, like perceiving the world with several sensors, moving and deciding what to do to reach is objectives.
- Communications, the serial port communications functions are used for wireless and wired connections, and we will need to communicate between controllers and with sensors and servos.
Books:
- The C++ Programming Language, by Bjarne Stroustrup, in my opinion the best book to acquire a very depth knowledge about C++ programming. The C++ Programming Language (4th Edition, 2013)
- C++ Concurrency in Action, by Anthony Williams, a wonderful book about the new concurrency classes.
- More books
C++ robotics programming
Well, this is really the goal, robotics programming.
As this is a workshop it will follow the creation of the the walker and vehicle Hexapod showed above in the video. This is currently the core architecture and the HexaWheels module (namespace classes):
AX C++ architecture v2
And these are the core classes:
The workshop will include:
– Basics
Like communications with serial port and wireless, using Dynamixels, sensors, … Language features for robotics, like asynchronous communications and threads and… delays!.
– Intermediate
Combination of basics features using sensors (like scanning) and servos (walking motions). For example, scanning with a DMS sensor:
As a simple example:
– Advanced
Advanced perception and behaviours
I think this could very funny, using an advanced sensor like Asus Xtion, to detect certain objects to interact, and create configurable and amusing behaviours.
CM-510 mirocontroller programming
– Tools:
PC, Raspberry Pi and Pandaboard, installation and configuration, tool and projects
– GNU C++, Boost
– Eclipse
– QT 5
The contents will come soon, very soon…