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

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)
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(WHITE);
  
  // 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.clearDisplay(); 
  display.fillRect(0, 0, map(posA, MIN_POS, MAX_POS, 0, 128), 15, WHITE); 
  display.setCursor(0, 16); 
  display.print(posToMicrosecs(posA)); 
  display.print(" :"); 
  display.print(incrementA); 
  display.fillRect(0, 32, map(posB, MIN_POS, MAX_POS, 0, 128), 15, WHITE); 
  display.setCursor(0, 48); 
  display.print(posToMicrosecs(posB)); 
  display.print(" :"); 
  display.print(incrementB); 
  display.display(); 

  // 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

 

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 !