Hardware Servo: Example Code

Hi everyone! I need to drive a few servos and I almost despaired when I found out the hard way that the Servo library has not yet been rewritten for the Due.
But then I read about the Due hardware PWM support. Using the code below (or something similar) you can drive up to four servos without software overhead, using only the PWM clock.

It doesn't convert from angles to microsecond pulses but you can do that easily enough.

DueServo.h:

/**
 * Hardware servo library. Available pins on the Arduino Due: 6, 7, 8, 9.
 */
#ifndef DUE_SERVO_H
#define DUE_SERVO_H

#include <Arduino.h>

#define PWM_DUTY_MIN    544
#define PWM_DUTY_MAX    2400

void writeMicros(int pin, uint16_t dutyCycle);
void initServo(int pin, uint16_t dutyCycle);
#endif

DueServo.cpp:

/**
 * Hardware Servo library. Available pins on the Arduino Due: 6, 7, 8, 9
 */
#include "DueServo.h"

#define PWM_CLOCK       1000000
#define PWM_PERIOD      20000

static bool PWMEnabled = false;

void initServo(int pin, uint16_t dutyCycle) {
        if(!PWMEnabled) {
                pmc_enable_periph_clk(PWM_INTERFACE_ID);
                PWMC_ConfigureClocks(PWM_CLOCK, 0, VARIANT_MCK);
                PWMEnabled = true;
        }
        const PinDescription *config = &g_APinDescription[pin];
        int channel = config->ulPWMChannel;
        if(channel == NOT_ON_PWM)
                return;

        PIO_Configure(
                config->pPort,
                config->ulPinType,
                config->ulPin,
                config->ulPinConfiguration);
        PWMC_ConfigureChannel(PWM_INTERFACE, channel, PWM_CMR_CPRE_CLKA, 0, 0);
        PWMC_SetPeriod(PWM_INTERFACE, channel, PWM_PERIOD);
        writeMicros(pin, dutyCycle);
        PWMC_EnableChannel(PWM_INTERFACE, channel);
}

void writeMicros(int pin, uint16_t dutyCycle) {
        int channel = g_APinDescription[pin].ulPWMChannel;

        if(channel == NOT_ON_PWM)
                return;

        if(dutyCycle < PWM_DUTY_MIN)
                dutyCycle = PWM_DUTY_MIN;
        if(dutyCycle > PWM_DUTY_MAX)
                dutyCycle = PWM_DUTY_MAX;

        PWMC_SetDutyCycle(PWM_INTERFACE, channel, dutyCycle);
}

Sketch code, alternating the servo between two positions:

#include <Arduino.h>
#include <DueServo.h>

#define SERVO_PIN 6
bool high = false;

void setup() {
        initServo(SERVO_PIN, 900);
}

void loop() {
        high = !high;
        writeMicros(SERVO_PIN, high?1200:900);
        delay(500);
}

EDIT: better function names
EDIT2: correct #include<Arduino.h> in DueServo.h

Sebastian,

I had been trying to generate a 24MHz clock for an image sensor and this post got me there. Thank you. I had tried poking around with some code you had put up in a thread on timer interrupts (which seemed to me the most obvious solution) but getting an output faster than about 200KHz seemed over my head. I believe I was limited by the speed of "write," even with the faster "port manipulation" method.

I also tried modifying some code from stimmer who's been working on VGA output. Unfortunately his code seemed to highjack every clock on the chip; I was getting 24MHz on the SCL pin, which is way to fast for an I²C bus. I couldn't use the serial port, either. Regardless, I'm up and running thanks to this Servo example. I hope this can help someone else heading down a similar path. In that vein, I'll share what I've discovered.

The timing for a square wave can be set by manipulating three factors in the example code: PWM_CLOCK, PWM_PERIOD, and dutyCycle. PWM_CLOCK is set to 1 000 000, let's say one million "ticks." PWM_PERIOD is set to 20 000, so our pin will go high every 20 000 "ticks." The dutyCycle variable is initially set at 1200, so the pin will remain high for 1200 "ticks" then go low. It will go high again after the end of the period; after 18800 more "ticks." (1200 + 18800 = 20 000 = PWM_PERIOD)

