Tag: C++

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;
}

Programming Lego Mindstorms NXT

Lego Mindstorms NXT is a great kit that allow you to build and program an almost unlimited variety of robots and automatisms.

NXT brick

Mindstorms NXT

The kit includes a “brick” that includes a programmable 32 bits microcontroller. Lego open sourced the software and the hardware of NXT, here you can find all the documents and source code.

You can program its microcontroller with the included visual programming tool, called NXT-G, that is very easy to use for small programs but probably you will find it too much limited, specially if you can program in other programming language. But don’t worry, you can use standard and professional programming languages like Java, C or C++ among others. All them free as in beer and as in speech.

Just a few words about which, in my opinion, are the best options:

C like language: NXC

Apart from NXT-G the easiest way for programming NXT is NXC. It’s a language similar to C, very easy to learn and you don’t should change the NXT Lego firmware, so you can continue using NXT-G.

Java: Lejos

If you know or want to learn Java this is your tool. I think that is the best tool, more difficult that NXC but far more powerful.

C/C++: NXT OSEK

Do you want to learn C or C++? Real time programming? This is your tool!

Choose hardware, firmware and language

This entry is part 1 of 3 in the series Programming Robotis Bioloid hardware

There are a lot of possible combinations of hardware, firmware and languages for programming Bioloid. I think that the table below show the the main combinations.

You can choose from the easy but limited Robotis own tool (Roboplus Task) and only your CM-5 or CM-510 to a SBC or “embedded” PC like Roboard and any language which can manage a serial port connection, like C, C++, Java, Python,…

Linux C++ Dynamixel reading and writing example

C# Dynamixel reading and writing example

Practical C++ programming tutorial for Bioloid

Programming Bioloid: choose hardware, firmware and languages

Programming Bioloid: choose hardware, firmware and languages

Robotis officially supports the programming solutions with the blue background:

  • The dark blue, RoboPlus Tasks, is the only one in which you can create the motions with RoboPlus Motion and execute it in CM-5/CM-510 with the program create with RoboPlus Tasks, after downloading the generated executable into the CM-5/CM-510, of course.

With these programming solutions you can use the Zigbee SDK to send and receive data between the CM5-/CM-510 and any computer, using Zig-110A or the new bluetooth BT-110, the Zig2Serial and the USB2Dynamixel. You can download example in Visual Basic .Net, C# and Visual C++

But there are more options!

Using a PC, SBC (Single Board Computer), PDA, Mobile or other light and battery powered computer

Using a serial port connection with your more beloved programming language:

  • USB2Dynamixel

If you have any device with a USB host and a FTDI driver you can use USB2Dynamixel to command programatically your Dynamixel servos using the Dynamixel protocol. You only will need your CM-5 or CM-510 to connect your Dynamixel to the battery.

  • Serial port cable

Same as the previous option but instead of using the USB2Dynamixel you only will need the serial cable and the “Toss Mode” launched with the ‘t’ command from the “Manage Mode”

  • Wireless control

Instead of only sending and receiving data, with the previous wireless connections you can command remotely your robot using the standard firmware and the “Toss Mode” launched with the ‘t’ command from the “Manage Mode”. You will need to command it using the Dynamixel protocol.

With these options and the CM-510 you will find a little problem… there is no way to read your sensor values! Well, you can use this firmware that offers a “Toss Mode” that supports reading CM-510 ports.

Start learning!

If you want to start learning how to program your CM-5 or CM-510 controller you will find interesting this post “Start programming  CM-5/CM-510 in C“. But may be you prefer to control your robot from a PC, SBC or other computer in C++ or C#

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;
}

Simple C++ class example using serial port, USB, wireless…

This entry is part 2 of 2 in the series Serial communications

This post is part of the Practical C++ programming tutorial for Bioloid

Here you can find a post serie about using serial port communications with C/C++ and C#, for Windows, Linux and microcontrollers.
This code is for Windows and Visual Studio and can be used for serial cable communications, USB2Dynamixel and indeed Zigbee:

Header:

class SerialPort {
private:
HANDLE serialPortHandle;

public:
SerialPort();
~SerialPort();

int connect ();
int connect (wchar_t *device);
//int connect (char *deviceName, int baudRate, SerialParity parity);
void disconnect(void);

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

void clear();
};

Body:


SerialPort::SerialPort() {
serialPortHandle = INVALID_HANDLE_VALUE;
}

SerialPort::~SerialPort() {
if (serialPortHandle!=INVALID_HANDLE_VALUE)
CloseHandle(serialPortHandle);

serialPortHandle = INVALID_HANDLE_VALUE;
}

int SerialPort::connect() {
return connect(L"COM1");
}

int SerialPort::connect( wchar_t* device) {
int error=0;
DCB dcb;

memset(&dcb,0,sizeof(dcb));

dcb.DCBlength = sizeof(dcb);

dcb.BaudRate = 57600;
dcb.Parity = NOPARITY;
dcb.fParity = 0;
dcb.StopBits = ONESTOPBIT;
dcb.ByteSize = 8;

serialPortHandle = CreateFile(device, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, NULL, NULL);

if (serialPortHandle != INVALID_HANDLE_VALUE) {
if(!SetCommState(serialPortHandle,&dcb))
error=2;
}
else {
error=1;
}

if (error!=0) {
disconnect();
}
else {
clear();
}

return error;
}

void SerialPort::disconnect(void) {
CloseHandle(serialPortHandle);
serialPortHandle = INVALID_HANDLE_VALUE;

//printf("Port 1 has been CLOSED and %d is the file descriptionn", fileDescriptor);
}

int SerialPort::sendArray(unsigned char *buffer, int len) {
unsigned long result;

if (serialPortHandle!=INVALID_HANDLE_VALUE)
WriteFile(serialPortHandle, buffer, len, &result, NULL);

return result;
}

int SerialPort::getArray (unsigned char *buffer, int len) {
unsigned long read_nbr;

read_nbr = 0;
if (serialPortHandle!=INVALID_HANDLE_VALUE)
{
ReadFile(serialPortHandle, buffer, len, &read_nbr, NULL);
}

return((int) read_nbr);
}

void SerialPort::clear() {
PurgeComm (serialPortHandle, PURGE_RXCLEAR | PURGE_TXCLEAR);
}

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;
}

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

%d bloggers like this: