Controlling servomotors on a Raspberry Pi

For a project (documentation to be added soon) of low-cost telepresence robot, I had to handle three servomotors with a Raspberry Pi. I chose a Raspberry Pi board as it is very cheap and has a decent camera which can be easily used to stream video at a good resolution and framerate, without using too much CPU. Then, I wanted to control everything with the Raspberry Pi, including the servomotors. Here comes the troubles…

First of all, I had to control three servomotors: two continuous rotation servos for the wheels, and an extra standard servo for the camera orientation. First revisions of the RaspberryPi only have one hard PWM, shared with the audio circuit, which makes it really difficult to use for this purpose. But Raspberry Pi 2 has two hardware PWMs (one is shared with the audio circuit, preventing you from using the onboard audio output). That is enough for my use case, as I have two continuous rotation servos which have to be precise (and will be controlled via hard PWM) and an extra servo for the camera orientation which can be easily controlled in soft PWM.

My code can be found here. The important files are the header of Servo class, the actual Servo class and a file containing some extra constants.

Hard PWM

Next problem is to find a way to control them easily. On an Arduino, you just have to include the Servo library and write angles to your servos. On the Raspberry Pi, on the contrary, there are no such things. WiringPi is a nice C library to handle GPIOs (including PWM) but there are no default failsafe settings to use, and one has to play a lot with the different configuration commands.

Most of the servos expect pulses of variable width, sent on a 20ms basis. Each 20ms, it samples the wave to get the width of the pulse. The base PWM frequency on the Raspberry Pi is 19.2MHz, and you can set a clock prescaler (clock) and a number of samples (range). The base frequency of your PWM signal will then be given by the formula (as 50Hz = 20ms):

19.2MHz / (clock * range) = 50Hz

The stop position is for 1.5ms pulses in general, for a continuous rotation servo. Then, we also want that there exists a valid int such that we could write it to stop the servo. pwmWrite(pin, STOP_VALUE) should stop the servo, with STOP_VALUE being an int. This is important as servos are symmetric with respect to this position and a slight offset will make the calibration of the servo speeds really difficult.

Good results were obtained on my robot with a clock prescaler of 200 and a range of 1920. Then, the stop value was 141 and the full forward value was 155 (value to write to make the servo turn at full speed). This gives me 14 different speeds for the servo, which is more than enough in my case. If you want finer control, you should play with the clock prescaler and range values.

Soft PWM

Then comes the control of the camera orientation using Software PWM. I explored two possibilities: ServoBlaster which is not compatible out of the box with the Raspberry Pi 2 (but a solution exists) and the software PWM library from wiringPi. Both gives equivalent results, including jitter (a bit less jitter for ServoBlaster, but still too much for a stable camera orientation). Then, I used wiringPi which was easier to integrate in my code. I found a dirty hack to fix servo jitter, as explained below.

First, the base frequency of the software PWM is 100Hz, but we need 50Hz for the servo control. Then, we have to softPwmCreate with a range of 200. We can then write values to the softPwm, and it will most likely work, at least if there are not much processes running.

My problem was that once I started the camera capture, my software pwm got preempted a lot, and this was giving a lot of jitter. This was really funny because when I was using autocompletion in the shell for instance, my servo had some jitter that I could hear. I tried to play with nice without success and could not get rid of the jitter.

However, I finally found a really hacky way of getting rid of the jitter (may not be applicable in your case): if I stop sending PWM signal to the servo, it holds still. Plus, if I send a 0 value on the PWM, this is an out-of-range value for the servo and he just ignores it, holding still as well. Then, the solution was easy. I just had to send the soft PWM signal to the servo, wait a bit until he has moved (based on the average speed in the datasheet, I know how long I have to wait) and then send 0 on the software PWM. This way, the servo holds still, and there is no more jittering.

Hope this helps :)