Tag: Dynamixel

CM-510 firmware for using as multiple sensor item updated

CM-510 firmware and post updated

 

 

(I) Reading sensors connected to Robotis CM-510

This entry is part 5 of 5 in the series Bioloid C++ tutorial

cm-510 sensorsUsing 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…

Robotis CM-900 in Toss Mode and querying connected sensors

This entry is part 3 of 5 in the series Programming CM-900

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.

CM-900_Size

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

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;

Robotis CM-900 as a tosser for Dynamixel commands

This entry is part 2 of 5 in the series Programming CM-900

CM-900 is really tiny and cheap, so is perfect to use as communication bridge between any computer (Raspberry Pi, Pandaboard, etc. included, of course) and the Dynamixel bus. Whatever it receives from the Serial USB (usually commands and queries) is sent to the Dynamixel bus, and what it receives from the Dynamixel bus is sent to the SerialUSB (usually answers)

Here is the source code of the little program for CM-900 IDE:

int counter;
bool onlyOnceHappened;

void blinkOnce()
{
    digitalWrite(BOARD_LED_PIN, LOW);
    delay_us(100);
    digitalWrite(BOARD_LED_PIN, HIGH);
}

void setup()
{  
  pinMode(BOARD_LED_PIN, OUTPUT);

  onlyOnceHappened=false;
  counter=0;

  //USB Serial initialize
  SerialUSB.begin();
//  SerialUSB.attachInterrupt(USBDataReceived);
  //DXL initialize
  Dxl.begin(1);  
}

byte aByte=0;
uint8 aUint8;

void loop() 
{	
//  SerialUSB.println (counter++);

  if (onlyOnceHappened==false)
  {    
    blinkOnce();
    onlyOnceHappened=true;
    delay (3000); //Some time to the user to activate the monitor/console
    SerialUSB.println ("v1.1.1 Orders receiver started");  
  }    
  
  if (SerialUSB.available())
  {
    aUint8=SerialUSB.read();
    blinkOnce();
    Dxl.writeRaw(aUint8);
//    delay(20);
  }   
  
  if (Dxl.available())
  {
    aByte=Dxl.readRaw();
    blinkOnce();
    SerialUSB.write(aByte);
//    delay(20);
  } 
}

Here the file.

In the next post I will include an improved version that could read sensors connected to the CM-900, “expanding” the Dynamixel protocol.

So… let’s the Bioloid, C++ and QT games start!

This entry is part 4 of 6 in the series Bioloid Workshop

To cretate this workshop I’m using Ubuntu 12.04, GNU g++ (Ubuntu/Linaro 4.6.3-1ubuntu5) 4.6.3 (see Synaptic installer capture),

Synaptic G++

Synaptic G++

QT 5.0.1 and QT creator 2.6.2,

QT creator is a very easy, and good, free IDE with a great design tool for creating user interfaces:

Be careful with this bug: Qt Creator 2.0: UI layout is not updated before launching the application http://qt-project.org/forums/viewthread/292 Is very annoying and time consuming, the solution is in the last comment (setting the UI_DIR in the .pro file)

QTCreator Bioloid QTWorkshop

QTCreator Bioloid QTWorkshop

I also will use it with Windows 7.

I also use Boost to get some C++11 features for threading. For example:

pWorkThread = new thread(&Activity::doWork, this, parameter);

std::lock_guard < std::mutex > guard(myMutex);

Don’t bother if you don’t understand it right now, it’s in the guts of the AXControl_v2 library we will use it in a very easy way, as you will see below. Here you will find a lot of resources to learn basic and advanced C++

Example context fot std::lock_guard < std::mutex > guard(myMutex);


short DynamixelCommunication::readSensorValue(int id, AX12Address address,
int port) {
std::lock_guard < std::mutex > guard(myMutex);
short value = -1;
try {
int size = getReadSensorWordCommand(buffer, (byte) (id), address,
(byte) (port));
memset(&result, 0, MaxBufferSize);
serialPort.query(buffer, result, size, WaitTimeReadSensor);
value = getQueryResult(result, size);
if (value <= 0)
Debug::show("DynamixelCommunication.readSensorValue", value);

} catch (exception e) {
Debug::show("DynamixelCommunication.readSensorValue", e.what());
}
return value;
}

And context for pWorkThread = new thread(&Activity::doWork, this, parameter);:


class Activity {
private:

protected:
std::thread *pWorkThread;

...

}

void Activity::start(int parameter) {
working = true;
pWorkThread = new thread(&Activity::doWork, this, parameter);
Util::sleepMS(10);
}
Let's see how to open the connection

Let’s see how to open the connection to the CM-510:

void MainWindow::on_B_Open_clicked()
{
//QString output="Open it";
//QMessageBox::information(this, "Output", output, QMessageBox::Ok);

//dynComm.open("/dev/ttyUSB0",57600); //open with these parameters
if (pDynComm->open()) //open getting the parameter from the configuration fiel
{
//Connection opened
setConnectionButtons(true); //User interface update
updateAX12Parameters(ui->SB_AX12_1_ID->text().toInt()); //User interface update
}
else
{
//Show problem opening the connection
string cantOpenPortName="I can't open port: ";
cantOpenPortName+=pConf->getStringValue(DynamixelCommunication::ParameterSerialPortName);

QMessageBox::information(this, "Error", QString::fromStdString(cantOpenPortName), QMessageBox::Ok);
}
}

and get a beep from the Robotis CM-510 (using this alternative firmware):

pDynComm->sendOrder(200, MyCommunications::BeepCM510, 0); //200 is the Dynamixel device ID used for the CM-510, 0 because beep doesn't need any additional value

Getting the AX12 position and showing it:

