Archive

Practical C++ programming tutorial for Bioloid robots

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

This is the main block of a simple Dynamixel example program using serial port.

Do you want to use the usb2dybnamixel? Changing it will be very easy. Here you can find several combinations of hardware, firmware and programming tools.

Here the Visual Studio C++ zipped project. And if you use this firmware you also can query sensor port values with this example:

Read more

Reading Dynamixel AX-12+ position

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

The intent of this code is to be clear and easy to understand, it not pretend to be the best code to do the job.

This method create the command to read the AX-12+ position :

int Dynamixel::getReadAX12PositionCommand(byte id)
{
    //OXFF 0XFF ID LENGTH INSTRUCTION PARAMETER1 …PARAMETER N CHECK SUM
    int pos = 0;

    buffer[pos++] = 0xff;
    buffer[pos++] = 0xff;
    buffer[pos++] = id;

    // length = 4
    buffer[pos++] = 4; //placeholder

    //the instruction, read => 2
    buffer[pos++] = 2;

    // pos registers 36 and 37
    buffer[pos++] = 36;

    //bytes to read
    buffer[pos++] = 2;

    byte checksum = checkSumatory(buffer, pos);
    buffer[pos++] = checksum;

    return pos;
}

int Dynamixel::getPosition(SerialPort *serialPort, int idAX12)
{
	int ret=0;

	int n=getReadAX12PositionCommand(idAX12);
	long l=serialPort->sendArray(buffer,n);
	Sleep(2);

	memset(bufferIn,0,BufferSize);
	n=serialPort->getArray(bufferIn, 8);

	short pos = -1;
	if (n>7)
	{
		pos = fromHexHLConversion(bufferIn[5], bufferIn[6]);
	}

	printf("nid=<%i> pos=<%i> length=<%i>n", idAX12, pos, n);
	if (pos<0 || pos > 1023)
		ret=-2;
	else
		ret=pos;

	return ret;
}

Writing Dynamixel AX-12+ position

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

The intent of this code is to be clear and easy to understand, it not pretend to be the best code to do the job.

This method create the command to write the AX-12+ position :

int Dynamixel::getSetAX12PositionCommand(byte id, short goal)
{
    int pos = 0;
    byte numberOfParameters = 0;
    //OXFF 0XFF ID LENGTH INSTRUCTION PARAMETER1 …PARAMETER N CHECK SUM

    buffer[pos++] = 0xff;
    buffer[pos++] = 0xff;
    buffer[pos++] = id;

    // bodyLength
    buffer[pos++] = 0; //place holder

    //the instruction, query => 3
    buffer[pos++] = 3;

    // goal registers 30 and 31
    buffer[pos++] = 0x1E;// 30;

    //bytes to write
    byte hexH = 0;
    byte hexL = 0;
    toHexHLConversion(goal, &hexH, &hexL);
    buffer[pos++] = hexL;
    numberOfParameters++;
    buffer[pos++] = hexH;
    numberOfParameters++;

    // bodyLength
    buffer[3] = (byte)(numberOfParameters + 3);

    byte checksum = checkSumatory(buffer, pos);
    buffer[pos++] = checksum;

    return pos;
}

int Dynamixel::setPosition(SerialPort *serialPort, int idAX12, int position)
{
	int error=0;

	int n=getSetAX12PositionCommand(idAX12, position);
	long l=serialPort->sendArray(buffer,n);
	Sleep(10);

	memset(bufferIn,0,BufferSize);
	n=serialPort->getArray(bufferIn, 8);

	if (n>4 && bufferIn[4] == 0)
		printf("nid=<%i> set at pos=<%i>n", idAX12, position);
	else {
		error=-1;
		printf("nid=<%i> error: <%i>n", idAX12, bufferIn[4]);
		bf(bufferIn, n);
	}

	return error;
}

Linux C++ Dynamixel reading and writing example

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

If you will like to use some boards like Raspberry or Beagleboard C++ is a great choice. So here you have a working Linux C++ Dynamixel reading and writing example. The main difference is the serial port access:

Here you can download the Eclipse project zipped. and here a simple but pretty complete Linux C++ example for Bioloid

