A Servo Driver using the ZDK
by Trenton Henry
11/03/21
This article is about using the ZDK to write a simple servo driver for the ZEMNCU boards. Many MCUs include special purpose hardware for driving PWM with varying capabilities. But most of them can only drive one pin per PWM module, and most of them have a very small number of PWM modules. Driving an RGB LED requires 3 channels, one per color, and driving servos for a robot requires one channel per servo. Often there isn't enough dedicated PWM hardware to go around. So instead of trying to get into the non-portable hardware details of the MCU, this article illustrates a way to drive multiple channels (servos, LEDs ...) with only minimal hardware resources.
When there are multiple channels / pins operating as PWM there are two ways to schedule them:
* Serial
* Parallel
Channels running in serial pulse one at a time, with each channel on for a time then off for a time, followed by the next channel, in succession. Each channel is given a time slot that occupies a portion of the period, and a timer measures the on time, then the off time, of each channel one after the other. Obviously the period must be a multiple of the time slot for this to work.
|<------------------------------------------------->| period
|<---------->|<---------->|<---------->|<---------->| slots
+------------+------------+------------+------------+
| +---+ | +---+ | +---+ | +---+ | on/off pulses
| -+ +--- | -+ +--- | -+ +--- | -+ +--- |
+------------+------------+------------+------------+
Examples:
Period Slot Slots
--------- --------- -----
20 msec 2000 usec 10 using nominial values
20 msec 2500 usec 8 using wider slots
22.5 msec 2500 usec 9 depending on the tolerance of the period
Channels running in parallel all start their pulses at the same time, at the start of the period, and then end them individually depending on the width of each pulse. The maximum resolution is determined by the capabilities of the timer, and the CPU.
|<-------------------------------------------------->| period
+----------------------------------------------------+
| |
| +-------+ | channel 0
| | +--------------------------------------- |
| |
+----------------------------------------------------+
| |
| +-----------------------+ | channel 1
| | +----------------------- |
| |
+----------------------------------------------------+
| |
| +-------------------------------------+ | channel 2
| | +--------- |
| |
+----------------------------------------------------+
| |
| +---------------+ | channel 3
| | +------------------------------- |
| |
+----------------------------------------------------+
Conveniently, the nominal period of typical hobby servos is 20 milliseconds, and each servo pulse is approximately 1 to 2 milliseconds wide, where ~1 millisecond is ~0 degrees, and ~2 milliseconds is ~180 degrees. Different servos from different manufacturers vary, so the pulses may range from ~500 to ~2500 microseconds, and the period may not be exactly 20 milliseconds. So, depending on the servos, it is possible to run 8 or 10 servos serially. The shortest interval that the timer needs to measure is ~500 microseconds, and the longest is ~2500.
Every 20 msec each servo has a 2500 usec slot during which its pin is pulsed. 8 x 2500 usec slots fit into one 20 msec period. Therefore 8 servos can be driven back to back, serially, using a single timer.
|---------------------------------------------------------------------------------------| 20 msec
slot 0 slot 1 slot 2 slot 3 slot 5 slot 5 slot 6 slot 7
|----------|----------|----------|----------|----------|----------|----------|----------| 2500 usec slots
| ----____ ----____ ----____ ----____ ----____ ----____ ----____ ----____ on time / off time
Some bookkeeping data is needed in order to implement this. This is probably overkill with the allowed and the enabled etc but it isn't terribly heavyweight.
#define _min_pulse 500 // usec
#define _max_pulse 2500 // usec
#define _max_servo 8 // 8 servos max 2500 usec ea 20 msec total period.
static const U32 allowed=0b11000000000000001100001100110100; // pins that can drive servos
static U32 enabled=0b00000000000000000000000000000000; // pins enabled as servos
static const U32 masks[_max_servo]={
0b00000000000000000000000000000100, // 0 on _pa02
0b00000000000000000000000000010000, // 1 on _pa04
0b00000000000000000000000000100000, // 2 on _pa05
0b00000000000000000000000100000000, // 3 on _pa08
0b00000000000000000000001000000000, // 4 on _pa09
0b00000000000000000100000000000000, // 5 on _pa14
0b00000000000000001000000000000000, // 6 on _pa15
0b01000000000000000000000000000000, // 7 on _pa30
};
static U16 pulses[_max_servo]={};
static U16 update[_max_servo]={
_min_pulse, _min_pulse, _min_pulse, _min_pulse,
_min_pulse, _min_pulse, _min_pulse, _min_pulse,
}; // _min_pulse to _max_pulse, in usec
static I8 ix=-1; // index of active servo
And there are two ISRs, one to turn the active servo off, and one to turn next servo on.
static VOID off_isr(VOID); // forward
static VOID on_isr(VOID) { // turn on the next servo
pulses[ix]=update[ix];
if (++ix>=_max_servo) ix=0; // next servo, wrapping at max
PG_A->OUTSET.reg=masks[ix]&enabled&allowed; // on
after(u2t(pulses[ix]), off_isr); } // stay on this long
static VOID off_isr(VOID) {
PG_A->OUTCLR.reg=masks[ix]&enabled&allowed; // off
I32 remaining=_max_pulse-_min_pulse-pulses[ix];
if (remaining<=0) on_isr();
else after(u2t(_max_pulse-_min_pulse-pulses[ix]), on_isr); } // wait until end of slot
And there is a function to enable a servo, and another to disable a servo.
VOID enservo(U8 s, U16 pulse) {
RETURN_IF(s>=_max_servo);
BOOL go=enabled==0;
enter_critsec();
update[s]=MAXOF(_min_pulse, MINOF(_max_pulse-_min_pulse, pulse));
enabled|=(masks[s]&allowed);
exit_critsec();
if (go) on_isr(); }
VOID disservo(U8 s) {
RETURN_IF(s>=_max_servo);
enter_critsec();
enabled&=~masks[s];
if (enabled==0) ix=-1;
exit_critsec(); }
That is a minimal core set of capabilities for a serialized servo driver.
And here is an example use of it:
#include "zdk.h"
TIOB tiob={};
VOID servos_max(VOID);
VOID servos_min(VOID) {
enservo(2, 1000);
enservo(3, 1000);
enservo(4, 1000);
enservo(5, 1000);
after(s2t(2), servos_max); }
VOID servos_mid(VOID) {
enservo(2, 1500);
enservo(3, 1500);
enservo(4, 1500);
enservo(5, 1500);
after(s2t(2), servos_min); }
VOID servos_max(VOID) {
enservo(2, 2000);
enservo(3, 2000);
enservo(4, 2000);
enservo(5, 2000);
after(s2t(2), servos_min); }
CC_NORETURN VOID main(VOID) {
enfcpu8(); // run at 8 MHz
ensystick(); // enable the systick timer
enable_tcs(); // enable the TC timers
tiob=enhif(); // enable the host interface, uart or usb
pin_as_output(_pa05); // configure 4 pins as outputs to use as servos
pin_as_output(_pa08);
pin_as_output(_pa09);
pin_as_output(_pa14);
servos_mid3(); // set the servos to the middle position
while (1); } // idle forever
That's it. The servos just spin back and forth. Obviously a real project would read some inputs or take some command to decide the position of each servo. This just shows how to build a servo driver using the ZDK.
End