Arduino Serial Servos - 20 Servos, 4 Pins, High Refresh Rates

In a recent post we looked at a very simple technique for controlling 10 servos with two Arduino pins using only one 30 cent chip.

I have since extended the library for two new functions -

1) Control upto 20 servos using only Four pins and two 30 cent chips
2) Variable refresh rate to maximise stability and holding torque

10 More Servos

The first addition - An additional 10 more servos has been achieved by duplicating the original libraries interaction with Timer1, OCR1A and the 4017 Chip onto OCR1B and a second 4017 chip. To put it another way each of the timer1 output compare registers is driving its own bank of ten servos through its own 4017 decade counter.

Refer to the original post here - http://rcarduino.blogspot.com/2012/08/arduino-serial-servos.html
For a description of the basic scheme.


Variable Refresh Rate
This second capability has always been a function of the library but is now more accessible.


The library blindly cycles through each bank of ten servos generating the required pulse. When the library gets to the last servo it points itself back to the first servo and starts again.

Remember, this is interrupt driven and is happening in the background, your main code is not effected.

A rough calculation suggests that driving 20 servos with this library will take a fraction more than half of one percent of Arduino processing power leaving you free to fill the other 99.5%

As the library continually cycles through the two banks of servos, a reduction in the number of servos will reduce the time taken for a cycle and therefore increase the servo refresh rate.

If we assume an average servo pulse of 1500ms, the following average refresh rates apply -
 

Servos In Each Bank	Average Servo Position	Time To Cycle Through Servos	Average Refresh Rate
1	1500	1500	666.6666667
2	1500	3000	333.3333333
3	1500	4500	222.2222222
4	1500	6000	166.6666667
5	1500	7500	133.3333333
6	1500	9000	111.1111111
7	1500	10500	95.23809524
8	1500	12000	83.33333333
9	1500	13500	74.07407407
10	1500	15000	66.66666667

I have highlighted the values for 3 and 4 servos as 250Hz seems to be a common request for the refresh rates of ESCs used in quadcopters.

What advantage do we get by increasing the servo refresh rate ? in the case of quadcopters faster refresh can improve stability.

In the case of robotics, higher refresh rates can increase holding torque and again improve stability.

To drive 8 servos with a high refresh rate a fast and simple option is to use both banks of servos and drive only four servos on each bank.

You may already be using a variety of refresh rates without knowing, some of my RC Car transmitters operate at 50Hz, others at 60Hz and one at 91Hz - I have been mixing and matching equipment for years and only discovered the different refresh rates by plugging my receivers into an Arduino.

The refresh rate is determined by the total pulse width for each bank of servos - four servos set to 1 microsecond will be cycled through 250 (1/(4*0.001) times a second, if the same four servos are set to 2 microseconds we slow down to 125 (1/(4*.002) times a second.

Shouldn't I add code to fix a constant refresh rate ? 

My thinking is that if we can refresh faster and take advantage of the increased stability and hold, why would we introduce code and complexity to prevent this ? If you would prefer a fixed refresh rate, keep a few more servos than you need and use them to adjust the total.

The Code - 
 
Here is the new library and test sketch for you to try. The test sketch sets up the servos with increasing pulse widths, an interrupt service routine is provided which allows you to test the pulse width on any of the servo outputs.

To add or remove the second bank of servos driven by OCR1B, refer to the following line in the RCArduinoSerialServos.h file

// COMMENT OR UNCOMMENT THIS LINE TO ENABLE THE SECOND BANK OF SERVOS
#define MORE_SERVOS_PLEASE 1 

 To change the number of servos driven by the library in order to increase the refresh rate refer to the following line in the RCArduinSerialServos.h file -

// THIS IS FOR ADVANCED USERS ONLY -
//
// Change the channel out count to adjust the frequency,
//
// BEFORE DOING THIS VERIFY THAT YOUR SERVOS AND ESCS ARE COMPATIBLE
// WITH HIGHER AND VARIABLE REFRESH RATES
//
// The library blindly pulses all ten servos one and after another
// If you change the RC_CHANNEL_OUT_COUNT to 4 servos, the library will pulse them more frequently than
// it can ten -
// 10 servos at 1500us = 15ms = 66Hz
// 4 Servos at 1500us = 6ms = 166Hz
// if you wanted to go even higher, run two servos on each 4017
// 2 Servos at 1500us = 3ms = 333Hz
#define RC_CHANNEL_OUT_COUNT 4

RCArduinoSerialServos.h

**************************************************************************************
// RCArduinoChannels by DuaneB is licensed under a Creative Commons Attribution-NonCommercial-ShareAlike 3.0 Unported License.
//
// http://rcarduino.blogspot.com
//
*****************************************************************************************************************************/


#include "Arduino.h"

// COMMENT OR UNCOMMENT THIS LINE TO ENABLE THE SECOND BANK OF SERVOS
#define MORE_SERVOS_PLEASE 1
// the second bank of servos uses pin 10 (clock) and 13 (reset) to drive an additional ten servos.
// the first bank uses pin 9 (clock) and 12 (reset)

// THIS IS FOR ADVANCED USERS ONLY -
//
// Change the channel out count to adjust the frequency,
//
// BEFORE DOING THIS VERIFY THAT YOUR SERVOS AND ESCS ARE COMPATIBLE
// WITH HIGHER AND VARIABLE REFRESH RATES
//
// The library blindly pulses all ten servos one and after another
// If you change the RC_CHANNEL_OUT_COUNT to 4 servos, the library will pulse them more frequently than
// it can ten -
// 10 servos at 1500us = 15ms = 66Hz
// 4 Servos at 1500us = 6ms = 166Hz
// if you wanted to go even higher, run two servos on each 4017
// 2 Servos at 1500us = 3ms = 333Hz
#define RC_CHANNEL_OUT_COUNT 4

#if defined (MORE_SERVOS_PLEASE)
#define RCARDUINO_MAX_SERVOS (RC_CHANNEL_OUT_COUNT*2)
#else
#define RCARDUINO_MAX_SERVOS (RC_CHANNEL_OUT_COUNT)
#endif

// Minimum and Maximum servo pulse widths, you could change these,
// Check the servo library and use that range if you prefer
#define RCARDUINO_SERIAL_SERVO_MIN 1000
#define RCARDUINO_SERIAL_SERVO_MAX 2000

//////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// CRCArduinoSerialServos
//
// A class for generating signals in combination with a 4017 Counter
//
// Output upto 10 Servo channels using just digital pins 9 and 12
// 9 generates the clock signal and must be connected to the clock pin of the 4017
// 12 generates the reset pulse and must be connected to the master reset pin of the 4017
//
// The class uses Timer1, as this prevents use with the servo library
// The class uses pins 9 and 12
// The class does not adjust the servo frame to account for variations in pulse width,
// on the basis that many RC transmitters and receivers designed specifically to operate with servos
// output signals between 50 and 100hz, this is the same range as the library
//
// Use of an additional pin would provide for error detection, however using pin 12 to pulse master reset
// at the end of every frame means that the system is essentially self correcting
//
// Note
// This is a simplified derivative of the Arduino Servo Library created by Michael Margolis
// The simplification has been possible by moving some of the flexibility provided by the Servo library
// from software to hardware.
//
////////////////////////////////////////////////////////////////////////////////////////////////////////////

class CRCArduinoSerialServos
{
public:
    CRCArduinoSerialServos();

    // configures timer1
    static void begin();

    // called by the timer interrupt service routine, see the cpp file for details.
    static void OCR1A_ISR();
   
#if defined(MORE_SERVOS_PLEASE)
    static void OCR1B_ISR();
#endif

    // called to set the pulse width for a specific channel, pulse widths are in microseconds - degrees are for wimps !
    static void writeMicroseconds(uint8_t nChannel,uint16_t nMicroseconds);

protected:
    // this sets the value of the timer1 output compare register to a point in the future
    // based on the required pulse with for the current servo
    static void setOutputTimerForPulseDurationA();
   
    // Records the current output channel values in timer ticks
    // Manually set by calling writeChannel, the function adjusts from
    // user supplied micro seconds to timer ticks
    volatile static uint16_t m_unChannelOutA[RC_CHANNEL_OUT_COUNT];
    // current output channel, used by the timer ISR to track which channel is being generated
    static uint8_t m_sCurrentOutputChannelA;
   
#if defined(MORE_SERVOS_PLEASE)
    // Optional channel B for servo number 10 to 19
    volatile static uint16_t m_unChannelOutB[RC_CHANNEL_OUT_COUNT];
    static uint8_t m_sCurrentOutputChannelB;
    static void setOutputTimerForPulseDurationB();
#endif

    // two helper functions to convert between timer values and microseconds
    static uint16_t ticksToMicroseconds(uint16_t unTicks);
    static uint16_t microsecondsToTicks(uint16_t unMicroseconds);
};

RCArduinoSerialServos.cpp

#include "RCArduinoSerialServos.h"

/*----------------------------------------------------------------------------------------

This is essentially a derivative of the Arduino Servo Library created by Michael Margolis

As the technique is very similar to the Servo class, it can be useful to study in order
to understand the servo class.

What does the library do ? It uses a very inexpensive and common 4017 Counter IC
To generate pulses to independently drive up to 10 servos from two Arduino Pins

As previously mentioned, the library is based on the techniques used in the Arduino Servo
library created by Michael Margolis. This means that the library uses Timer1 and Timer1 output
compare register A.

OCR1A is linked to digital pin 9 and so we use digital pin 9 to generate the clock signal
for the 4017 counter.

Pin 12 is used as the reset pin.

*/

// Timer1 Output Compare A interrupt service routine
// call out class member function OCR1A_ISR so that we can
// access out member variables
ISR(TIMER1_COMPA_vect)
{
    CRCArduinoSerialServos::OCR1A_ISR();
}

void CRCArduinoSerialServos::OCR1A_ISR()
{
    // If the channel number is >= 10, we need to reset the counter
    // and start again from zero.
    // to do this we pulse the reset pin of the counter
    // this sets output 0 of the counter high, effectivley
    // starting the first pulse of our first channel
  if(m_sCurrentOutputChannelA >= RC_CHANNEL_OUT_COUNT)
  {
    // reset our current servo/output channel to 0
    m_sCurrentOutputChannelA = 0;

    // pulse reset on the counter - we set it high here
    PORTB|=16;

    // set the duration of the output pulse
    CRCArduinoSerialServos::setOutputTimerForPulseDurationA();

    // finish the reset pulse - we set it low here
    PORTB^=16;
 }
 else
 {
  // pulse the clock pin high
    PORTB|=2;

    // set the duration of the output pulse
    CRCArduinoSerialServos::setOutputTimerForPulseDurationA();

    // finish the clock pulse - set it back to low
    PORTB^=2;
  }
    // done with this channel so move on.
    m_sCurrentOutputChannelA++;
}
// After we set an output pin high, we need to set the timer to comeback for the end of the pulse
void CRCArduinoSerialServos::setOutputTimerForPulseDurationA()
{
  OCR1A = TCNT1 + m_unChannelOutA[m_sCurrentOutputChannelA];
}

#if defined(MORE_SERVOS_PLEASE)
// Timer1 Output Compare B interrupt service routine
// call out class member function OCR1B_ISR so that we can
// access out member variables
ISR(TIMER1_COMPB_vect)
{
    CRCArduinoSerialServos::OCR1B_ISR();
}

void CRCArduinoSerialServos::OCR1B_ISR()
{
    // If the channel number is >= 10, we need to reset the counter
    // and start again from zero.
    // to do this we pulse the reset pin of the counter
    // this sets output 0 of the counter high, effectivley
    // starting the first pulse of our first channel
  if(m_sCurrentOutputChannelB >= RC_CHANNEL_OUT_COUNT)
  {
    // reset our current servo/output channel to 10 -
    // Note that 10 is the first servo on output compare B
    m_sCurrentOutputChannelB = 0;

    // pulse reset on the counter - we set it high here
    PORTB|=32;

    // set the duration of the output pulse
    CRCArduinoSerialServos::setOutputTimerForPulseDurationB();

    // finish the reset pulse - we set it low here
    PORTB^=32;
 }
 else
 {
  // pulse the clock pin high
    PORTB|=4;

    // set the duration of the output pulse
    CRCArduinoSerialServos::setOutputTimerForPulseDurationB();

    // finish the clock pulse - set it back to low
    PORTB^=4;
  }
    // done with this channel so move on.
    m_sCurrentOutputChannelB++;
}

// After we set an output pin high, we need to set the timer to comeback for the end of the pulse
void CRCArduinoSerialServos::setOutputTimerForPulseDurationB()
{
  OCR1B = TCNT1 + m_unChannelOutB[m_sCurrentOutputChannelB];
}
#endif

// updates a channel to a new value, the class will continue to pulse the channel
// with this value for the lifetime of the sketch or until writeChannel is called
// again to update the value
void CRCArduinoSerialServos::writeMicroseconds(uint8_t nChannel,uint16_t unMicroseconds)
{
    // dont allow a write to a non existent channel
    if(nChannel > RCARDUINO_MAX_SERVOS)
        return;

  // constraint the value just in case
  unMicroseconds = constrain(unMicroseconds,RCARDUINO_SERIAL_SERVO_MIN,RCARDUINO_SERIAL_SERVO_MAX);

#if defined(MORE_SERVOS_PLEASE)
  if(nChannel >= RC_CHANNEL_OUT_COUNT)
  {
    // disable interrupts while we update the multi byte value output value
    uint8_t sreg = SREG;
    cli();
     
    m_unChannelOutB[nChannel-RC_CHANNEL_OUT_COUNT] = microsecondsToTicks(unMicroseconds);

    // enable interrupts
    SREG = sreg;
    return;
  }
#endif
 
  // disable interrupts while we update the multi byte value output value
  uint8_t sreg = SREG;
  cli();
 
  m_unChannelOutA[nChannel] = microsecondsToTicks(unMicroseconds);

  // enable interrupts
  SREG = sreg;
}

uint16_t CRCArduinoSerialServos::ticksToMicroseconds(uint16_t unTicks)
{
    return unTicks / 2;
}

uint16_t CRCArduinoSerialServos::microsecondsToTicks(uint16_t unMicroseconds)
{
 return unMicroseconds * 2;
}

void CRCArduinoSerialServos::begin()
{
    // set the pins we will use for channel A (OCR1A) as outputs
    pinMode(9,OUTPUT); // clock uses OCR1A and should not be changed
    pinMode(12,OUTPUT); // reset, if you really needed to, you could change this, but remember to change it in the ISR as well

    // pulse reset
    digitalWrite(12,HIGH);
    digitalWrite(12,LOW);

#if defined (MORE_SERVOS_PLEASE)

    // set the pins we will use for channel B (OCR1B) as outputs
    pinMode(10,OUTPUT); // clock uses OCR1B and should not be changed
    pinMode(13,OUTPUT); // reset, if you really needed to, you could change this, but remember to change it in the ISR as well

    // pulse reset
    digitalWrite(12,HIGH);
    digitalWrite(12,LOW);

#endif

    TCNT1 = 0;              // clear the timer count  

    // Initilialise Timer1
    TCCR1A = 0;             // normal counting mode
    TCCR1B = 2;     // set prescaler of 64 = 1 tick = 4us

    // ENABLE TIMER1 OCR1A INTERRUPT to enabled the first bank (A) of ten servos
    TIFR1 |= _BV(OCF1A);     // clear any pending interrupts;
    TIMSK1 |=  _BV(OCIE1A) ; // enable the output compare interrupt 

#if defined(MORE_SERVOS_PLEASE)

    // ENABLE TIMER1 OCR1B INTERRUPT to enable the second bank (B) of 10 servos
    TIFR1 |= _BV(OCF1B);     // clear any pending interrupts;
    TIMSK1 |=  _BV(OCIE1B) ; // enable the output compare interrupt 

#endif

    OCR1A = TCNT1 + 4000; // Start in two milli seconds
}

// See the .h file
volatile uint16_t CRCArduinoSerialServos::m_unChannelOutA[RC_CHANNEL_OUT_COUNT];
uint8_t CRCArduinoSerialServos::m_sCurrentOutputChannelA;

#if defined(MORE_SERVOS_PLEASE)

volatile uint16_t CRCArduinoSerialServos::m_unChannelOutB[RC_CHANNEL_OUT_COUNT];
uint8_t CRCArduinoSerialServos::m_sCurrentOutputChannelB;

#endif

 RCArduinoSerialServos sketch

// RCArduinoSerialServos by DuaneB is licensed under a Creative Commons Attribution-NonCommercial 3.0 Unported License.
// Based on a work at rcarduino.blogspot.com.
#include <RCArduinoSerialServos.h>

volatile uint32_t ulRiseTime;
volatile uint32_t ulPulseWidth;

void setup()
{
  Serial.begin(9600);
  Serial.println("RCArduinoSerialServos");
 
  // set the channels
  for(uint16_t nChannel = 0;nChannel < RCARDUINO_MAX_SERVOS;nChannel++)
  {
    CRCArduinoSerialServos::writeMicroseconds(nChannel,1000+(nChannel*50));
  }

  CRCArduinoSerialServos::begin();
 
  attachInterrupt(0,calcPulse,CHANGE);
}

void loop()
{
  delay(10);

  if(ulPulseWidth != 0)
  {
    // disable interrupts so that our pulse value does not get overwritten while we try and read it
    uint8_t sReg = SREG;
    cli();
   
    // take a local copy of the pulse witdth so that we can reenable interrupts as soon as possible
    uint32_t ulLocalPulseWidth = ulPulseWidth;
   
    // clear the pulse width so that we will only pick up new values written by calcPulse rather
    // than keep printing old values.
    ulPulseWidth = 0;
   
    // turn interrupts back on
    SREG = sReg;
   
    // print the pulse width
    Serial.println(ulLocalPulseWidth);
  }
 
}

// Read pulse width applied to digital pin 2 (interrupt 0)
void calcPulse()
{
  if(digitalRead(2))
  {
    ulRiseTime = micros();
  }
  else
  {
    ulPulseWidth = micros()- ulRiseTime;
  }
}