Obviously, the dutyCycle must be less than or equal to PWM_PERIOD, and PWM_PERIOD must be less than or equal to PWM_CLOCK. I'm not certain about the top speed of PWM_CLOCK, but it's somewhere between 48 000 000 and 100 000 000. (If I had to make a bet, I'd guess 84MHz is the magic number.) Outputting a 24MHz wave the squareness appears much more sine, though this may be a result of my low bandwidth scope that hasn't been calibrated since 1989. Eh, the image sensor eats it up and runs just fine, so I don't worry.

I'm glad I could help

And thank you for a very good explanation of the parameters! I made PWM_CLOCK, PWM_PERIOD, PWM_DUTY_MIN and PWM_DUTY_MAX constants inside the library because they need no modification if you use the code as a servo library.

But in your case and any similar ones it might be better to make them parameters to the writeMicros() method so the caller can set them freely. Also a rename of the methods might be proper, because if you modify PWM_CLOCK it will no longer be microseconds you write. initPWM() and writePWM, perhaps?
Of course, PWM_DUTY_MIN and PWM_DUTY_MAX are only relevant for the servo implementation, you will most probably want to remove them completely or modify them to suit your needs.

/Sebastian

Hi,
I got hold of an Arduino Due today so will have a go at getting this library ported to Due - it is useful not because it keeps the pin count low, but because the 4017s will take car of the level shifting for the servos.

I assume that the Arduino team are working on porting the existing servo library ? and so there would be nothing to gain by porting the following library other than the PPM Reading ?

Duane B

rcarduino.blogspot.com

DuaneB:
I assume that the Arduino team are working on porting the existing servo library ?

I've pushed a first porting of the Servo library (the one distributed with Arduino IDE) for the Due, you can find it on the latest revision of the ide-1.5.x branch.

I did some rapid tests, and it seems to work.

DuaneB:
and so there would be nothing to gain by porting the following library other than the PPM Reading ?

Please do it, porting your 4017 library to the Due would be great! The Servo library "consume" 1 Timer every 12 servo connected, porting your library allows us to keep Timers for other tasks.

Thanks for writing this code, I was also surprised that the Due rolled out without a formal way to manage servo control. Thanks for the code. As far as integrating the modified servo.h library to use, how does one take the code for the modified servo.h and integrate into my working copy onto one's computer?

cmaglie:
I've pushed a first porting of the Servo library (the one distributed with Arduino IDE) for the Due, you can find it on the latest revision of the ide-1.5.x branch.

Added Servo library for Arduino Due · arduino/Arduino@db81f52 · GitHub

I did some rapid tests, and it seems to work.

I can see the code at Added Servo library for Arduino Due · arduino/Arduino@db81f52 · GitHub

but I do not know how to get these files and how to put them in my Arduino Due IDE?

Please have mercy with a beginner!

Can I drive a servo with pins 26, 28 and 30 ?

Greetings, Conrad

In attachment a zip with a copy of the files. Unzip it into hardware/arduino/sam/libraries.

Servo_Due.zip (8.24 KB)

cmaglie:
In attachment a zip with a copy of the files. Unzip it into hardware/arduino/sam/libraries.

Thank you, I did that, and the example Sweep compiled.

How about using pins 26, 28 and 30 for a Servo?

Does it have to be pin 9?

Greetings, Conrad

P.S.: I tried with pins 26, 28 and 30. There seems to be something going on, because I see the LEDs connected (via a 1 K resistor to GND) to these pins dimming and getting brighter as the Servo example Sweep should do. Seems I am set up, thank you C.

P.S.: Servo works on pin 30. So, download the library (ZIP-File) from cmaglie's post above this post, and you are set to use Servos with the Arduino Due.

Hello everyone!

After reading the data sheet properly, I've updated my hardware servo library to use all available PWM channels and all available output pins, see the following table. This overrides the Arduino distribution variant.cpp pin configuration g_APinDescription[] for the Due, which is far from complete.

This means you can output a servo signal to any of the above pins, but only one pin per channel. For example, you cannot output two different servo signals on pin A10 and CANTX at the same time because they are both driven by the hardware PWM channel 3.

It is possible to output a servo/PWM signal on a bunch of other pins as well by using the complementary PWM signal for each channel but the above pins suit me well enough not to warrant writing the code for handling inversion of duty cycle parameters (and thereby increasing the runtime size of the code)

To use the library, simply unpack the ZIP into your libraries folder, restart Arduino and open the DueServo example Sweep.

DueServo.zip (2.55 KB)

Leonardo Hardware PWM Servo Control Library:

Just in case other leonardo users come to this page in search of salivation, I've written the above library to allow access to all of the available PWM controllers at their full resolutions. YMMV, but it seems to work well for me at least. 8)

I would have liked to incorporate it into the existing library by the OP, but the only preprocessor tag for the leonardo is in use by the teensy 2.0 and I wasn't sure how to solve that problem.