Arduino, when the Servo library is not good enough…


I went back to try and finish this ball balancing project, but after some improvements I started to get frustrated by the fact that the servos kept being “jittery”.

Time for my new and shiny servo tester I thought… ! It turned out I was seeing the same problem.

I then tried with some other servos, and they had the same problem, so, convinced it was to do with cheap servos and mechanical problems, I went ahead and order new, more expensive ones. It’s funny how biased we become once we think we understand something 🙂

Got the new servos, and they were being as jittery as the other ones…

Great, time for my “new” oscilloscope I thought (the one that I bought months ago, and never really used 🙂 ):

"New" Siglent SDS1102CNL Oscilloscope

“New” Siglent SDS1102CNL Oscilloscope

Then I realised that the problem was not with the servos, but that the PWM generated was noisy, as you can see in the video at the top:

  • the 1st half shows the signal generated with an Arduino using Hardware PWM (any of the 3 timers, configured directly through the respective registers) – the fluctuations in the length of the 1500 microseconds signal are ~0.1 μs
  • then in the 2nd half I revert to using the Arduino Servo library (what both my servo tester and the ball balancing projects were using) – the fluctuations become ~ 5-10 μs, which is actually what was causing the “jittery” servos !

Once I found the real problem, I went back and started by improving the Servo Tester itself.

I initially wanted to use the Timer 0, as the PWM output are on pins 5 & 6 where my connectors were already in place.

However due to the fact that this is a 8bit timer only, I was having resolution issues: the smallest “step” was going to be 64μs which was too much.

The only 16bit timer on the Arduino is the Timer 1, BUT that one is used by the millis(), micros() and probably other Arduino functions. Luckily, I don’t need them, so went ahead and wrote the necessary low level / registers configuration.

Now the “resolution” is 0.5 μs which is probably too much so I artificially decrease it to 1 μs.

I would later on realise that, this is actually how the initial / old Servo library was probably working, back in the time when one could only use pins 9 & 10.

* trandi 25 Oct 2015
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#include <ooPinChangeInt.h>
#include <AdaEncoder.h>
#include "Switch.h"

// for ooPinChangeInt library
// #define NO_PORTB_PINCHANGES // to indicate that port b will not be used for pin change interrupts
#define NO_PORTC_PINCHANGES // to indicate that port c will not be used for pin change interrupts
#define NO_PORTD_PINCHANGES // to indicate that port d will not be used for pin change interrupts

#define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET);

AdaEncoder encoderA = AdaEncoder('a', 5, 6);
AdaEncoder encoderB = AdaEncoder('b', 8, 11);
int16_t lastClicksA=0, lastClicksB=0, currentClicksA=0, currentClicksB=0, posA=3003, posB=3003;
uint8_t incrementA=10, incrementB=10;

Switch swA = Switch(13);
Switch swB = Switch(12);

int16_t MIN_POS = microsecsToPos(600);
int16_t MAX_POS = microsecsToPos(2400);

