Raspberry Pi and Pololu Servo Controller using C

To acheave our androneee project we had to get some sort of Servo controller, which we can extend easily and use multiple servo's no matter what the base operator. For this we decided to choose the Pololu Servo Controller. This servo can be controlled using USB or UART. I have created two helloworld programs to test how the servo's work

• servoUSB.cpp
• servoUART.cpp

The USB version uses the Compact protocol as documented here, whereas the UART version uses the Pololu protocol.

Before we begin you must configure the Pololu Servo Controller in the correct mode, using the getting started guide for the Pololu board. For USB I use USB Dual Mode and for the UART I use UART, detect baud rate mode. So given your project, please adjust the board accordingly.

For this tutorial I will talk about the UART program as it is the only one I can get working on my Raspberry Pi.

Setting up your PI for UART

Before you can actually use Rx and Tx to communicate, you need the kernel to not use these for it self. To do this edit your /boot/cmdline.txt and remove the options that have console=ttyAMA0,115200 and kgdboc=ttyAMA0,115200, so your /boot/cmdline.txt looks like:

smsc95xx.turbo_mode=N dwc_otg.lpm_enable=0 console=tty1 \
root=/dev/mmcblk0p2 rootfstype=ext4 elevator=noop rootwait

Note the above is in one line only, the \ is just to break a line without actually doing so.

You also then need to get getty to not use /dev/ttyAMA0, to disable this edit /etc/initd and remove/comment the following line as I have done using #:

#T0:23:respawn:/sbin/getty -L ttyAMA0 115200 vt100

Now you are ready, but you will require a reboot of the Pi before these changes can take effect. It might not be a bad idea to have your Raspberry Pi up to date, so run the following and get a coffee:

sudo apt-get update
sudo apt-get dist-upgrade
sudo apt-get upgrade
sudo reboot

Installing the Wiring

You now need to get wires from the Raspberry Pi to your Pololu Servo Controller. See the Picture I took of my setup, I am using the 5V power(Pin 4) and Ground(Pin 6) to power my board, and; Tx(Pin 8) and Rx(Pin 10) to communicate with my board. You can find a good pin numbering diagram here.

Note that Tx from the Pi goes to Rx on the Pololu board, and respectively Rx on the Pi goes to Tx on the Pololu board. A Picture will help here:

Running our program

I will explain the program later, but you should now be able to compile servoUART.cpp and run it with success, using:

> g++ servoUART.cpp -o servoUART
> ./servoUART
Error is 0.
Current position is 0.
Setting target to 7000 (1750 us).
Current position is 7000.
Setting target to 5000 (1250 us).
Current position is 5000.

Note running the second time, the board will fail and a red light will be lit, To solve this currently I just take the power out and back again, This problem can be solved by driving the reset pin low on the Pololu board. However for this tutorial I will leave this.

The program explained

Our program does the following tasks

• Open the device
• Set the UART baud rate and settings
• Get any Board Errors
• Get/Set/Get values to ensure our values are working
• Close the device

Opening

To open we simply use the open to get our file handler, which will be used by the error, get and set functions. To open a serial device we run:

const char * device = "/dev/ttyAMA0";  // Linux
int fd = open(device, O_RDWR | O_NOCTTY);
if (fd == -1)
{
    perror(device);
    return -1;
}

This will open our terminal device in Read and Write mode, and return a handler to the fd variable.

Setting UART options

Before we can read and write to our device, we need to set our UART options to do this we run the following after opening our device:

struct termios options;
tcgetattr(fd, &options);
cfsetispeed(&options, B9600);
cfsetospeed(&options, B9600);

options.c_cflag &= ~PARENB;
options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS8;

// no flow control
options.c_cflag &= ~CRTSCTS;

options.c_cflag |= CREAD | CLOCAL;  // turn on READ & ignore ctrl lines
options.c_iflag &= ~(IXON | IXOFF | IXANY); // turn off s/w flow ctrl

options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // make raw
options.c_oflag &= ~OPOST; // make raw

// see: http://unixwiz.net/techtips/termios-vmin-vtime.html
options.c_cc[VMIN]  = 0;
options.c_cc[VTIME] = 20;

if (tcsetattr(fd, TCSANOW, &options) < 0)
{
    perror("init_serialport: Couldn't set term attributes");
    return -1;
}

This will set the Baud rate and the Parity, Please read more at http://linux.die.net/man/3/termios to understand the options in detail.

See if there are any board errors

int maestroGetError(int fd)
{
    unsigned char command[] = { 0xAA, 0xC, 0x21 };
    if (write(fd, command, sizeof(command)) != 3)
    {
        perror("error writing");
        return -1;
    }

    int n = 0;
    unsigned char response[2];
    do
    {
        int ec = read(fd, response+n, 1);
        if(ec < 0)
        {
            perror("error reading");
            return ec;
        }
        if (ec == 0)
        {
            continue;
        }
        n++;

    } while (n < 2);

    return (int)sqrt(response[0] + 256*response[1]);
}

Given a file descriptor, It will send 3 bytes to the board, 0xAA, 0xC and 0x21, where 0xAA is a constant, 0xC represents the device number (by default 12, but can be changed in the board configuration) and 0x21 the command which represents the error.

After this we try to read 2 bytes back from the board, We do our UART communication byte by byte as read will return 0 if it is in wait mode. Which means that we can try polling read again till the response byte is set. the loop will continue till the response character is set.

We then turn the two byte character into a recognizable number before we return.

Setting a Target

Setting a Target is alot less complicated as the only thing involved is writing to the device.

int maestroSetTarget(int fd, unsigned char channel, unsigned short target)
{
    unsigned char command[] = {0xAA, 0xC, 0x04, channel, target & 0x7F, target >> 7 & 0x7F};
    if (write(fd, command, sizeof(command)) == -1)
    {
        perror("error writing");
        return -1;
    }
    return 0;
}

The Pololu protocol byte represent the following

1. 0xAA - Constance as required by the board
2. 0xC - The device number
3. 0x04 - The Command type (SetTarget)
4. channel - The byte representing a channel number
5. data-byte-1 - The first databyte
6. data-byte-2 - The second databyte.

Note that 5th and 6th databyte make up one number, Please see the Pololu Protocol documentation to understand the logic.

Getting a Target

This function is similar to the maestroGetError(...) function, the only difference is the protocol. For this the following data is written to the board

unsigned char command[] = {0xAA, 0xC, 0x10, channel};

Where the first byte is a constant 0xAA, the second represents the device number 12(0xC) the third represent the command 0x10 and the last represents the channel

Given this command, we then try reading two bytes, which represent the same data used in maestroSetTarget(...).

Conclusion

I recommend you try out the servoUART.cpp and please let me know if you run into any problems I will try helping as much as I can. Also it will be a wise Idea to understand the termios functions try man 2 termios in command line.

Good Luck