Here you can find several combinations of hardware, firmware and programming tools.

The main body

#include
using namespace std;

#include "SerialPort.h"
#include "Dynamixel.h"

int main() {
cout << "AX Control starts" << endl; // prints AX Control int error=0; int idAX12=18; SerialPort serialPort; Dynamixel dynamixel; if (serialPort.connect("//dev//ttyS0")!=0) { dynamixel.sendTossModeCommand(&serialPort); int pos=dynamixel.getPosition(&serialPort, idAX12); if (pos>250 && pos <1023)
dynamixel.setPosition(&serialPort, idAX12, pos-100);
else
printf ("nPosition <%i> under 250 or over 1023n", pos);

serialPort.disconnect();
}
else {
printf ("nCan't open serial port");
error=-1;
}

cout << endl << "AX Control ends" << endl; // prints AX Control
return error;
}

Header

#ifndef SERIALPORT_H_
#define SERIALPORT_H_

#include
#include
#include
#include

class SerialPort {
private:
int fileDescriptor;

public:
int connect ();
int connect (char * device);
void disconnect(void);

int sendArray(unsigned char *buffer, int len);
int getArray (unsigned char *buffer, int len);

int bytesToRead();
void clear();
};

#endif /* SERIALPORT_H_ */

Body:

#include
#include <sys/ioctl.h>

#include "SerialPort.h"

int SerialPort::connect() {
return connect("//dev//ttyS0");
}

int SerialPort::connect(char *device) {
struct termios terminalAttributes;

/*
* http://linux.die.net/man/2/open
*
* Open the serial port
* read/write
* not become the process's controlling terminal
* When possible, the file is opened in nonblocking mode
*
*/
fileDescriptor = open(device, O_RDWR | O_NOCTTY | O_NDELAY | O_FSYNC );

// clear terminalAttributes data
memset(&terminalAttributes, 0, sizeof(struct termios));

/*    http://linux.die.net/man/3/termios
*
*  control modes: c_cflag flag constants:
*
* 57600 bauds
* 8 bits per word
* Ignore modem control lines.
* Enable receiver.
*/

terminalAttributes.c_cflag = B57600 | CS8 | CLOCAL | CREAD;

/*
* input modes: c_iflag flag constants:
*
* Ignore framing errors and parity errors.
* (XSI) Map NL to CR-NL on output.
*/
terminalAttributes.c_iflag = IGNPAR |  ONLCR;

/*
* output modes: flag constants defined in POSIX.1
*
* Enable implementation-defined output processing.
*/

terminalAttributes.c_oflag = OPOST;

/*
* Canonical and noncanonical mode
*
* min time
* min bytes to read
*/

//terminalAttributes.c_lflag = ICANON;
terminalAttributes.c_cc[VTIME] = 0;
terminalAttributes.c_cc[VMIN] = 1;

/*
* http://linux.die.net/man/3/tcsetattr
* Set the port to our state
*
* the change occurs immediately
*/

tcsetattr(fileDescriptor, TCSANOW, &terminalAttributes);

/*
* http://linux.die.net/man/3/tcflush
*
* flushes data written but not transmitted.
* flushes data received but not read.
*/

tcflush(fileDescriptor, TCOFLUSH);
tcflush(fileDescriptor, TCIFLUSH);

return fileDescriptor;
}

void SerialPort::disconnect(void)
{
close(fileDescriptor);
printf("nPort 1 has been CLOSED and %d is the file descriptionn", fileDescriptor);
}

int SerialPort::sendArray(unsigned char *buffer, int len) {
int n=write(fileDescriptor, buffer, len);
return n;
}

int SerialPort::getArray (unsigned char *buffer, int len)
{
int n1=bytesToRead();
int n=read(fileDescriptor, buffer, len);
return n;
}

void SerialPort::clear()
{
tcflush(fileDescriptor, TCIFLUSH);
tcflush(fileDescriptor, TCOFLUSH);
}

int SerialPort::bytesToRead()
{
int bytes=0;
ioctl(fileDescriptor, FIONREAD, &bytes);

return bytes;
}

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

%d bloggers like this: