Sunday, October 28, 2012

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;
  }
}










14 comments:

  1. Hello, I got a problem while verifying the code. It said" error: 'RCArduinoSerialServos' has not been declared ". Is there any suggestions to debug? thanks.

    ReplyDelete
  2. You need to put the first two files in your libraries folder, the restart rhe Arduino IDE. For more detailed instructions, search for 'Arduino installing a library'

    Duane B

    ReplyDelete
  3. I've tried it, but I still can run the program. I'm not sure whether it's the problem of my computer or paste and copy. Is there any way you and offer a zip file for the code, thanks.

    ReplyDelete
  4. Sure, send me your email address in a PM on the Arduino forum. My user name is Duane B

    ReplyDelete
  5. Hi could I have all the code as a .zip file to?

    On the arduino forum I am DataShark

    ReplyDelete
    Replies
    1. Hi,
      Can you send me a reminder of the code your looking for and your email address on the Arduino forum - I get lots of requests so it helps me if you include the project your after as well as your email address.

      Thanks

      Duane B

      Delete
  6. Hi, Just a check for RCArduinoSerialServos.cpp lines 190-191 ( MORE_SERVOS_PLEASE

    digitalWrite(12,HIGH); ----- should this be 13 ?
    digitalWrite(12,LOW); ----- "

    ReplyDelete
  7. Hi, can I control each servo individually?

    I compile your code with arduino IDE 1.0.5-r2, but there are errors anywhere, could you send me .zip file
    I haven't join arduino forum, but can I?

    thanks, please pardon my english.

    ReplyDelete
    Replies
    1. I have added a link to download a zip file on the download page. Click the link for the project you want and then look for a file menu in the top left of the screen, from this menu select 'download' to download a zip file.

      http://rcarduino.blogspot.ae/2014/02/rcarduino-downloads.html

      Duane B

      Delete
  8. Hi I have a couple of questions....

    (1) You mentioned the refresh rates when using 1 - 10 servos respectively. What I'm trying to figure out is, for comparison, what is the refresh rate when one uses the Arduino library?

    (2) What exactly does the sketch do? I presume it is the method writeMicroseconds() that moves the servos, but it isn't called at all in the loop function so..?

    ReplyDelete
  9. (1) 50Hz,
    (2) This library and the standard servo library refresh the servos in the background, all that writeMircoseconds does is update the value that will be sent on the next refresh, the refresh will happen whether writeMicroseconds is called or not. If you look at the set up function, each servo is set to a different position, this way you can easily check your circuit by connecting a single servo to each pin in sequence, you should see it update its position as its connected to each pin. Try moving the servo with your fingers and you will feel it resist, this is because it is constantly being told what position to hold by the library without us having to add code in loop.
    Once your happy that your circuit works, you can start adding commands within loop to get the servos to do something more interesting.

    Duane B

    ReplyDelete
  10. Hi thanks for the quick reply :)

    (1) That's what my googling turned up too, but I couldn't believe it, since that means that even when driving 20 servos with 2 x 4017s, you'd still get a better refresh rate than that of the Arduino servo library?!

    (2) Ok so the code in loop() is just for refreshing. I'd like to ask though, is there a way to wrap this code up so that the code for controlling servos doesn't mix with the general loop() code. I was thinking adding a method to the class, with your permission off course, so all you'd have to do in the loop() function is to call CRCArduinoSerialServos::refresh() or something like that. Or even better, you wouldn't have to do anything else than call CRCArduinoSerialServos::writeMicroseconds() whenever you want to change the position of a servo. Sorry if this question seem noobish...

    (3) Any tips as to additional components that might improve the setup? For instance I noticed that a servo shield from RenBotics, which is based on two 4017s, uses a 10nF capacitor for each 4017, though I'm not sure what for.

    http://www.seeedstudio.com/depot/images/product/servoshield.pdf

    Ah and thanks a lot for sharing this. If I get this working it would be truly awesome :)

    ReplyDelete
  11. 'Or even better, you wouldn't have to do anything else than call CRCArduinoSerialServos::writeMicroseconds() whenever you want to change the position of a servo'

    This is what the standard servo library and the rcarduino library already do

    Duane

    ReplyDelete
    Replies
    1. " uses a 10nF capacitor for each 4017"

      There are decoupling capacitors, the reduce electrical noise in your circuits making them more reliable. More here - http://www.thebox.myzen.co.uk/Tutorial/De-coupling.html

      Duane

      Delete