Double Servo Tester with Display

This is something I’ve already worked on but was never really happy with the result, mainly because the potentiometre that I used was of poor quality and hence non linear and “jumpy”…

The problem is simple: I need to have a reliable and easy to use servo tester, that I can use to test/play with a standard RC servo or ESC or anything else that uses the same control protocol.

This necessity has arisen again quite recently, while working on the 2nd iteration of my ball balancing device.

So here’s my 2nd (and hopefully last) attempt at making something nice and functional…

There’s not much to this project, except some patience for the wiring and a quite expensive BOM for what it does 🙂

Proof of concept

Proof of concept

Again, not many bits went into it:

  • 2 rotary encoders from Sparkfun (with RGB LED on them, but unused)
  • 1 monochrome OLED from Adafruit
  • an Arduino nano pro clone from eBay
The few standard components

The few standard components

Hardest bit was the manual wiring...

Hardest bit was the manual wiring…



And surprisingly it worked from the 1st try

Almost there...

Almost there…

And now with a nice case, necessary for its protection, though it doesn’t do justice to the nice and shiny OLED Display.




And of course, the Arduino code running on the microcontroller, using the adaencoder library for the rotary encoders which depends on ooPinChangeInt library.

This of course, beyond the standard Servo, SPI, Wire and Adafruit libraries…

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

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

#include <Servo.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', 10, 11);
AdaEncoder encoderB = AdaEncoder('b', 8, 9);
int16_t posA=1500, posB=1500, lastClicksA=0, lastClicksB=0, currentClicksA=0, currentClicksB=0;
int8_t pressesA=0, pressesB=0;

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

Servo servoA;
Servo servoB;

void setup(){
 display.begin(SSD1306_SWITCHCAPVCC, 0x3D); // initialize with the I2C addr 0x3D (for the 128x64)

void loop(){
 currentClicksA = encoderA.getClicks();
 currentClicksB = encoderB.getClicks();
 pressesA = swA.getCount() + 1;
 if(pressesA > 5) {
 pressesA = 1;
 pressesB = swB.getCount() + 1;
 if(pressesB > 5) {
 pressesB = 1;
 posA += (currentClicksA - lastClicksA) * pressesA * 10;
 posB += (currentClicksB - lastClicksB) * pressesB * 10;
 posA = constrain(posA, 600, 2400);
 posB = constrain(posB, 600, 2400);
 display.fillRect(0, 0, map(posA, 600, 2400, 0, 128), 15, WHITE);
 display.setCursor(0, 16);
 display.print(posA); display.print(" :"); display.print(pressesA);
 display.fillRect(0, 32, map(posB, 600, 2400, 0, 128), 15, WHITE);
 display.setCursor(0, 48);
 display.print(posB); display.print(" :"); display.print(pressesB);
 lastClicksA = currentClicksA;
 lastClicksB = currentClicksB;

And the small utility class Switch.h that you see imported in the above Arduino code:

#if defined(ARDUINO) && ARDUINO >= 100
 #include <Arduino.h>
 #include <pins_arduino.h>
 #include <WProgram.h>

class Switch : public CallBackInterface {
 Switch (uint8_t _pin): pin(_pin){
 pinMode(pin, INPUT);
 PCintPort::attachInterrupt(pin, this, RISING);

 void cbmethod() {
 // do NOT do any software debounce, as we plan on having a hardware one: a 100nF capacitor in parallel with the switch
 uint8_t getCount() {
 return count;
 uint8_t reset() {

 uint8_t count = 0;
 uint8_t pin;

5 Responses to Double Servo Tester with Display

  1. Martin Klimš says:

    Hello, I tryed to compile Your program, but in arduino ide it write: C:\Users\Martin\Documents\Arduino\libraries\ooPinChangeInt-master/ooPinChangeInt.h:94:10: fatal error: ../cbiface/cbiface.h: No such file or directory
    #include “../cbiface/cbiface.h”
    and in platformio this:
    VCSBaseException: VCS: Unknown repository type c:\Users\Martin\Plocha\ooPinChangeInt-master:
    File “C:\Users\Martin\.platformio\penv\lib\site-packages\platformio\builder\”, line 177:
    File “C:\Users\Martin\.platformio\packages\tool-scons\scons-local-4.1.0\SCons\Script\”, line 591:
    return _SConscript(self.fs, *files, **subst_kw)
    File “C:\Users\Martin\.platformio\packages\tool-scons\scons-local-4.1.0\SCons\Script\”, line 280:
    exec(compile(scriptdata, scriptname, ‘exec’), call_stack[-1].globals)
    File “C:\Users\Martin\.platformio\platforms\atmelavr\builder\”, line 162:
    target_elf = env.BuildProgram()
    File “C:\Users\Martin\.platformio\packages\tool-scons\scons-local-4.1.0\SCons\”, line 658:
    return self.method(*nargs, **kwargs)
    File “C:\Users\Martin\.platformio\penv\lib\site-packages\platformio\builder\tools\”, line 62:
    File “C:\Users\Martin\.platformio\packages\tool-scons\scons-local-4.1.0\SCons\”, line 658:
    return self.method(*nargs, **kwargs)
    File “C:\Users\Martin\.platformio\penv\lib\site-packages\platformio\builder\tools\”, line 140:
    project_lib_builder = env.ConfigureProjectLibBuilder()
    File “C:\Users\Martin\.platformio\packages\tool-scons\scons-local-4.1.0\SCons\”, line 658:
    return self.method(*nargs, **kwargs)
    File “C:\Users\Martin\.platformio\penv\lib\site-packages\platformio\builder\tools\”, line 1062:
    File “C:\Users\Martin\.platformio\penv\lib\site-packages\platformio\builder\tools\”, line 898:
    File “c:\users\martin\.platformio\penv\lib\site-packages\platformio\package\manager\”, line 49:
    spec, silent=silent, skip_dependencies=skip_dependencies, force=force
    File “c:\users\martin\.platformio\penv\lib\site-packages\platformio\package\manager\”, line 86:
    File “c:\users\martin\.platformio\penv\lib\site-packages\platformio\package\manager\”, line 97:
    pkg = self.install_from_url(spec.url, spec, silent=silent)
    File “c:\users\martin\.platformio\penv\lib\site-packages\platformio\package\manager\”, line 141:
    vcs =, url)
    File “c:\users\martin\.platformio\penv\lib\site-packages\platformio\package\”, line 60:
    raise VCSBaseException(“VCS: Unknown repository type %s” % remote_url)
    Where can be problem? Thank you. Klims

    • trandi says:

      Hi Martin, sorry all I can see is this bit but not sure why it happens :” No such file or directory
      #include “../cbiface/cbiface.h” “

  2. stet vasile says:

    It was more useful with a potentiometer. Encoder is not accurate and fast 🙂

    • trandi says:

      It is actually MUCH more accurate than the potentiometer, and you can see exactly the value generated on the display… That was the whole point of it 🙂
      As for the speed, one can change the increments by pressing on the button : from 1usec for super precision to 100usecs per increment for speed…

  3. Pingback: Arduino, when the Servo library is not good enough… | Robotics / Electronics / Physical Computing

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: