Arduino UNO Controlling 20 Servos With 15 bit Precision And Low Jitter

I made a program (or sketch) for Arduino UNO controlling 20 servos:

I have used Arduino with hobby servos often, but I was getting annoyed of the jitter of the standard servo-library. So I took my newly acquired Rigol DS2072 and tried to figure out what was causing the servos to rattle and jitter.

It turned out most jitter is caused by timer 0 that is used in delay() function and so on. If you turn off timer 0 interrupts with,

TIMSK0 = 0;

then most of the  jitter is gone. But I discovered that the 50 Hz PWM signal needed for the servo was generated in such a way that the 50 Hz frequency of  was not stable. This can also cause jitter on all channels if only one servo is moving fast. I guess this will only affect some servos, since the pulse width is always correct.

So I ended up writing all the servo PWM generating code from scratch.

I used timer 2 for 50 Hz timing to get correct frequency for all servos all the time. I then used timer 1 for timing the pulse width. The original servo library uses prescaler 8 for timer 1. This will give a resolution of 1/2 us steps. I decided to go for prescaler 1. This will give a resolution of 1/16 us. A servo that accept a 500-2500 μs range, will get this in 32000 steps. Thus 15 bit resolution. This will not always be valid since there will still be some jittering. By jittering I mean that the pulse width is not stable. I got it Down to 200-400 ns.

By using both compare A and B for timer 1, I was able to control 2*10 servos. Every servo is starting its pulse 2000 μs after the previous one. This means the pulse can extend into the next servo timing. To do this I used compare A and B for timer 2 as well.

Timing diagram:

Arduino 20 servos Timing diagram_

1A and 1B = Compare A and B for timer 1
2A and 2B = Compare A and B for timer 2
Timer 1 and timer 2 both restarts at 2B
2A -> 2A = 2000 μs
2A -> 2B = 500 μs
2B -> 1A = Pulse width -500 μs for servos 0-9
2B -> 1B = Pulse width – 500 μs for servos 10-19
Period total = 10 * 2000 μs = 20 ms => 50 Hz

Download the code: http://www.lamja.com/blogfiles/UNO_20_Servos_Controller.ino

New version 2016.04.24 : http://www.lamja.com/blogfiles/UNO_20_Servos_Controller_v1_2_1.ino

The code: (use the download link abobe to test the code)


//////////////////////////////////////////////////////////////////////////////////////////////////////
// UNO_20_Servos_Controller.ino - High definition (15 bit), low jitter, 20 servo software for Atmega328P and Arduino UNO. Version 1.
// Jitter is typical 200-400 ns. 32000 steps resolution for 0-180 degrees. In 18 servos mode it can receive serial servo-move commands.
//
// Copyright (c) 2013 Arvid Mortensen.  All right reserved. 
// http://www.lamja.com
// 
// This software is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
// 
// This software is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.
// 
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
//
//////////////////////////////////////////////////////////////////////////////////////////////////////
//                              !!!!!!!!!!!!!!!!!!!
// This software will only work with ATMEGA328P (Arduino UNO and compatible. Or that is what I have tested it with anyways....  !!!!
//                              !!!!!!!!!!!!!!!!!!!
// How it works:
// 18 or 20 pins are used for the servos. These pins are all pre configured.
// All 18/20 servos are always active and updated. To change the servo position, change the
// values in the ServoPW[] array, use the serial commands (int 18 servos mode only) or use 
// ServoMove() function. The range of the ServoPW is 8320 to 39680. 8320=520us. 39680=2480us.
//
// Some formulas:
// micro second = ServoPW value / 16
// angle = (ServoPW value - 8000) / 177.77  (or there about)
// ServoPW value = angle * 177.77 + 8000
// ServoPW value = micro second * 16
//
// Channels are locked to these pins:
// Ch0=Pin2, Ch1=Pin3, Ch2=Pin4, Ch3=Pin5, Ch4=Pin6, Ch5=Pin7, Ch6=Pin8, Ch7=Pin9, Ch8=Pin10, Ch9=Pin11
// Ch10=Pin12, Ch11=Pin13, Ch12=PinA0, Ch13=PinA1, Ch14=PinA2, Ch15=PinA3, Ch16=PinA4, Ch17=PinA5, Ch18=Pin0, Ch19=Pin1
//
// Serial commands:
// # = Servo channel
// P = Pulse width in us
// p = Pulse width in 1/16 us
// S = Speed in us per second
// s = Speed in 1/16 us per second
// T = Time in ms
// PO = Pulse offset in us. -2500 to 2500 in us. Used to trim servo position.
// Po = Pulse offset in 1/16us -40000 to 40000 in 1/16 us
// I = Invert servo movements.
// N = Non-invert servo movements.
// Q = Query movement. Return "." if no servo moves and "+" if there are any servos moving.
// QP = Query servo pulse width. Return 20 bytes where each is from 50 to 250 in 10us resolution. 
//      So you need to multiply byte by 10 to get pulse width in us. First byte is servo 0 and last byte is servo 20.
//  = Carrage return. ASCII value 13. Used to end command.
//
// Examples:
// #0 P1500 T1000                        - Move Servo 0 to 1500us in 1 second.
// #0 p24000 T1000                       - Move Servo 0 to 1500us in 1 second.
// #0 p40000 s1600                       - Move Servo 0 to 2500us in 100us/s speed
// #0 p40000 S100                        - Move Servo 0 to 2500us in 100us/s speed
// #0 P1000 #1 P2000 T2000               - Move Servo 0 and servo at the samt time from current pos to 1000us and 2000us in 2 second.
// #0 P2400 S100                         - Move servo 0 to 2400us at speed 100us/s
// #0 P1000 #1 P1200 S500 #2 P1400 T1000 - Move servo 0, 1 and 2 at the same time, but the one that takes longes S500 or T1000 will be used.
// #0 PO100 #1 PO-100                    - Will set 100 us offset to servo 0 and -100 us ofset to servo 1
// #0 Po1600 #1 Po-1600                  - Will set 100 us offset to servo 0 and -100 us ofset to servo 1
// #0 I                                  - Will set servo 0 to move inverted from standard
// #0 N                                  - Will set servo 0 back to move non-inverted
// Q                                     - Will return "." if no servo moves and "+" if there are any servos moving
// QP                                    - Will retur 18 bytes (each 20ms apart) for position of servos 0 to 17
//
// 18 or 20 channels mode:
// #define HDServoMode 18           - This will set 18 channels mode so you can use serial in and out. Serial command interpreter is activated.
// #define HDServoMode 20           - This will set 20 channels mode, and you can not use serial. 
//                                    A demo will run in the loop() routine . Serial command interpreter is not active.
//                                    use ServoMove(int Channel, long PulseHD, long SpeedHD, long Time) to control servos.
//                                    one of SpeedHD or Time can be set to 0 to just use the other one for speed. If both are used,
//                                    the one that takes the longest time will be used. You can also change the values in the 
//                                    ServoPW[] array directly, but take care not to go under/over 8320/39680.
//
// #deefine SerialInterfaceSpeed 115200      - Serial interface Speed
//////////////////////////////////////////////////////////////////////////////////////////////////////

#include <avr/interrupt.h>
#define HDServoMode 20
#define SerialInterfaceSpeed 115200    // Serial interface Speed

static unsigned int iCount;
static volatile uint8_t *OutPortTable[20] = {&PORTD,&PORTD,&PORTD,&PORTD,&PORTD,&PORTD,&PORTB,&PORTB,&PORTB,&PORTB,&PORTB,&PORTB,&PORTC,&PORTC,&PORTC,&PORTC,&PORTC,&PORTC,&PORTD,&PORTD};
static uint8_t OutBitTable[20] = {4,8,16,32,64,128,1,2,4,8,16,32,1,2,4,8,16,32,1,2};
static unsigned int ServoPW[20] = {24000,24000,24000,24000,24000,24000,24000,24000,24000,24000,24000,24000,24000,24000,24000,24000,24000,24000,24000,24000};
static byte ServoInvert[20] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
static byte Timer2Toggle;
static volatile uint8_t *OutPort1A = &PORTD;
static volatile uint8_t *OutPort1B = &PORTB;
static uint8_t OutBit1A = 4;
static uint8_t OutBit1B = 16;
static volatile uint8_t *OutPortNext1A = &PORTD;
static volatile uint8_t *OutPortNext1B = &PORTB;
static uint8_t OutBitNext1A = 4;
static uint8_t OutBitNext1B = 16;

static long ServoStepsHD[20] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
static long ServoLastPos[20] = {24000,24000,24000,24000,24000,24000,24000,24000,24000,24000,24000,24000,24000,24000,24000,24000,24000,24000,24000,24000};
static long StepsToGo[20] = {-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1};
static int ChannelCount;

static long ServoGroupStepsToGo = 0;
static long ServoGroupServoLastPos[20];
static long ServoGroupStepsHD[20];
static int ServoGroupChannel[20];
static int ServoGroupNbOfChannels = 0;

static char SerialIn;
static int SerialCommand = 0; //0= none, 1 = '#' and so on...
static long SerialNumbers[10];
static int SerialNumbersLength = 0;
static boolean FirstSerialChannelAfterCR = 1;

static int SerialChannel = 0;
static long SerialPulseHD = 24000;
static long SerialPulseOffsetHD[20] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
static long SerialPulseOffsetTempHD = 0;
static long SerialSpeedHD = 0;
static long SerialTime = 0;
static long SerialNegative = 1;
static boolean SerialNeedToMove = 0;
static char SerialCharToSend[50] = ".detratS slennahC 81 ovreSDH";
static int SerialNbOfCharToSend = 0;  //0= none, 1 = [0], 2 = [1] and so on...

void ServoMove(int Channel, long PulseHD, long SpeedHD, long Time)
{
// Use ServoMove(int Channel, long PulseHD, long SpeedHD, long Time) to control servos.
// One of th SpeedHD or Time can be set to 0 to only  use the other one for speed. If both are used,
// the one that takes the longest time, will be used
  ServoGroupMove(Channel, CheckRange(PulseHD), SpeedHD, Time);
  ServoGroupMoveActivate();
}

void setup()
{
  ServoSetup();                       //Initiate timers and misc.

  #if HDServoMode == 18
    TIMSK0 = 0;                       // Disable timer 0. This can reduse jitter some more. But it's used for delay() funtions.
  #endif                              // This will disable delay()!
}

void loop()
{
  #if HDServoMode == 18               //Serial command interpreter is acive. 18-servos mode.
    CheckSerial();
  #elif HDServoMode == 20             //Demo dance is active. 20-servos mode.
    DemoDance1();
    DemoDance2();
    DemoDance3();
    DemoDance1();
    DemoDance4();
    DemoDance5();
    DemoDance6();
  #endif
}

void DemoDance6()
{
  long MinPulse = 8320;
  long MaxPulse = 35200;  
  int i = 0;
  int i2 = 0;
  for(i2 = 0; i2 < 5 ; i2++)           // All servos go to random position. Then go back to middle in same time.
  {
    for(i = 0; i < 20 ; i++)
    {
      long a = random(MaxPulse-MinPulse)+MinPulse;        
      ServoMove(i, a, 0, 100);
    }
    delay(1500);
    for(i = 0; i < 20 ; i++)
    {
      ServoMove(i, 24000, 0, 3000);
    }
    delay(4000);
  }
}

void DemoDance5()
{
  long MinPulse = 8320;
  long MaxPulse = 35200;  
  int i = 0;
  int i2 = 0;
  i = 0;                                         // All servos moving randomly
  for(i2 = 0; i2 < 1000 ; i2++)   {     long a = random(MaxPulse-MinPulse)+MinPulse;             int DemoTime = random(1000)+300;     ServoMove(i, a, 0, DemoTime);     delay(DemoTime/25);     i++;     if(i > 19) i = 0;
  }
}

void DemoDance4()
{
  long MinPulse = 8320;
  long MaxPulse = 35200;  
  int i = 0;
  int i2 = 0;
  float i4 = 0;
  float pi = 3.14159265359;

  for(i2 = 10; i2 > 0 ; i2--)                // Faster bounce
  {
    for(i4 = 0 ; i4 < pi ; i4 += 0.03)
    {
      for(i = 0; i < 20 ; i++) ServoPW[i] = sin(i4)*((MaxPulse-MinPulse)/2) + (MinPulse);       delay(10);     }   } } void DemoDance3() {   long MinPulse = 8320;   long MaxPulse = 35200;     int i = 0;   int i2 = 0;   float i4 = 0;   float pi = 3.14159265359;   for(i2 = 10; i2 > 0 ; i2--)                // Slow bounce
  {
    for(i4 = 0 ; i4 < pi ; i4 += 0.005+(0.001*i2))
    {
      for(i = 0; i < 20 ; i++) ServoPW[i] = sin(i4)*(MaxPulse-MinPulse) + MinPulse;
      delay(10);
    }
  }
}

void DemoDance2()
{
  long MinPulse = 8320;
  long MaxPulse = 35200;
  int i = 0;
  int i2 = 0;

  for(i2 = 0; i2 < 10 ; i2++)              //Sweep servos, every other way, from fast to slow
  {
    for(i = 0; i < 20 ; i+=2) 
    {
      ServoMove(i, MinPulse, 0, 1000+i2*100);
      ServoMove(i+1, MaxPulse, 0, 1000+i2*100);
    }
    delay(1000+i2*100);  
    for(i = 0; i < 20 ; i+=2) 
    {
      ServoMove(i, MaxPulse, 0, 1000+i2*100);
      ServoMove(i+1, MinPulse, 0, 1000+i2*100);
    }
    delay(1000+i2*100);
  }
}

void DemoDance1()
{
  long MinPulse = 8320;
  long MaxPulse = 35200;
  int i = 0;
  int i2 = 0;
  int i3 = 16;

  for(i = 0; i < 100 ; i++)                // Wave move circular   {     ServoMove(i2, MinPulse, 0, 800);     ServoMove(i3, MaxPulse, 0, 800);     delay(200);     i2++;     i3++;     if(i2>19) i2=0;
    if(i3>19) i3=0;
  }
}

long CheckRange(long PulseHDValue)
{
  if(PulseHDValue > 39680) return 39680;
  else if(PulseHDValue < 8320) return 8320;   else return PulseHDValue; } long CheckChannelRange(long CheckChannel) {   if(CheckChannel >= HDServoMode) return (HDServoMode-1);
  else if(CheckChannel < 0) return 0;   else return CheckChannel; } void CheckSerial()     //Serial command interpreter. {   int i = 0;   if(Serial.available() > 0)
  {
    SerialIn = Serial.read();
    if(SerialIn == '#') 
    {
      SerialCommand = 1;
      SerialNeedToMove = 1;
      if(!FirstSerialChannelAfterCR) ServoGroupMove(SerialChannel, CheckRange(SerialPulseHD), SerialSpeedHD, SerialTime);
      FirstSerialChannelAfterCR = 0;
    }
    if(SerialIn == 'p') SerialCommand = 2;
    if(SerialIn == 's') SerialCommand = 3;
    if(SerialIn == 'T') SerialCommand = 4;
    if(SerialIn == 'P') 
    {
      if(SerialCommand == 9) SerialCommand = 10;   // 'QP'
      else SerialCommand = 5;                      // 'P'
    }
    if(SerialIn == 'S') SerialCommand = 6;
    if(SerialIn == 'o') {SerialCommand = 7; SerialNeedToMove = 1;}
    if(SerialIn == 'O') {SerialCommand = 8; SerialNeedToMove = 1;}
    if(SerialIn == 'Q') SerialCommand = 9; 
    if(SerialIn == 'I') SerialCommand = 11; 
    if(SerialIn == 'N') SerialCommand = 12; 
    if(SerialIn == ' ' || SerialIn == 13)
    {
      if(SerialCommand == 1) {SerialChannel = CheckChannelRange(ConvertSerialNumbers()); SerialCommand = 0;}
      if(SerialCommand == 2) {SerialPulseHD = ConvertSerialNumbers() + SerialPulseOffsetHD[SerialChannel]; SerialCommand = 0;}
      if(SerialCommand == 3) {SerialSpeedHD = ConvertSerialNumbers(); SerialCommand = 0;}
      if(SerialCommand == 4) {SerialTime = ConvertSerialNumbers(); SerialCommand = 0;}
      if(SerialCommand == 5) {SerialPulseHD = ConvertSerialNumbers()*16 + SerialPulseOffsetHD[SerialChannel]; SerialCommand = 0;}
      if(SerialCommand == 6) {SerialSpeedHD = ConvertSerialNumbers()*16; SerialCommand = 0;}
      if(SerialCommand == 11) {ServoInvert[SerialChannel] = 1; SerialCommand = 0;}
      if(SerialCommand == 12) {ServoInvert[SerialChannel] = 0; SerialCommand = 0;}
      if(SerialCommand == 7) 
      {
        SerialPulseOffsetTempHD = ConvertSerialNumbers();
        SerialPulseHD = ServoPW[SerialChannel] - SerialPulseOffsetHD[SerialChannel] + SerialPulseOffsetTempHD;
        SerialTime = 10;
        SerialPulseOffsetHD[SerialChannel] = SerialPulseOffsetTempHD;
        SerialCommand = 0;
      }
      if(SerialCommand == 8) 
      {
        SerialPulseOffsetTempHD = ConvertSerialNumbers()*16;
        SerialPulseHD = ServoPW[SerialChannel] - SerialPulseOffsetHD[SerialChannel] + SerialPulseOffsetTempHD;
        SerialTime = 10;
        SerialPulseOffsetHD[SerialChannel] = SerialPulseOffsetTempHD;
        SerialCommand = 0;
      }
      if(SerialIn == 13) 
      {
        if(SerialNeedToMove)
        {
          ServoGroupMove(SerialChannel, CheckRange(SerialPulseHD), SerialSpeedHD, SerialTime);
          ServoGroupMoveActivate();
          FirstSerialChannelAfterCR = 1;
          SerialCommand = 0;
          SerialSpeedHD = 0;
          SerialTime = 0;
          SerialNeedToMove = 0;
        }
        if(SerialCommand == 9)
        {
          SerialCharToSend[0] = '.';
          for(i = 0; i < 20 ; i++)           {             if(StepsToGo[i] > 0) SerialCharToSend[0] = '+';
          }
          SerialNbOfCharToSend = 1;
          SerialCommand = 0;
        }
        if(SerialCommand == 10)
        {
          for(i = 0; i < 18 ; i++)           {             SerialCharToSend[17 - i] = (ServoPW[i] - SerialPulseOffsetHD[i])/160;           }           SerialNbOfCharToSend = 18;           SerialCommand = 0;         }       }     }     if((SerialIn >= '0') && (SerialIn <= '9')) {SerialNumbers[SerialNumbersLength] = SerialIn - '0'; SerialNumbersLength++;}     if(SerialIn == '-') SerialNegative = -1;   } } long ConvertSerialNumbers()         //Converts numbers gotten from serial line to long. {   int i = 0;      long ReturnValue = 0;   long Multiplier = 1;   if(SerialNumbersLength > 0)
  {
    for(i = SerialNumbersLength-1 ; i >= 0 ; i--)
    {
      ReturnValue += SerialNumbers[i]*Multiplier;
      Multiplier *=10;
    }
    ReturnValue *= SerialNegative;
    SerialNumbersLength = 0;
    SerialNegative = 1;
    return ReturnValue;
  }
  else return 0;
}

void ServoGroupMove(int Channel, long PulseHD, long SpeedHD, long Time)    //ServoMove used by serial command interpreter
{
  long StepsToGoSpeed=0;
  long StepsToGoTime=0;

  ServoGroupChannel[ServoGroupNbOfChannels] = Channel;
  if(SpeedHD < 1) SpeedHD = 3200000;   StepsToGoSpeed = abs((PulseHD - ServoPW[Channel]) / (SpeedHD / 50));   StepsToGoTime = Time / 20;   if(StepsToGoSpeed > ServoGroupStepsToGo) ServoGroupStepsToGo = StepsToGoSpeed;
  if(StepsToGoTime > ServoGroupStepsToGo) ServoGroupStepsToGo = StepsToGoTime;
  ServoGroupChannel[ServoGroupNbOfChannels] = Channel;
  ServoGroupServoLastPos[ServoGroupNbOfChannels] = PulseHD;
  ServoGroupNbOfChannels++;
}

void ServoGroupMoveActivate()                       //ServoMove used by serial command interpreter
{
  int ServoCount = 0;

  for(ServoCount = 0 ; ServoCount < ServoGroupNbOfChannels ; ServoCount++)
  {
    ServoStepsHD[ServoGroupChannel[ServoCount]] = (ServoGroupServoLastPos[ServoCount] - ServoPW[ServoGroupChannel[ServoCount]]) / ServoGroupStepsToGo;
    StepsToGo[ServoGroupChannel[ServoCount]] =ServoGroupStepsToGo;
    ServoLastPos[ServoGroupChannel[ServoCount]] = ServoGroupServoLastPos[ServoCount];
  }
  ServoGroupNbOfChannels = 0;
  ServoGroupStepsToGo = 0;
}

void RealTime50Hz() //Move servos every 20ms to the desired position.
{
  if(SerialNbOfCharToSend) {SerialNbOfCharToSend--; Serial.print(SerialCharToSend[SerialNbOfCharToSend]);}
  for(ChannelCount = 0; ChannelCount < 20; ChannelCount++)   {     if(StepsToGo[ChannelCount] > 0)
    {
      ServoPW[ChannelCount] += ServoStepsHD[ChannelCount];
      StepsToGo[ChannelCount] --;
    }
    else if(StepsToGo[ChannelCount] == 0)
    {
      ServoPW[ChannelCount] = ServoLastPos[ChannelCount];
      StepsToGo[ChannelCount] --;
    }
  }
}

ISR(TIMER1_COMPA_vect) // Interrupt routine for timer 1 compare A. Used for timing each pulse width for the servo PWM.
{ 
  *OutPort1A &= ~OutBit1A;                //Pulse A finished. Set to low
}

ISR(TIMER1_COMPB_vect) // Interrupt routine for timer 1 compare A. Used for timing each pulse width for the servo PWM.
{ 
  *OutPort1B &= ~OutBit1B;                //Pulse B finished. Set to low
}

ISR(TIMER2_COMPA_vect) // Interrupt routine for timer 2 compare A. Used for timing 50Hz for each servo.
{ 
  *OutPortNext1A |= OutBitNext1A;         // Start new pulse on next servo. Write pin HIGH
  *OutPortNext1B |= OutBitNext1B;         // Start new pulse on next servo. Write pin HIGH
}

ISR(TIMER2_COMPB_vect) // Interrupt routine for timer 2 compare A. Used for timing 50Hz for each servo.
{ 
  TIFR1 = 255;                                       // Clear  pending interrupts
  TCNT1 = 0;                                         // Restart counter for timer1
  TCNT2 = 0;                                         // Restart counter for timer2
  sei();
  *OutPort1A &= ~OutBit1A;                           // Set pulse low to if not done already
  *OutPort1B &= ~OutBit1B;                           // Set pulse low to if not done already
  OutPort1A = OutPortTable[Timer2Toggle];            // Temp port for COMP1A
  OutBit1A = OutBitTable[Timer2Toggle];              // Temp bitmask for COMP1A
  OutPort1B = OutPortTable[Timer2Toggle+10];         // Temp port for COMP1B
  OutBit1B = OutBitTable[Timer2Toggle+10];           // Temp bitmask for COMP1B
  if(ServoInvert[Timer2Toggle]) OCR1A = 48000 - ServoPW[Timer2Toggle] - 7985;                // Set timer1 count for pulse width.
  else OCR1A = ServoPW[Timer2Toggle]-7980;
  if(ServoInvert[Timer2Toggle+10]) OCR1B = 48000 - ServoPW[Timer2Toggle+10]-7970;            // Set timer1 count for pulse width.
  else OCR1B = ServoPW[Timer2Toggle+10]-7965;
  Timer2Toggle++;                                    // Next servo in line.
  if(Timer2Toggle==10)
  { 
    Timer2Toggle = 0;                                // If next servo is grater than 9, start on 0 again.
    RealTime50Hz();                                  // Do servo management
  }
  OutPortNext1A = OutPortTable[Timer2Toggle];        // Next Temp port for COMP1A
  OutBitNext1A = OutBitTable[Timer2Toggle];          // Next Temp bitmask for COMP1A
  OutPortNext1B = OutPortTable[Timer2Toggle+10];     // Next Temp port for COMP1B
  OutBitNext1B = OutBitTable[Timer2Toggle+10];       // Next Temp bitmask for COMP1B
}

void ServoSetup()
{
  // Timer 1 setup(16 bit):
  TCCR1A = 0;                     // Normal counting mode 
  TCCR1B = 1;                     // Set prescaler to 1 
  TCNT1 = 0;                      // Clear timer count 
  TIFR1 = 255;                    // Clear  pending interrupts
  TIMSK1 = 6;                     // Enable the output compare A and B interrupt 
  // Timer 2 setup(8 bit):
  TCCR2A = 0;                     // Normal counting mode 
  TCCR2B = 6;                     // Set prescaler to 256
  TCNT2 = 0;                      // Clear timer count 
  TIFR2 = 255;                    // Clear pending interrupts
  TIMSK2 = 6;                     // Enable the output compare A and B interrupt 
  OCR2A = 93;                     // Set counter A for about 500us before counter B below;
  OCR2B = 124;                    // Set counter B for about 2000us (20ms/10, where 20ms is 50Hz);

  #if HDServoMode == 18
    for(iCount=2;iCount<14;iCount++) pinMode(iCount, OUTPUT);    // Set all pins used to output:
    OutPortTable[18] = &PORTC;    // In 18 channel mode set channel 18 and 19 to a dummy pin that does not exist.
    OutPortTable[19] = &PORTC;
    OutBitTable[18] = 128;
    OutBitTable[19] = 128;
    Serial.begin(SerialInterfaceSpeed);  
    SerialNbOfCharToSend = 28;
  #elif HDServoMode == 20
    for(iCount=0;iCount<14;iCount++) pinMode(iCount, OUTPUT);    // Set all pins used to output:
  #endif
  DDRC = 63;                      //Set analog pins A0 - A5 as digital output also.
}

When I was at it, I also implemented an 18 servos mode. Then you can use digital pins 0 and 1 for serial communication. I also made a serial command interpreter to receive serial commands to control the servos:

Serial commands:
# = Servo channel
P = Pulse width in μs
p = Pulse width in 1/16 μs
S = Speed in us per second
s = Speed in 1/16 μs per second
T = Time in ms
PO = Pulse offset in us. -2500 to 2500 in us. Used to trim servo position.
Po = Pulse offset in 1/16 μs -40000 to 40000 in 1/16 μs
I = Invert servo movements.
N = Non-invert servo movements.
Q = Query movement. Return “.” if no servo moves and “+” if there are any servos moving.
QP = Query servo pulse width. Return 20 bytes where each is from 50 to 250 in 10 μs resolution.  So you need to multiply byte by 10 to get pulse width in us. First byte is servo 0 and last byte is servo 20.
<cr> = Carriage return. ASCII value 13. Used to end command.
Examples:

  • #0 P1500 T1000<cr> – Move Servo 0 to 1500 μs in 1 second.
  • #0 p24000 T1000<cr> – Move Servo 0 to 1500 μs in 1 second.
  • #0 p40000 s1600<cr> – Move Servo 0 to 2500us in 100 μs/s speed
  • #0 p40000 S100<cr> – Move Servo 0 to 2500us in 100u μs/s speed
  • #0 P1000 #1 P2000 T2000<cr> – Move Servo 0 and servo at the same time from current pos to 1000 μs and 2000 μs in 2 second.
  • #0 P2400 S100<cr> – Move servo 0 to 2400 μs at speed 100us/s
  • #0 P1000 #1 P1200 S500 #2 P1400 T1000<cr> – Move servo 0, 1 and 2 at the same time, but the one that takes longes S500 or T1000 will be used.
  • #0 PO100 #1 PO-100<cr> – Will set 100 μs offset to servo 0 and -100 μs ofset to servo 1
  • #0 Po1600 #1 Po-1600<cr> – Will set 100 μs offset to servo 0 and -100 μs ofset to servo 1
  • #0 I<cr> – Will set servo 0 to move inverted from standard
  • #0 N<cr> – Will set servo 0 back to move non-inverted
  • Q<cr> – Will return “.” if no servo moves and “+” if there are any servos moving
  • QP<cr> – Will retur 18 bytes (each 20ms apart) for position of servos 0 to 17

18 or 20 channels mode:

  • #define HDServoMode 18 – This will set 18 channels mode so you can use serial in and out. Serial command interpreter is activated.
  • #define HDServoMode 20 – This will set 20 channels mode, and you can not use serial.  A demo will run in the loop() routine . Serial command interpreter is not active. Use ServoMove(int Channel, long PulseHD, long SpeedHD, long Time) to control servos. One of SpeedHD or Time can be set to zero to just use the other one for speed. If both are set, the one that takes the longest time will be used. You can also change the values in the ServoPW[] array directly, but take care not to go under/over 8320/39680.

Setting the serial speed:

  • #define SerialInterfaceSpeed 115200      – Serial interface Speed

I hope this will be useful as I can’t see any other doing it the same way on the www.

Please comment and share.

This entry was posted in Software projects. Bookmark the permalink.

68 Responses to Arduino UNO Controlling 20 Servos With 15 bit Precision And Low Jitter

  1. GIANNHSitia says:

    Hello
    I saw the code you have made for Arduino uno moving 20 servos.
    I have a Arduino mega 2560 and I’d like to ask you , how can I use it ?
    I tried it and servo was sparse in Arduino’s Pin and no in row. Can you help me?

  2. GIANNHSitia says:

    Is there any ability of adding in stadard servo library the query movement of servos? You use it in your code with letter “Q”. Can you help me with tĥat?

    • arvid says:

      Then you will need to implement the all “Serial interpreter” Block of the code. I might do that some time, but I don’t have the time right now. You will need to find a free timer to run the 50Hz realtime routine. That should be possible on the Mega.

  3. GIANNHSitia says:

    goodmoring my friend

    Sorry for the bad english, but I am from Greece and do not know English well. I tried a bit of your code but failed something. And I have written a code that sends commands via serial port checks. Uses varspeedservo library, but do not know how to create a query movonent servo. breaking down the smooth movement of the servo. Use the stop command slowmove to the speed is controlled. you cant help me for with? thx a lot

  4. GIANNHSitia says:

    Hello

    try working the Serial port servo it worked but not, i change this #define HDServoMode 20 to 18 and in Serial motitor displays a message “HDServo 18 Channels Started.” . i send #0P2400T200″ but no work. plz helpme

  5. Carlos Terraza says:

    Great project

    Can you modified to work with 100Hz PWM?, “Period total = 10 * 2000 μs = 20 ms => 50 Hz” maybe if you start every pulse 1000 μs after the previous one.

    Thanks

  6. ena says:

    I am using arduino uno for generating 50 hz signal on 4 PWM pins. by default it generates 500 Hz signal. after surfing i found that by using set pin frequency library ii can generate 50HZ signal but that library is for 8 bit timers, to get 50Hz on 4 channels i would have to use 16 bit timers too, can anyone have an idea how to do that?

  7. GIANNHSitia says:

    i test your example but not working. i test and command Q and no print to Serial monitor . or +. i use arduino nano v3 at328p. mabe this is the problem?

  8. Sam says:

    I have Arduino pro mini trying to get it to give me 3 random servos moving randomly for haloween ghost statue..do you think you can help me correct it? here is my code

    #include

    Servo servo1, servo2;

    int pos = 0; // variable to store the servo position

    void setup()
    {
    pinMode(1, OUTPUT);
    analogWrite(11, 200); // light up eyes
    }

    void loop()
    {
    int time = random(0, 2000);
    delay(time);

    int position = random(60, 120);

    servo1.attach(2); // attaches the servo on pin 2 to the servo object
    servo2.attach(3); // attaches the servo on pin 3 to the servo object

    delay(15);
    servo1.write(position);
    servo2.write(position);

    delay(15);
    servo1.detach(); // detach from servo
    servo2.detach(); // de-powers motor to save batteries
    }

  9. Anonymous says:

    Hi,
    Firstly, your program has immense potential for application. thanks for making it an open source.
    I am trying to send the angles from another arduino to the servo controlling arduino board but not able to get the correct way to print the data from the master arduino.
    sending the values through the serial monitor works great. Following is one such attempt to send–#0 P1500 T1000

    Serial.print(‘#’);
    Serial.print(0);
    Serial.print(‘ ‘);
    Serial.print(‘P’);
    Serial.print(600);
    Serial.print(‘ ‘);
    Serial.print(‘T’);
    Serial.print(1000);
    Serial.println(‘\r’);

    • arvid says:

      Hi. I’m not sure if I checked for CR or CR+LF at the end of the line. (Carrige Return / Line Feed) And I’m not sure what /r sends. Might be worth looking into.

      Regards
      Arvid

  10. kingkongsy says:

    Hi,

    i was trying to control servos via serial-commands today.
    Baudrate ist correct, HDServoMode is set to 18, I had no problems using
    the Mode 20 Demos but when I try to send something like #0 p24000 T1000
    via serial-monitor, nothing is happening. Even sending Q isn´t working.

    I did get the “HDServo 18 Channels started…” message in SM, but hitting send doesn´t seem to make anything happen.

    Did I miss anything? I did vary the messages attaching at the end or leaving it empty and I also changed the options at the bottom right concerning CR, NL and such.

    Any help would be appreciated. I later on want to send messages from vvvv, but for now, there seems to be no chance to get this up and running.

    Thanks in advance, K

    • arvid says:

      Hi. Not sure, I did not have any such problems. The only thing I can think of is if you use delay(), then you will have to enable (or comment out the disable) of timer 0:

      #if HDServoMode == 18
      TIMSK0 = 0; // Disable timer 0. This can reduse jitter some more. But it’s used for delay() funtions.
      #endif // This will disable delay()!

      put // in front of TIMSK0=0;
      like this :
      // TIMSK0=0;

      Or try a slower serial speed at 9600 or something:
      #define SerialInterfaceSpeed 9600

      Regards
      Arvid

  11. kingkongsy says:

    Got it to work!

    I accidently uncommented the “‘define SerialInterfaceSpeed 115200” in the codes description (Four lines above the referenced code), which seems to have messed up the serial-connection.

    I wouldn´t have been seeing this, if you didn´t point me into the direction of baud-rate.

    Thx. This is absolutely nice stuff, btw. !
    Many thx for sharing!

    Cheers, K

  12. kingkongsy says:

    Hi again,

    just one other question.
    Would it be possible to change the “18-pin”-mode
    into something like a “9-pin”-mode?

    I only have to control about 5 or so servos (perhaps up to 9), but
    I need the other pins to switch some stuff in sync with the movement.

    I was wondering, if it would only be changing some eighteens to nines
    or if this would be way more complicated and resulting in any
    unforseeable problems, I wouldn´t be able to debug without
    some have a month of studying the theory behind your code!?

    Cheers, K

    • arvid says:

      Hi.

      The easyest way is to do the same that I did with pin 18 and 19:
      OutPortTable[18] = &PORTC; // In 18 channel mode set channel 18 and 19 to a dummy pin that does not exist.
      OutPortTable[19] = &PORTC;
      OutBitTable[18] = 128;
      OutBitTable[19] = 128;
      This is almost at the end of the code, in the setup section.
      Just continue with 17-9 like this and add :
      OutPortTable[17] = &PORTC;
      OutBitTable[17] = 128;
      OutPortTable[16] = &PORTC;
      OutBitTable[16] = 128;
      OutPortTable[15] = &PORTC;
      OutBitTable[15] = 128;
      OutPortTable[14] = &PORTC;
      OutBitTable[14] = 128;
      OutPortTable[13] = &PORTC;
      OutBitTable[13] = 128;
      OutPortTable[12] = &PORTC;
      OutBitTable[12] = 128;
      OutPortTable[11] = &PORTC;
      OutBitTable[11] = 128;
      OutPortTable[10] = &PORTC;
      OutBitTable[10] = 128;
      OutPortTable[9] = &PORTC;
      OutBitTable[9] = 128;

      Then the 18 mode will be a 9 mode instead (0-8). It will not be so efficient, since the CPU is using time for all channels anyways, but it’s easy…

      Regards
      Arvid

      • kingkongsy says:

        I guess “OutBitTable[9]=0” would be
        setting Pin 9 to LOW and “OutBittTable[9]=255” would
        be Pin 9 set to HIGH, right?

        Or would I be able to treat the “dummy pins” as usual using digitalWrite?

        Cheers, K

        • arvid says:

          Hi.
          You would be able to treat the dummy pins as usual. My program is not using digitalWrite for the servos. It uses OutPortTable and OutBitTable which is pointing to the correct pins. So when setting those dummy pins to PORTC and bit 128, the program will not touch the real pins. It will change PORTC,bit 8, which is a non existing pin.

          Regards
          Arvid

  13. Imuley50 says:

    Hi, I’m working on a biped robot which have 22 servos for the movement of the head till the wolking ones; thanks for this 20servos controller program what i tryin to understand

    then i have to control the biped robot with adafruit.com bluetooth module nrf8001. the robot has allready an arduino mega ADK for android is there some can give me more ideas? please i need your help
    Thanks in advance!!!!!!!!

    • arvid says:

      Hi. If you use Bluetooth Controller you will need rx and tx for the module. Then there is 18 servos to be controlled. I use 18 servos and a HC-05 module to control a hexapod, but I-m working on a robot that will use more servod, so I planned using two arduinos to controll them.

    • roid says:

      The Arduino Mega can drive upto 48 servos using the standard Arduino Servo library. So you do not really need Arvid’s code at all.

  14. Gianmarco says:

    Avid
    This program is fantastic and exactly what I need but I would like to connect multiple arduinos so that I can control 54 or more servos
    To do that I2C interface would be more appropriate.
    How can I get in HDServoMode 18 but freeing up pin A4 and A5 rather then 0 and 1

    • arvid says:

      Hi.

      I think it should be possible if you edit the 2 lines:
      static volatile uint8_t *OutPortTable[20] = {&PORTD,&PORTD,&PORTD,&PORTD,&PORTD,&PORTD,&PORTB,&PORTB,&PORTB,&PORTB,&PORTB,&PORTB,&PORTC,&PORTC,&PORTC,&PORTC,&PORTC,&PORTC,&PORTD,&PORTD};
      static uint8_t OutBitTable[20] = {4,8,16,32,64,128,1,2,4,8,16,32,1,2,4,8,16,32,1,2};
      If you put you A4 and A5 at the end. These two tables correspond to the pin mapping:
      // Ch0=Pin2, Ch1=Pin3, Ch2=Pin4, Ch3=Pin5, Ch4=Pin6, Ch5=Pin7, Ch6=Pin8, Ch7=Pin9, Ch8=Pin10, Ch9=Pin11
      // Ch10=Pin12, Ch11=Pin13, Ch12=PinA0, Ch13=PinA1, Ch14=PinA2, Ch15=PinA3, Ch16=PinA4, Ch17=PinA5, Ch18=Pin0, Ch19=Pin1
      so you just have to switch ch16/ch17 With ch18/ch19 like this:
      static volatile uint8_t *OutPortTable[20] = {&PORTD,&PORTD,&PORTD,&PORTD,&PORTD,&PORTD,&PORTB,&PORTB,&PORTB,&PORTB,&PORTB,&PORTB,&PORTC,&PORTC,&PORTC,&PORTC,&PORTD,&PORTD,&PORTC,&PORTC};
      static uint8_t OutBitTable[20] = {4,8,16,32,64,128,1,2,4,8,16,32,1,2,4,8,1,2,16,32};

      And you might want to have timer 0 enabled. Just comment out these lines with // in the beginning of the lines:
      // #if HDServoMode == 18
      // TIMSK0 = 0; // Disable timer 0. This can reduse jitter some more. But it’s used for delay() funtions.
      // #endif // This will disable delay()!

      At least I think that should be enough.

      Regards
      Arvid

  15. roid says:

    Hey Arvid, the Arduino Mini Pro uses the same Atmega328 chip as the Uno, but it has an extra 2 analog pins (PinA6 and PinA7).
    So it seems like your code might be adapted to drive 2 extra servos, yes?
    ie: 20 servos with serial, or 22 servos without.

    The Mini Pro is also the cheapest Arduino around. So there’s probably a lot of people who are running your code on Mini Pros, who could use those extra 2 servos ;).

    • arvid says:

      Hi. I’m also planning using pro mini with 22 IO-pins. But my code can’t be easilly adapted for 22 servos. This is because I have focused on precision and that the timing do not overlap. This makes it fixed to 20 servos. Even in 18 servo mode, the code is running the last two servos as dummys. The good thing about the pro mini is that you can use the two extra pins in 20-servo mode and still have RX and TX free.

      Regard
      Arvid

  16. Spenna says:

    Hey Arvid.

    Love your projects, you are a true artist 🙂
    I´m wondering where you are buying your components from?
    Do you have any recommendations for an aspiring arduionoer?

    Takker på forhånd, send gjerne svar på epost.

    Kind regards
    Espen

  17. erdal inci says:

    Hi
    am using arduino Due 🙂
    can you tell me how to change the servo write Resolution to 12-bit Resolution
    do i need to add analogWriteResolution(12);
    or how !
    what do you think!

    • arvid says:

      Hi. This sketch won’t work with Due without a lot of modification. And I don’t use the analog write function either.

  18. jema says:

    Hi Arvid.

    Thanks for your fantastic contribution to Arduino comunity.

    Is it possible to use the Arduino Pro Mini 3,3V board running at 8MHz instead of 16MHz?
    Which things should be changed in your code? just Prescaler value or more parameters?

    Have you the final sketch code for 20 servos plus Tx-RX (with command interpreter) for this board that has more I/O pins?

    Thanks again.

    • roid says:

      Hi, i’m not Arvid, but i’ve been meaning to come back here and say this:

      After some reading, it seems that A6 & A7 cannot be used as outputs, those 2 pins are unique in that they can only be used as analog-in and nothing else. This is likely the reason why a lot of Arduino Pro Mini boards put pins A6 and A7 in strange board locations: those pins have little use.

      I suppose it might be possible to use one of them as a serial-in RX pin*, but they can’t be a TX pin, nor as a PWM pin to output to a servo. So the theoretical upper capabilities of an Arduino running an ATmega328 would be 20x servos plus an RX pin, no TX at all, and you’ll also have a spare unused analog-in pin (maybe you could wire this to the battery to constantly measure the load).

      *If what i’ve read is true and those 2 pins can’t even be digital inputs (can only be analog inputs), then i wonder if that means you’d have to do your own ADC in software if you’re to use them as serial RX.

      • arvid says:

        I also noticed this.
        Regards
        Arvid

        • jema says:

          Thanks both for the answer.
          And what about the 8MHz version instead 16MHz one ?
          Which things I need to modify in sketch code ?
          Kind regards

          • arvid says:

            Hi.

            You’ll need to change the preescaler for the timers that is used, but since I used prescaler = 1 for the 16 bit timer, this can’t be set any lower. In theory it should work to divide the timer compare registers by two. There are two places you’ll have to change some code. It’s almost at the bottom:
            I’m not sure if it will work correctly.

            From:
            OCR2A = 93; // Set counter A for about 500us before counter B below;
            OCR2B = 124; // Set counter B for about 2000us (20ms/10, where 20ms is 50Hz);

            if(ServoInvert[Timer2Toggle]) OCR1A = 48000 – ServoPW[Timer2Toggle] – 7985; // Set timer1 count for pulse width.
            else OCR1A = ServoPW[Timer2Toggle]-7980;
            if(ServoInvert[Timer2Toggle+10]) OCR1B = 48000 – ServoPW[Timer2Toggle+10]-7970; // Set timer1 count for pulse width.
            else OCR1B = ServoPW[Timer2Toggle+10]-7965;

            To:
            OCR2A = 93/2; // Set counter A for about 500us before counter B below;
            OCR2B = 124/2; // Set counter B for about 2000us (20ms/10, where 20ms is 50Hz);

            if(ServoInvert[Timer2Toggle]) OCR1A = (48000 – ServoPW[Timer2Toggle] – 7985)/2; // Set timer1 count for pulse width.
            else OCR1A = (ServoPW[Timer2Toggle]-7980)/2;
            if(ServoInvert[Timer2Toggle+10]) OCR1B = (48000 – ServoPW[Timer2Toggle+10]-7970)/2; // Set timer1 count for pulse width.
            else OCR1B = (ServoPW[Timer2Toggle+10]-7965)/2;

            But again, I’m not sure it will work. And it might introduce some jitter.
            Regards
            Arvid

  19. jema says:

    Thanks Arvid for your coments.

  20. Rob says:

    Hi Arvid,

    I printed a SG92-Servo-Robot from here: http://www.thingiverse.com/thing:797211. I would like to control it via an arduino nano. Therefore i found you genious code wich can control 18 servos synchroiously! I saw that you used your lib to control a hexapod. I would like to use the same configuration. Would you mind providing me the code for that?

    Cheers

    Robert

    • arvid says:

      Hi. Thanks for your comment. I actually used another arduino to send the serial command to the hexapod via bluetooth. I think one arduino is not fast enough to do it all. Especially when I’m doing some inverse kinematics to control the servo angles. In the first hexapod I made, I used a PC to send the serial bluetooth command to the servo-Arduino. I made a program in MS Visual Basic to calculate all the servo angles. I was never quite happy with that program. It was “a bit” messy. I am now working on another program for an Arduino for my second hexapod, but I’m not finished with that yet. My plan is to have one arduino in a “remote unit” sending commands to a second arduino on the hexapod via RF. Then this Arduino will calulate all angles and send them to the third Arduion that will run this code that you’ll see on this site. I will publish the code when I’m finished. But it might take some time. Regards Arvid

      • Rob says:

        Hi Arvid,

        thanks for your comment. I would like to do a similar thing you have done. I use the nano mainly for controlling the servo. then a second one which is on a remote control unit. This one should produce commands regarding on the control buttons on this unit. I think it would not need realtime kinematics in my case. I will try to store the moving patterns in some tables on the nano of the hexapod. Looking forward to your code.

        Regards Robert

      • Montilien says:

        Hi Arvid,
        very nice work your sketch. But i wrote a sketch for Arduino Nano to send serial commands through serial wireless module but the results are eratics….
        Here my code:
        /*RC coder for 18 servos for use with UART Radios
        Can send 8 proportional channels with analog input
        can send 10 more PWM channels with digital input
        ALL channels 15 bits resolution

        ARDUINO Nano Based
        A0 to A7 proportionnal input channels
        D2 to D11 for On/Off input switches
        DO output datas (TxD)

        André PIQUEMAL
        12 novembre 2015
        */

        int AI_Pin_ch1 = 0;
        int AI_Pin_ch2 = 1;
        int AI_Pin_ch3 = 2;
        int AI_Pin_ch4 = 3;
        int AI_Pin_ch5 = 4;
        int AI_Pin_ch6 = 5;
        int AI_Pin_ch7 = 6;
        int AI_Pin_ch8 = 7;

        int AI_Raw_ch1;
        int AI_Raw_ch2;
        int AI_Raw_ch3;
        int AI_Raw_ch4;
        int AI_Raw_ch5;
        int AI_Raw_ch6;
        int AI_Raw_ch7;
        int AI_Raw_ch8;

        int ch1_uS = 1500;
        int ch2_uS = 1500;
        int ch3_uS = 1500;
        int ch4_uS = 1500;
        int ch5_uS = 1500;
        int ch6_uS = 1500;
        int ch7_uS = 1500;
        int ch8_uS = 1500;

        //int outPinData = 0;
        int outPinTest = 13;

        int input_sw_1 = 2;
        int input_sw_2 = 3;
        int input_sw_3 = 4;
        int input_sw_4 = 5;
        int input_sw_5 = 6;
        int input_sw_6 = 7;
        int input_sw_7 = 8;
        int input_sw_8 = 9;
        int input_sw_9 = 10;
        int input_sw_10 = 11;

        int sw_1;
        int sw_2;
        int sw_3;
        int sw_4;
        int sw_5;
        int sw_6;
        int sw_7;
        int sw_8;
        int sw_9;
        int sw_10;

        int sw1_uS = 2100;
        int sw2_uS = 2100;
        int sw3_uS = 2100;
        int sw4_uS = 2100;
        int sw5_uS = 2100;
        int sw6_uS = 2100;
        int sw7_uS = 2100;
        int sw8_uS = 2100;
        int sw9_uS = 2100;
        int sw10_uS = 2100;

        ISR(TIMER1_COMPA_vect) {
        dataoutput(); // Jump to dataoutput subroutine
        }

        void setup() {

        Serial.begin(115200); //set serial out to 115200 bauds
        //Serial.println(“debut du Setup”);
        //pinMode(outPinData, OUTPUT);
        pinMode(outPinTest, OUTPUT);
        pinMode(input_sw_1, INPUT);
        digitalWrite(input_sw_1, HIGH);
        pinMode(input_sw_2, INPUT);
        digitalWrite(input_sw_2, HIGH);
        pinMode(input_sw_3, INPUT);
        digitalWrite(input_sw_3, HIGH);
        pinMode(input_sw_4, INPUT);
        digitalWrite(input_sw_4, HIGH);
        pinMode(input_sw_5, INPUT);
        digitalWrite(input_sw_5, HIGH);
        pinMode(input_sw_6, INPUT);
        digitalWrite(input_sw_6, HIGH);
        pinMode(input_sw_7, INPUT);
        digitalWrite(input_sw_7, HIGH);
        pinMode(input_sw_8, INPUT);
        digitalWrite(input_sw_8, HIGH);
        pinMode(input_sw_9, INPUT);
        digitalWrite(input_sw_9, HIGH);
        pinMode(input_sw_10, INPUT);
        digitalWrite(input_sw_10, HIGH);

        // Setup timer
        TCCR1A = B00110001; // Compare register B used in mode ‘3’
        TCCR1B = B00010010; // WGM13 and CS11 set to 1
        TCCR1C = B00000000; // All set to 0
        TIMSK1 = B00000010; // Interrupt on compare B
        TIFR1 = B00000010; // Interrupt on compare B
        OCR1A = 20000; // 20mS frame output refresh
        OCR1B = 1000;
        }

        void dataoutput() {

        digitalWrite(outPinTest, HIGH);

        Serial.print(“#0 P”);
        Serial.print(ch1_uS);
        Serial.println(‘\r’);
        delay(1);
        Serial.print(“#1 P”);
        Serial.print(ch2_uS);
        Serial.println(‘\r’);
        delay(1);
        Serial.print(“#2 P”);
        Serial.print(ch3_uS);
        Serial.println(‘\r’);
        delay(1);
        Serial.print(“#3 P”);
        Serial.print(ch4_uS);
        Serial.println(‘\r’);
        delay(1);
        Serial.print(“#4 P”);
        Serial.print(ch5_uS);
        Serial.println(‘\r’);
        delay(1);
        Serial.print(“#5 P”);
        Serial.print(ch6_uS);
        Serial.println(‘\r’);
        delay(1);
        Serial.print(“#6 P”);
        Serial.print(ch7_uS);
        Serial.println(‘\r’);
        delay(1);
        Serial.print(“#7 P”);
        Serial.print(ch8_uS);
        Serial.println(‘\r’);
        delay(1);

        Serial.print(“#8 P”);
        Serial.print(sw1_uS);
        Serial.println(‘\r’);
        delay(1);
        Serial.print(“#9 P”);
        Serial.print(sw2_uS);
        Serial.println(‘\r’);
        /*Serial.print(“#10 P”);
        Serial.print(sw3_uS);
        Serial.println(‘\r’);
        Serial.print(“#11 P”);
        Serial.print(sw4_uS);
        Serial.println(‘\r’);
        */
        /*Serial.print(“#”);
        Serial.print(“12″);
        Serial.print(” “);
        Serial.print(“P”);
        Serial.print(sw5_uS);
        Serial.print(” “);
        Serial.println(“T99”);

        Serial.print(“#”);
        Serial.print(“13″);
        Serial.print(” “);
        Serial.print(“P”);
        Serial.print(sw6_uS);
        Serial.print(” “);
        Serial.println(“T99”);

        Serial.print(“#”);
        Serial.print(“14″);
        Serial.print(” “);
        Serial.print(“P”);
        Serial.print(sw7_uS);
        Serial.print(” “);
        Serial.println(“T99”);

        Serial.print(“#”);
        Serial.print(“15″);
        Serial.print(” “);
        Serial.print(“P”);
        Serial.print(sw8_uS);
        Serial.print(” “);
        Serial.println(“T99”);

        Serial.print(“#”);
        Serial.print(“16″);
        Serial.print(” “);
        Serial.print(“P”);
        Serial.print(sw9_uS);
        Serial.print(” “);
        Serial.println(“T99”);

        Serial.print(“#”);
        Serial.print(“17″);
        Serial.print(” “);
        Serial.print(“P”);
        Serial.print(sw10_uS);
        Serial.print(” “);
        Serial.println(“T99”);*/
        Serial.flush();
        digitalWrite(outPinTest, LOW);

        }
        void loop() {
        // Read 8 proportional channel

        int AI_Raw_ch1 = analogRead(A0);
        int AI_Raw_ch2 = analogRead(A1);
        int AI_Raw_ch3 = analogRead(A2);
        int AI_Raw_ch4 = analogRead(A3);
        int AI_Raw_ch5 = analogRead(A4);
        int AI_Raw_ch6 = analogRead(A5);
        int AI_Raw_ch7 = analogRead(A6);
        int AI_Raw_ch8 = analogRead(A7);

        // Calcule la durée des impulsions de commande des servos du RX

        ch1_uS = map(AI_Raw_ch1, 0, 1023, 700, 2300);
        ch2_uS = map(AI_Raw_ch2, 0, 1023, 700, 2300);
        ch3_uS = map(AI_Raw_ch3, 0, 1023, 700, 2300);
        ch4_uS = map(AI_Raw_ch4, 0, 1023, 700, 2300);
        ch5_uS = map(AI_Raw_ch5, 0, 1023, 700, 2300);
        ch6_uS = map(AI_Raw_ch6, 0, 1023, 700, 2300);
        ch7_uS = map(AI_Raw_ch7, 0, 1023, 700, 2300);
        ch8_uS = map(AI_Raw_ch8, 0, 1023, 700, 2300);

        sw_1 = digitalRead (input_sw_1);
        sw_2 = digitalRead (input_sw_2);
        /*sw_3 = digitalRead (input_sw_3);
        sw_4 = digitalRead (input_sw_4);
        sw_5 = digitalRead (input_sw_5);
        sw_6 = digitalRead (input_sw_6);
        sw_7 = digitalRead (input_sw_7);
        sw_8 = digitalRead (input_sw_8);
        sw_9 = digitalRead (input_sw_9);
        sw_10 = digitalRead (input_sw_10);*/

        if (sw_1==0) {
        sw1_uS = 900;}
        else {
        sw1_uS = 2100;}
        if (sw_2==0) {
        sw2_uS = 900;}
        else {
        sw2_uS = 2100;}
        //Serial.println(“OK”);
        /*if (sw_3==0) {
        sw3_uS = 900;}
        else {
        sw3_uS = 2100;}
        if (sw_4==0) {
        sw4_uS = 900;}
        else {
        sw4_uS = 2100;}
        Serial.println(“OK”);
        if (sw_5==0) {
        sw5_uS = 900;}
        else {
        sw5_uS = 2100;}
        if (sw_6==0) {
        sw6_uS = 900;}
        else {
        sw6_uS = 2100;}
        if (sw_7==0) {
        sw7_uS = 900;}
        else {
        sw7_uS = 2100;}
        if (sw_8==0) {
        sw8_uS = 900;}
        else {
        sw8_uS = 2100;}
        if (sw_9==0) {
        sw9_uS = 900;}
        else {
        sw9_uS = 2100;}
        if (sw_10==0) {
        sw10_uS = 900;}
        else {
        sw10_uS = 2100;}*/
        }
        It is written for 12 channels (8 proportionals and 4 switches)
        I think it mlght be a problem of timing between serial input and servo setting????
        What do you think of that?
        Best regards
        Andre

        • arvid says:

          Hi.
          It looks for me that you call the interrupt every 10ms (if you are using a 16MHz arduino). And then the ISR(TIMER1_COMPA_vect) is using 25-30 ms to complete. If your arduino is 8MHz, it will still not work, because the ISR is using more that 20ms to complete. So the ISR will be called again before it is finished. Just a guess…
          Regards Arvid

  21. Erwin says:

    Hai,

    Is there a list of the ch0………..ch19 Hand/ foot/head/arm and so on
    I have also made one
    http://www.thingiverse.com/make:166362

    Kind regards

    • arvid says:

      Hi. I have not used the code for biped-robot. This is only generic servo controlling code. Then you’ll have to code the movements yourself.
      Regards Arvid

  22. Brandon says:

    Hi arvid ,
    Is there a way to turn off the servo after some period of time?

    • arvid says:

      Like power off? Or just not moving? You’ll have to control the movement with your own additional program. If you want to turn off the power to the servo, You’ll have to connect it to a MOSFET and then control the MOSFET on or off. Like I did in the useless box-project.

      • Brandon says:

        how about adding a serial command to stop/start a pin to generate the pwm signal?

        • arvid says:

          A servo needs a PWM at all time it has power. If not, the servo will do some unpredictable things.

          • Brandon says:

            Thanks arvid,
            But I want to try. Except the function taking the serial commands, which part of your code should I look at for this experiment?

          • arvid says:

            Hi. You’ll have to set ouporttable and outportbit to some dummy values for those pins, as mentioned in some other comment. Then you’ll have to change the init for digital and analog pins.

            static volatile uint8_t *OutPortTable[20] = {&PORTD,&PORTD,&PORTD,&PORTD,&PORTD,&PORTD,&PORTB,&PORTB,&PORTB,&PORTB,&PORTB,&PORTB,&PORTC,&PORTC,&PORTC,&PORTC,&PORTC,&PORTC,&PORTD,&PORTD};
            static uint8_t OutBitTable[20] = {4,8,16,32,64,128,1,2,4,8,16,32,1,2,4,8,16,32,1,2};

            for(iCount=2;iCount<14;iCount++) pinMode(iCount, OUTPUT); // Set all pins used to output:

            DDRC = 63; //Set analog pins A0 - A5 as digital output also.

  23. Dhruba says:

    Dear Arvind,
    Your code has been of great importance for my biped. I would like to know whether we can modify the code to do perform an action like attach or detach servo(arduino)so as to decouple the servo pins on demand.

    • arvid says:

      Hi. I think that would be dufficult. you may be better off using the standard servo library in Arduino instead.

  24. abooosidhu says:

    hai iam beginer to robotics..i need to know the cost for the servo you purchased and how did you connected the 20 servos in single board,and last question is is it arduino uno is comfortable for controlling these 20 servos at a time or need board like arduiono mega..thankyou

    • arvid says:

      Hi. These servos are not that good for robots, but I’ve used some MG996R from ebay. Some of them ar ok, and some are not that great. Some have a big dead-band, so they are not that precise. But they are cheap, about 5$ each. All servos are connected to + and – 6V, and the signal-pin for the servos (PWM) are each connected to a output on the Arduino. Arduino UNO need 5V, not 6V… Both ground for 5V and 6V are connected together. UNO can handle 20 servos fine, but you don’t have any extra output or input left on the Arduino then to controll other stuff. If you use the MEGA, you’ll need to use the Arduino standard mega servo library. Then you’ll should be able to controll 48 servos on the one MEGA. The standard servo librarey only supports 12 servos for the UNO though.
      Regards Arvid

  25. Montilien says:

    Hi, Arvi,
    thanks for your response but I checked my nano V3.0 and it use 16 Mhz clock.
    The ISR is called every 20 ms and I have included at the end of the void loop of my sketch, a flag “Serial.print(“OK”); so I can check with the serial monitor that all datas are correctly sent and that the void loop is completly proceeded . “OK” appears 3 times between the stream of datas. all is OK.
    I have also a question: what is the lower and higher limit of “T” and “S” parameter? What happens if I resend datas for the same output when the previous is not finished?
    Yhanks very much for your help… and sorry for my poor english….
    Best regards
    Andre

    • arvid says:

      Hi.
      T : Limits shoud be 0 to 20000, or there about. The actual limit uppwards is 2 billion milliseconds, but that would not work well. It depends on how long the move is. The longer the move, the longer time limit. If the move is 2000 us difference, the limit should be 2000*16ms=32000ms. It subtract 1/16 of a ms at minimum, for each 20ms cycle.
      S : Limits should be 1 – 200000 or there about. 0 is fast as possible. 200000 will be 4000us swing in 20ms.
      If you send new data for the same servo before it is finished. It will calculate the movement from the current position that the servos are at the moment. Or more correct, the position the servo should be at the moment. It does not take into account that servos have a max speed for movements.

      Regards
      Arvid

  26. Andy says:

    Hi,
    Your code is excellent – much more robust and stable than most of the other examples around.
    I would like to be able to use this alongside some other libraries that require me to use PIN 2 – ideally I would like to be able to run a version of your code reduced for 8 servos with this other library.
    Is it a simple operation to ‘trim’ down to 8 servos?
    Thanks in advance,
    Andy.

    • arvid says:

      Hi.
      Yes, sort of. The code will prosess all channels but you get the pin free if you set channels to dummy pins. Like channel 18 and 19 in 18 channel mode. Just fill in the channels you’ll not need.

      OutPortTable[18] = &PORTC; // In 18 channel mode set channel 18 and 19 to a dummy pin that does not exist.
      OutPortTable[19] = &PORTC;
      OutBitTable[18] = 128;
      OutBitTable[19] = 128;

      Regards
      Arvid

  27. Andy says:

    Hi Arvid,
    I’m wanting to use your code alongside the ‘Wire’ library. Everything compiles and loads fine, however, i’m finding that I am getting some jitter on the servos (only running 8 as per previous post).
    It looks like it may be the Wire library causing the issues.
    I was wondering if you have your code implemented with Wire anywhere?
    Andy.

    • arvid says:

      Hi. No I have not. I think that the problem is that the Wire library is very dependant on timing as well, and only one program can run a interrupt at a time…
      /Arvid

  28. lua muniz says:

    Hi, great challenge
    Would you know to tell me if possible stop a servo during 10 minutes, and after making it run again?. How must I program motors to stop 10 minutes and run ten minutes in a predefinied way?
    I think is impossible with delay(), millis() or micros(), but I don’t find how stop it so long.
    I’m begginer, whatever help welcome
    best regards¡

    • arvid says:

      delay(600000); // 0 to 4294967295

      • lua muniz says:

        Thanks again for your quick response Arvid.
        I had read that delay() blocks processor, and prevents to do anything else.
        I try to do an installation with 20 servos. They must rotate 180º and expect a different time each one before turning to rotate 180º again.
        (Each servo drives a sand glass that takes different time to change it sands)
        Servos do the same 8 hours a day for 3.5 months. Really, I can use delay then?

        This could be the code?:

        /* Sweep
        by BARRAGAN
        This example code is in the public domain.

        modified 8 Nov 2013
        by Scott Fitzgerald

        http://www.arduino.cc/en/Tutorial/Sweep

        modified 21 Apri 2016
        by Lua Muniz

        thanks Arvid’s contribution
        */

        #include

        Servo myservo1;
        Servo myservo2;
        Servo myservo3;
        Servo myservo4;
        Servo myservo5;
        Servo myservo6;
        Servo myservo7;
        Servo myservo8;
        Servo myservo9;
        Servo myservo10;
        Servo myservo11;
        Servo myservo12;
        Servo myservo13;
        Servo myservo14;
        Servo myservo15;
        Servo myservo16;
        Servo myservo17;
        Servo myservo18;
        Servo myservo19;
        Servo myservo20;

        int pos = 0;

        void setup() {
        myservo1.attach(2); //It must delay 15 minutes between turns of 180º
        myservo2.attach(3); //It must delay 10 minutes between turns of 180º
        myservo3.attach(4); //It must delay 8 minutes between turns of 180º
        myservo4.attach(5); //It must delay 7 minutes between turns of 180º
        myservo5.attach(6); //It must delay 6 minutes between turns of 180º
        myservo6.attach(7); //It must delay 5 minutes between turns of 180º
        myservo7.attach(8); //It must delay 4 minutes between turns of 180º
        myservo8.attach(9); //It must delay 3 minutes between turns of 180º
        myservo9.attach(10); //It must delay 1 minutes between turns of 180º
        myservo10.attach(11); //It must delay 30 SECONDS between turns of 180º
        myservo11.attach(12); //It must delay 15 minutes between turns of 180º
        myservo12.attach(13); //It must delay 10 minutes between turns of 180º
        myservo13.attach(22); //It must delay 8 minutes between turns of 180º
        myservo14.attach(23); //It must delay 7 minutes between turns of 180º
        myservo15.attach(24); //It must delay 6 minutes between turns of 180º
        myservo16.attach(25); //It must delay 5 minutes between turns of 180º
        myservo17.attach(26); //It must delay 4 minutes between turns of 180º
        myservo18.attach(27); //It must delay 3 minutes between turns of 180º
        myservo19.attach(28); //It must delay 1 minutes between turns of 180º
        myservo20.attach(29); //It must delay 30 SECONDS between turns of 180º
        }

        void loop() {
        //////////////////////////// TURN 180º /////////////////////////
        for (pos = 0; pos <= 180; pos += 1) { // servo1 turn 0-180º and…
        myservo1.write(pos);
        delay(900.000); // …servo1 DELAY 15 min.
        }
        for (pos = 0; pos <= 180; pos += 1) { // servo2 turn 0-180º and…
        myservo2.write(pos);
        delay(600.000); // ..servo2 DELAY 10 min.
        }
        for (pos = 0; pos <= 180; pos += 1) { // servo3 turn 0-180º and…
        myservo3.write(pos);
        delay(480.000); // …servo3 DELAY 8 min.
        }
        for (pos = 0; pos <= 180; pos += 1) { // servo4 turn 0-180º and…
        myservo4.write(pos);
        delay(420.000); // …servo4 DELAY 7 min.
        }
        for (pos = 0; pos <= 180; pos += 1) { // servo5 turn 0-180º and…
        myservo5.write(pos);
        delay(360.000); // …servo5 DELAY 6 min.
        }
        for (pos = 0; pos <= 180; pos += 1) { // servo6 turn 0-180º and…
        myservo6.write(pos);
        delay(300.000); // ..servo6 DELAY 5 min.
        }
        for (pos = 0; pos <= 180; pos += 1) { // servo7 turn 0-180º and…
        myservo7.write(pos);
        delay(240.000); // …servo7 DELAY 4 min.
        }
        for (pos = 0; pos <= 180; pos += 1) { // servo8 turn 0-180º and…
        myservo8.write(pos);
        delay(180.000); // …servo8 DELAY 3 min.
        }
        for (pos = 0; pos <= 180; pos += 1) { // servo9 turn 0-180º and…
        myservo9.write(pos);
        delay(60.000); // …servo9 DELAY 1 min.
        }
        for (pos = 0; pos <= 180; pos += 1) { // servo10 turn 0-180º and…
        myservo10.write(pos);
        delay(30.000); // …servo10 DELAY 30 SECONDS.
        }
        for (pos = 0; pos <= 180; pos += 1) { // servo11 turn 0-180º and…
        myservo11.write(pos);
        delay(900.000); // …servo11 DELAY 15 min.
        }
        for (pos = 0; pos <= 180; pos += 1) { // servo12 turn 0-180º and…
        myservo12.write(pos);
        delay(600.000); // ..servo12 DELAY 10 min.
        }
        for (pos = 0; pos <= 180; pos += 1) { // servo13 turn 0-180º and…
        myservo13.write(pos);
        delay(480.000); // …servo13 DELAY 8 min.
        }
        for (pos = 0; pos <= 180; pos += 1) { // servo14 turn 0-180º and…
        myservo14.write(pos);
        delay(420.000); // …servo14 DELAY 7 min.
        }
        for (pos = 0; pos <= 180; pos += 1) { // servo15 turn 0-180º and…
        myservo15.write(pos);
        delay(360.000); // …servo15 DELAY 6 min.
        }
        for (pos = 0; pos <= 180; pos += 1) { // servo16 turn 0-180º and…
        myservo16.write(pos);
        delay(300.000); // ..servo16 DELAY 5 min.
        }
        for (pos = 0; pos <= 180; pos += 1) { // servo17 turn 0-180º and…
        myservo17.write(pos);
        delay(240.000); // …servo17 DELAY 4 min.
        }
        for (pos = 0; pos <= 180; pos += 1) { // servo18 turn 0-180º and…
        myservo18.write(pos);
        delay(180.000); // …servo18 DELAY 3 min.
        }
        for (pos = 0; pos <= 180; pos += 1) { // servo19 turn 0-180º and…
        myservo19.write(pos);
        delay(60.000); // …servo19 DELAY 1 min.
        }
        for (pos = 0; pos = 0; pos -= 1) { // servo1 turn 180-0º and…
        myservo1.write(pos);
        delay(900.000); // …servo1 DELAY 15 min.
        }
        for (pos = 1800; pos >= 0; pos -= 1) { // servo2 turn 180-0º and…
        myservo2.write(pos);
        delay(600.000); // ..servo2 DELAY 10 min.
        }
        for (pos = 180; pos >= 0; pos -= 1) { // servo3 turn 180-0º and…
        myservo3.write(pos);
        delay(480.000); // …servo3 DELAY 8 min.
        }
        for (pos = 1800; pos >= 0; pos -= 1) { // servo4 turn 180-0º and…
        myservo4.write(pos);
        delay(420.000); // …servo4 DELAY 7 min.
        }
        for (pos = 180; pos >= 0; pos -= 1) { // servo5 turn 180-0º and…
        myservo5.write(pos);
        delay(360.000); // …servo5 DELAY 6 min.
        }
        for (pos = 1800; pos >= 0; pos -= 1) { // servo6 turn 180-0º and…
        myservo6.write(pos);
        delay(300.000); // ..servo6 DELAY 5 min.
        }
        for (pos = 180; pos >= 0; pos -= 1) { // servo7 turn 180-0º and…
        myservo7.write(pos);
        delay(240.000); // …servo7 DELAY 4 min.
        }
        for (pos = 1800; pos >= 0; pos -= 1) { // servo8 turn 180-0º and…
        myservo8.write(pos);
        delay(180.000); // …servo8 DELAY 3 min.
        }
        for (pos = 180; pos >= 0; pos -= 1) { // servo9 turn 180-0º and…
        myservo9.write(pos);
        delay(60.000); // …servo9 DELAY 1 min.
        }
        for (pos = 1800; pos >= 0; pos -= 1) { // servo10 turn 180-0º and…
        myservo10.write(pos);
        delay(30.000); // …servo10 DELAY 30 SECONDS.
        }
        for (pos = 180; pos >= 0; pos -= 1) { // servo11 turn 180-0º and…
        myservo11.write(pos);
        delay(900.000); // …servo11 DELAY 15 min.
        }
        for (pos = 1800; pos >= 0; pos -= 1) { // servo12 turn 180-0º and…
        myservo12.write(pos);
        delay(600.000); // ..servo12 DELAY 10 min.
        }
        for (pos = 180; pos >= 0; pos -= 1) { // servo13 turn 180-0º and…
        myservo13.write(pos);
        delay(480.000); // …servo13 DELAY 8 min.
        }
        for (pos = 1800; pos >= 0; pos -= 1) { // servo14 turn 180-0º and…
        myservo14.write(pos);
        delay(420.000); // …servo14 DELAY 7 min.
        }
        for (pos = 180; pos >= 0; pos -= 1) { // servo15 turn 180-0º and…
        myservo15.write(pos);
        delay(360.000); // …servo15 DELAY 6 min.
        }
        for (pos = 1800; pos >= 0; pos -= 1) { // servo16 turn 180-0º and…
        myservo16.write(pos);
        delay(300.000); // ..servo16 DELAY 5 min.
        }
        for (pos = 180; pos >= 0; pos -= 1) { // servo17 turn 180-0º and…
        myservo17.write(pos);
        delay(240.000); // …servo17 DELAY 4 min.
        }
        for (pos = 1800; pos >= 0; pos -= 1) { // servo18 turn 180-0º and…
        myservo18.write(pos);
        delay(180.000); // …servo18 DELAY 3 min.
        }
        for (pos = 180; pos >= 0; pos -= 1) { // servo19 turn 180-0º and…
        myservo19.write(pos);
        delay(60.000); // …servo19 DELAY 1 min.
        }
        for (pos = 1800; pos >= 0; pos -= 1) { // servo20 turn 180-0º and…
        myservo20.write(pos);
        delay(30.000); // …servo20 DELAY 30 SECONDS.
        }

        }

  29. M. Junaid Ali says:

    I had tried much but failed to convert this code for arduino mega 2560. I am beginner so, please help me. thanks in advance.

  30. Anthony says:

    Dear arvid,
    thanks for your time and passion to answer to our comments.
    I’m working on a project with 28 mircoservos and an Arduino Mega, the total current will be nearly 30A.
    How can I provide such power supply to my servos?
    Wish you a great day.

    Anthony

Comments are closed.