void setup(){
  display.begin(SSD1306_SWITCHCAPVCC, 0x3D);  // initialize with the I2C addr 0x3D (for the 128x64)
  // Can't use the standard Arduino Servo library as the signal produced has errors of 1-10uSecs
  // This is enough to produce "jitter" in the Servos.
  // The below method using directly Timer1 in fast PWM mode, reduces that to max ~0.1uSecs
  pinMode(9, OUTPUT);  // OC1A
  pinMode(10, OUTPUT);  // OC1B
  // Output frequency : 16 MHz / 8 / 32768 = 61.03515625Hz -> A total period of 16384uSecs
  TCCR1A = _BV(WGM11); // WGM 1110 -> Fast PWM (on 16 bits), top ICR1
  TCCR1B = _BV(WGM13) | _BV(WGM12);
  // below it's important to set at least 1 of each COM1[A/B]x, to enable BOTH OC1A and OC1B !
  TCCR1A |= _BV(COM1A1) | _BV(COM1B1); //COM 10 -> Clear OC1A/OC1B on Compare Match, set OC1A/OC1B at BOTTOM (non-inverting mode)
  TCCR1B |= _BV(CS11); // CS 010 -> pre scaler 8
  ICR1 = 32768;  // 2^15

void loop(){
  currentClicksA = encoderA.getClicks();
  currentClicksB = encoderB.getClicks();
  if(swA.getCount() > 6) swA.reset();
  incrementA = swCountToIncrement(swA.getCount());
  if(swB.getCount() > 6) swB.reset();
  incrementB = swCountToIncrement(swB.getCount());
  posA -= ((currentClicksA - lastClicksA) * incrementA) << 1; // multiply by 2 so that we change 1 uSec each time
  posB -= ((currentClicksB - lastClicksB) * incrementB) << 1; 
  posA = constrain(posA, MIN_POS, MAX_POS);   
  posB = constrain(posB, MIN_POS, MAX_POS); 
  display.fillRect(0, 0, map(posA, MIN_POS, MAX_POS, 0, 128), 15, WHITE); 
  display.setCursor(0, 16); 
  display.print(" :"); 
  display.fillRect(0, 32, map(posB, MIN_POS, MAX_POS, 0, 128), 15, WHITE); 
  display.setCursor(0, 48); 
  display.print(" :"); 

  // The length of the pulse will be (OCR0x + 1) * 64 uSecs 
  // OCR0X = Period in uSecs * 2 - 1 
  OCR1A = posB; // OCR1A -> PIN 9
  OCR1B = posA; // OCR1B -> PIN 10
  lastClicksA = currentClicksA;
  lastClicksB = currentClicksB;

int16_t posToMicrosecs(int16_t pos) {
  return (pos - 3) >> 1;  // divide by 2. The "-3" rather than "+1" is just so that on my Oscilloscope I get exactly the right amount of uSecs

int16_t microsecsToPos(int16_t microsecs) {
  return (microsecs << 1) + 3; // multiply by 2. We should then substract 1, but there are a few uSecs shown missing on my oscilloscope when doing that. So "+3" to compensate.

uint8_t swCountToIncrement(uint8_t count) {
  switch(count) {
    case 0: return 10;
    case 1: return 20;
    case 2: return 50;
    case 3: return 100;
    case 4: return 1;
    case 5: return 2;
    case 6: return 5;

Also, as you can see in the code, I had to use different pins, more exactly 9 & 10 instead of the old 5 & 6.
This required some soldering re-work, and also learning the hard way that, for the AdaEncoder library the 2 pins of each encoder HAVE to be connected to Arduino pins of the SAME PORT !

Updated Schematic. Using pins 9 & 10 that correspond to Timer 1

Updated Schematic. Using pins 9 & 10 that correspond to Timer 1


At the end of the day, I’m quite pleased as I not only solved this frustrating problem, but also got to properly use the oscilloscope for once, and I can confirm that I couldn’t have investigated and debugged the problem without such a tool !

8 Responses to Arduino, when the Servo library is not good enough…

  1. Daniel says:


    I think I have found an improvement in Line 52. You could set ICR1 to 40000, which would result in an exact frequency of 50Hz. It would also increase the resolution up to about 15,287-bit.


    • trandi says:

      Thanks for the suggestion. But will that improved resolution make any difference when driving a servo for example ?

      • Daniel says:

        First of all, most RC servos are built for receiving a 50Hz PWM signal. That is the first reason why utilizing 50Hz is better then 61.035Hz.

        The second reason is that the resolution gets increased.
        Just check for yourself: 20000us(=1/50Hz) divided by 40000 equals 0.5us, whereas 16393us(=1/61Hz) divided by 32768 equals 0.500288us. This doesn’t seem much, but looking at a standard servo-controller IC datasheet (M51660L) shows that the minimum “dead bandwidth” is at 1.5us. At first glance, this looks like it still fits the requirements by a factor of 3, but keep in mind that you don’t operate the PWM in the range of 1.5us. The 180° angle requires a positive pulse width of 2500us. By dividing this time with our two possible resolution times (0.5us and 0.500288us) shows that there is a difference of 3 counter-counts. Multiply these 3 counts with the two resolution times (0.5us and 0.500288us) you can see, that the 50Hz solution stays exactly underneath the dead bandwidth of the IC, whereas the 61Hz solution is off by one “servo-internal increment”. This results in a real-life error of 0.135°.

        So I guess one can live with that amount of error, but it is still worth mentioning. Maybe the dead bandwidth gets smaller in the next few years. Since it only requires the change of ICR1 I will stick with my solution. 🙂

  2. Pingback: Ball Balancing – V2 | Robotics / Electronics / Physical Computing

  3. Francis says:


    I have a problem when compiling

    I get the following error “expected class-name before ‘{‘ token” about the class switch

    thank you to enlighten me.

    Francis in france

    • trandi says:


      C’est tout simplement parce qu’il n’y a pas de class “Switch”.
      Il s’agit d’une petite classe qui gere les 2 boutons, mais j’ai oublie de copie/coller son contenu donc le compiler ne la trouve pas.
      Enleve le “include” et toutes les references a la classe.


  4. Ytai says:

    Or should we acknowledge the fact that you’ve outgrown Arduinos and it’s time to move on the the big kids’ stuff? 🙂 I feel like these are not the kind of problems one should be worrying about in 2015…

    • trandi says:

      This is a double edge sword… Arduino and it’s library is trying to solve exactly this problem: not having to worry about this in 2015. It doesn’t work all the time, but still…
      On this little project I also use an OLED Screen and 2 encoders, they just worked flawlessly out of the box thanks to Arduino libraries !!
      I don’t even want to imagine the amount of work necessary to search and put together code for these 2, on other “big kid’s” platforms 🙂

      The really great stuff about Arduino is its community and the huuuuge amount of information available!
      I found it quite surreal that I would find more info when searching for “Arduino timers” than “Atmega328 timers” for example.


Leave a Reply

Fill in your details below or click an icon to log in: Logo

You are commenting using your account. Log Out /  Change )

Google photo

You are commenting using your Google account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )

Connecting to %s

%d bloggers like this: