• Top
  • Comment
  • Reply

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

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

By

9th Jun 2013
© 2011 Shahmir Javaid - http://shahmirj.com/blog/33

Andrew Wrzesinski

1st Dec 2013

hi im looking for the servoUART.cpp file and your link is broken could you possibly send it to me?

Shahmir Javaid

1st Dec 2013

Eeek we changed to github. Ive updated the link

Hang Cui

3rd Dec 2013

Hi, I try to use servoUSB.cpp on pcDuino in order to control pololu maestro, but it always returns a error value. And when I try to use servoUART.cpp I don't know how to deal with /dev/ttyAMA0. Hope I can get some advice. Thank you!

Shahmir Javaid

3rd Dec 2013

@Hang Cui, im afraid the above is tested on the Pi. If you are using serial try using the pololu on static baudrate to see if it makes a difference.

For our project we run the reset pin every time the program restarts just incase it has mangled the transfer of data from last run. You can manually hard reset the board before starting your program.

If you do figure get it working on the pcDuino please post back, might help someone else.

Hi, i'm wondering how did you manage to connect rxd, txd 3v3 logic to micro servo uart 5v logic without converters? i'm thaving an issue with connection serial servo controller to rpi. It starts blinking green, plus constant red leds.

Shahmir Javaid

6th Apr 2014

I just connected the the 5V to the Vin. So (http://elinux.org/RPi_Low-level_peripherals) P1-04 -> Vin and P1-06 -> ground. Note I am using this one http://www.pololu.com/product/1352. But should work with the 6 channel one.

I did notice that in my program I had to drive reset pin high every time i restart the program. Also you will need to configure it to be in serial mode.

You can see https://github.com/dashee-bot/dashee/blob/master/server/src/ServoController/UART.cpp file for how the UART settings are done if it helps

yes, I saw you use USB version. It is a bit different, but protocol is the same, first two bytes are different, 0x80 and 0x1. I use P1-02 -> VIN and P1-06 -> ground. And I do reset it before use as you mentioned. After reset yellow led becomes off, then I send 3000 to a servo. I can see it turns on and moves slightly. Yellow led turns on as well. Then whenever I send to servo it starts blinking green with constant red. I was thinking it might be related to 3v3 vs 5 logic. But looks like you managed to make it work without logic converters... Don't have any other ideas how to make it work so far...

i found the issue :) I supplied a power to the servo direclty from rpi from 5v. Looks like RPi couldn't provide enough current.

Shahmir Javaid

6th Apr 2014

Sweet!, yes sorry I just remembered my servo's are actually powered off a different battery but my board is powerd through the pi.

Petr Pacner

8th Jun 2014

Hi,
first excuse me for my english,i'm Czech.
Since wednesday have i tried to connect my RPi with Mini Maestro 12, but without success.
I have done everything you have written on your page, but when i run the program (./servoUART), the cursor jump on the next line and nothing happens, than i must kill the program.
After this experience i was looking for solution for my problem, but i found just one forum with the same problem as my as well

http://stackoverflow.com/questions/20772110/raspberry-pi-uart-serial-wont-work

I also proved script rpi-serial-console, but it didn't work, so i removed it.
I think my UART pins aren't shortcutted because i took another RPi (the same type B) and there were the same problem.
Now i haven't any idea what to do.

please could you give me some advice.

Thanks

BTW i use OS raspbian wheezy 7.5.

Shahmir Javaid

8th Jun 2014

It sounds like the settings on the board are not correctly set. Try connecting it using a computer with USB and launch the settings. These settings should be able to let you set the controller board in UART mode. Look into the manual for the board online to see which settings need to be changed for the board to be set in Serial/UART mode.

Petr Pacner

9th Jun 2014

Thanks for your help, now it works. The problem was broken wire between RPi and Maestro12.

Elijah Paul

21st Aug 2017

Shahmir Javaid thank you for this post. I am looking for the source as I am a beginner to C programming in the Pi. If you could please be so kind to provide me with what your reference material is :

How did you know the kernel uses the tx and rx pins for itself?
How did you know about the getty using it?

how did you learn this C programming specifically for Pi, you typed in some _base thing and assigned a value etc.. Please let me know how you got that information

Shahmir Javaid

21st Aug 2017

- Not sure what you mean by _base.
- The code files are attached in the blog (at the top).
- There are links to the board getting started guides that point you in the right direction.

Elijah Paul

24th Aug 2017

Thanks I got it Shahmirji,

I cannot complile the servoUART.cpp code, I get this error

g++ -Wall -c "servoUART.cpp" (in directory: /home/pi/Desktop)

in file included from /usr/include/features.h
from /usr/include/fcntl.h:25
from servoUART.cpp:13:

error:expected unqualified-id before 'extern' __BEGIN_DECLS.

I removed the header <fcntl.h> and the error persists for the next header file.

Any help is appreciated

Elijah Paul

25th Aug 2017

Thank you Shahmir. Can you help me on this situation.I am unable to compile servoUART.cpp

it does not compile fcntl.h
if i off that it does not compile the next header

This is the error message

error expected unqualified-id before extern _BEGIN_DECLS

Elijah Paul

27th Aug 2017

Ok i got everything working and servoUART.cpp compiled and ran giving the same output as you described. But the servo which i connected to channel 1 did not move. How to make it move. Do I change the channel variable to a number?

Shahmir Javaid

29th Aug 2017

try: maestroSetTarget(fd, 1, 50)



Back to Top
All content is © copyrighted, unless stated otherwise.
Subscribe, @shahmirj, Shahmir Javaid+