void MainWindow::on_B_AX12_1_GET_POS_clicked()
{
    QString qStr;

    int id = getAX12_1_Id();

    int position = pDynComm->readValue(id, MyCommunications::PresentPosition);

    ui->SB_AX12_1_POS->setValue(position);

Setting the AX12 position:

void MainWindow::on_B_AX12_1_SET_POS_clicked()
{
    int id = getAX12_1_Id();

    int position= ui->SB_AX12_1_POS->text().toInt();
    pDynComm->sendOrder(id, MyCommunications::GoalPosition,position);
}

Putting the selected AX12 LED on and off

void MainWindow::on_CH_AX12_1_LED_clicked(bool checked)
{
    int id = getAX12_1_Id();

    pDynComm->sendOrder(id, MyCommunications::LED, (checked?1:0));// if checked 1, else 0
}

And an auxiliary UI method to get the id from the selected AX-12

{
int MainWindow::getAX12_1_Id()
{
    QString qStr=ui->SB_AX12_1_ID->text();
    int id=Util::convertToInt(qStr.toStdString());

    return id;
}

You can download sources and Linux binaries here

A diagram with all the AXControl_v2 lib classes and their methods:

c++ signatura

c++ signatura

[Update]

QT5 Bioloid Workbench

QT5 Bioloid Workbench

C++, Bioloid and Raspberry Pi (v0.2)

This entry is part 3 of 6 in the series Bioloid Workshop

[V.02 updates: AX C++ architecture, core classes diagram and HexaWheels scanning video]

Why C++, Bioloid and Raspberry Pi?

TCPL4thEnglishC++, 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

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

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


Starting:

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.

Web resources:

If you want more C++ links, these found at JUCE will help you.

Programming -- Principles and Practice Using C++Programming -- Principles and Practice Using C++

Programming — Principles and Practice Using C++

Free books and documents:

– 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:

Advancing:

In robotics, and embedded programming in general, we will need some advanced knowledge and practices to reach our goals.

Free books and documents:

  • 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:

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

AX C++ architecture v2

And these are the core classes:

todo_signatura

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…

Playing With Qt 5, C++ and Bioloid

I have been searching the best UI (libraries and UI creating tools) multiplatform framework for several years, and QT 5 is the best option I have found. Its libraries are easy to use, specially the slots (UI widgets connection to code) of QT5.  QT Creator is pretty easy to use and the UI designer is the free best tool I have tested. Really I’m learning to use it as creating these examples.

A very simple example:

QT Bioloid Workshop

QT Bioloid Workshop

The code uses two classes from the AXControl_v2 library:

Configuration, it loads the  parameters from the file HexaWheels.conf (currently in spanish):

Tiempo_Espera_ms=36 // wait time
Tiempo_Espera_Sensores_ms=60 // wait time for sensors
Nombre_Puerto_Serie=/dev/ttyUSB0 // serial port name
Baudios_Puerto_Serie=57600 // baud rate

DynamixelCommunication, it offers the commands to control Dynamixel items

Connecting to the Dynamixel bus:

void MainWindow::on_B_Open_clicked()
{
pDynComm->open(); // open the connection using the parameters from the configuration file
setConnectionButtons(true); // enable and disable conveniently the buttons
}

Getting and setting the AX12 Dynamixel position:

void MainWindow::on_B_AX12_1_GET_POS_clicked()
{
QString qStr;

int id=ui->SB_AX12_1_ID->text().toInt();
int position = pDynComm->readValue(id, MyCommunications::PresentPosition);
string str=std::to_string(position);

ui->E_AX12_1_POS->setText(qStr.fromStdString(str));
}

void MainWindow::on_B_AX12_1_SET_POS_clicked()
{
QString qStr=ui->SB_AX12_1_ID->text();
int id=Util::convertToInt(qStr.toStdString());

int position= ui->E_AX12_1_POS->text().toInt();
pDynComm->sendOrder(id, MyCommunications::GoalPosition,position);
}

And this is the (unfinished) main window code:

#include

#include "mainwindow.h"
#include "ui_mainwindow.h"

#include "Configuration.h"
#include "DynamixelCommunication.h"
#include "Util.h"

MainWindow::MainWindow(QWidget *parent) :
QMainWindow(parent),
ui(new Ui::MainWindow)
{
ui->setupUi(this);
pConf=new Configuration ("/home/jose/proyectos_svn_win/trunk/bioloid/Comun/CPP/AXControl_v2/src/HexaWheels.conf");
pDynComm=new DynamixelCommunication (pConf);
}

MainWindow::~MainWindow()
{
delete ui;

pDynComm->close();
delete pDynComm;
delete pConf;
}

void MainWindow::setConnectionButtons(bool onOff)
{
ui->B_Open->setEnabled(!onOff);
ui->B_Beep->setEnabled(onOff);
ui->B_Close->setEnabled(onOff);
}

void MainWindow::on_B_Open_clicked()
{
//QString output="Open it";
//QMessageBox::information(this, "Output", output, QMessageBox::Ok);

//dynComm.open("/dev/ttyUSB0",57600);
pDynComm->open();

setConnectionButtons(true);
}

void MainWindow::on_B_Beep_clicked()
{
pDynComm->sendOrder(200, MyCommunications::BeepCM510, 5);
}

void MainWindow::on_B_Close_clicked()
{
pDynComm->close();
setConnectionButtons(false);
QString output="Port closed!";
QMessageBox::information(this, "Output", output, QMessageBox::Ok);

}

void MainWindow::on_B_AX12_1_GET_POS_clicked()
{
QString qStr;

int id=ui->SB_AX12_1_ID->text().toInt();
int position = pDynComm->readValue(id, MyCommunications::PresentPosition);
string str=std::to_string(position);

ui->E_AX12_1_POS->setText(qStr.fromStdString(str));
}

void MainWindow::on_B_AX12_1_SET_POS_clicked()
{
QString qStr=ui->SB_AX12_1_ID->text();
int id=Util::convertToInt(qStr.toStdString());

int position= ui->E_AX12_1_POS->text().toInt();
pDynComm->sendOrder(id, MyCommunications::GoalPosition,position);
}

But it’s “growing”:

QT_Workbench

QT_Workbench

I have uploaded the source and binary of this work in progress to
https://www.box.com/s/mdsdeoem0gg4o2rapipj/s/mdsdeoem0gg4o2rapipj

%d bloggers like this: