Linux C++ Dynamixel reading and writing example

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

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

The main body

[sourcecode language="cpp"]
#include <iostream>
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;
}
[/sourcecode]

Header

[sourcecode language="cpp"]
#ifndef SERIALPORT_H_
#define SERIALPORT_H_

#include <stdio.h>
#include <termios.h>
#include <fcntl.h>
#include <unistd.h>

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_ */
[/sourcecode]

Body:

[sourcecode language="cpp"]
#include <string.h>
#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;
}
[/sourcecode]