Servo functionality

I have been playing with servos, and I needed two features that the servo libraries do not provide; servo trim and servo reversing. Each physical dis-assembly and reassembly puts the servos at a slightly different position, so servo trimming is required. And it would be very desirable to have the ability to reverse servos to have them operate in the same direction, regardless of servo orientation.

I know that I can write code in the main program to deal with this, but I would prefer that these be encapsulated within the servo functionality.

So, I modified the servo libraries so that each servo has 2 additional properties, reverse and trim, and two additional methods, setreverse() and settrim().

This will (in my understanding) make each servo about 3 bytes longer, and each servowrite() call a few instructions longer.

Code included. I tried to include comments with my initials (vjh) describing each change.

I did test this and am getting the results I desire.

Questions I have:

  1. Am I insane for changing the servo library?
  2. Do you see anything I have done that will be a ‘gotcha’?

My setup and loop

#include <math.h>
#include <Servo.h>

void waitforcharacter(int terminationchar);

Servo ServoA;   //initialize
Servo ServoB;   //initialize
Servo ServoC;   //initialize
char buffer[6];
int received;
uint8_t reverse;
int trim;

void setup()
  int i;
  ServoA.attach(7, 700, 2000);
  ServoB.attach(8, 700, 2300);
  ServoC.attach(9, 1100, 2300);

  trim = false;

void loop()


  ServoA.writeMicroseconds(1500);  ServoB.writeMicroseconds(1500);  ServoC.writeMicroseconds(1500);
  Serial.println ("1500");
  waitforcharacter(92); //wait for a backslash to be entered
  ServoA.writeMicroseconds(1700);  ServoB.writeMicroseconds(1700);  ServoC.writeMicroseconds(1700);
  Serial.println ("1700");
  waitforcharacter(92); //wait for a backslash to be entered
  ServoA.writeMicroseconds(1900);  ServoB.writeMicroseconds(1900);  ServoC.writeMicroseconds(1900);
  Serial.println ("1900");
  waitforcharacter(92); //wait for a backslash to be entered
  if (reverse)
    Serial.println ("switching ServoA to be reversed and have a trim of 100");
    Serial.println ("switching ServoA to be normal and have a trim of -100");


My servo.h

  Servo.h - Interrupt driven Servo library for Arduino using 16 bit timers- Version 2

vjh snip comments for forum posting space


#ifndef Servo_h
#define Servo_h

#include <inttypes.h>

#define Servo_VERSION           2      // software version of this library

#define MIN_PULSE_WIDTH       544     // the shortest pulse sent to a servo  
#define MAX_PULSE_WIDTH      2400     // the longest pulse sent to a servo 
#define DEFAULT_PULSE_WIDTH  1500     // default pulse width when servo is attached
#define REFRESH_INTERVAL    20000     // minumim time to refresh servos in microseconds 

#if defined(__AVR_ATmega1280__)
#define MAX_SERVOS             48     // the maximum number of servos  (valid range is from 1 to 48)
#define MAX_SERVOS             12     // this library supports up to 12 on a standard Arduino

#define INVALID_SERVO         255     // flag indicating an invalid servo index

typedef struct  {
  uint8_t nbr        :6 ;             // a pin number from 0 to 63
  uint8_t isActive   :1 ;             // true if this channel is enabled, pin not pulsed if false 
} ServoPin_t   ;  

typedef struct {
  ServoPin_t Pin;
  unsigned int ticks;
  uint8_t reverse;                    // vjh property added to handle reversing flag 0 is normal and 1 is reversed
  int trim;                           // vjh property added to handle trim
} servo_t;

class Servo
  uint8_t attach(int pin);           // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure
  uint8_t attach(int pin, int min, int max); // as above but also sets min and max values for writes. 
  void detach();
  void write(int value);             // if value is < 200 its treated as an angle, otherwise as pulse width in microseconds 
  void writeMicroseconds(int value); // Write pulse width in microseconds 
  int read();                        // returns current pulse width as an angle between 0 and 180 degrees
  int readMicroseconds();            // returns current pulse width in microseconds for this servo (was read_us() in first release)
  bool attached();                   // return true if this servo is attached, otherwise false 
  void setreverse(int value);        // vjh add method to handle servo reverse flag 0 is normal and 1 is reversed
  void settrim(int value);           // vjh add method to handle servo trim
   uint8_t servoIndex;               // index into the channel data for this servo
   int8_t min;                       // minimum is this value times 4 added to MIN_PULSE_WIDTH    
   int8_t max;                       // maximum is this value times 4 added to MAX_PULSE_WIDTH   


Mp servo.cpp (relevant pieces only, the file is too large for a single post)

uint8_t Servo::attach(int pin, int min, int max)
  if(this->servoIndex < MAX_SERVOS ) {
    pinMode( pin, OUTPUT) ;                                   // set servo pin to output
    servos[this->servoIndex].Pin.nbr = pin;  
    // todo min/max check: abs(min - MIN_PULSE_WIDTH) /4 < 128 
    this->min  = (MIN_PULSE_WIDTH - min)/4; //resolution of min/max is 4 uS
    this->max  = (MAX_PULSE_WIDTH - max)/4; 
      servos[this->servoIndex].reverse = false;  //vjh
      servos[this->servoIndex].trim = 0;     //vjh
    // initialize the timer if it has not already been initialized 
    servoTimer_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
    if(isTimerActive(timer) == false)
    servos[this->servoIndex].Pin.isActive = true;  // this must be set after the check for isTimerActive
  return this->servoIndex ;

void Servo::setreverse(int value)  
{  //vjh add method to set the reverse  0 for normal, 1 for reverse
  servos[this->servoIndex].reverse= value;  

void Servo::settrim(int value)  
{  //vjh add method to set the trim  + or - millis
  servos[this->servoIndex].trim= value;  

void Servo::writeMicroseconds(int value)
  // calculate and store the values for the given channel
  byte channel = this->servoIndex;
  if( (channel >= 0) && (channel < MAX_SERVOS) )   // ensure channel is valid
    if (servos[channel].reverse)
            value = map(value,600,2400,2400,600);   //vjh if reverse flag is set, swap the value
      value += servos[channel].trim;              //vjh add the servo trim to the value
      if( value < SERVO_MIN() )          // ensure pulse width is valid
      value = SERVO_MIN();
    else if( value > SERVO_MAX() )
      value = SERVO_MAX();   
    value = (value-TRIM_DURATION) * TICKS_PER_uS;  // convert to ticks after compensating for interrupt overhead
    uint8_t oldSREG = SREG;
    servos[channel].ticks = value;  
    SREG = oldSREG;   

You could always start a 2nd or 3rd post to get the full code posted. There is no problem modifying the libraries. Keep doing some testing and make sure everything works out fine. Then post your full code in the exhibition and see about getting it uploaded to the playground.

Certainly nothing wrong with modifying a library to make it better suit your needs. And thanks for sharing your efforts.

Although memory may not be a problem with your application, your approach does double the RAM footprint (albeit from only 3 bytes to 6 bytes per channel). Here are some tips that provide the additional similar functionality without using any more memory than the standard library.

You can add the reverse flag into the ServoPin_t structure. This is a bitfield that has one bit unused that can hold the reverse flag so no additional memory is needed :

typedef struct { uint8_t nbr :6 ; // a pin number from 0 to 63 uint8_t isActive :1 ; // true if this channel is enabled, pin not pulsed if false uint8_t reverse :1 ; // true if this channel is reversed } ServoPin_t ;

Here is an example of how this can be used

void Servo::setReverse(uin8_t value) { value is boolean, false is normal, true is for reverse servos[this->servoIndex].Pin.reverse= value; }

you can handle trim without using an additional integer by modifying the min and max values. To increase trim, add the same trim value to min and max. To decrease trim , subtract the same values. This is a little complicated because min and max are actually offset values from the minimum and maximum values. This is the general idea:

void Servo::setTrim(int value) { // method to set the trim + or - millis this->min += value/4; //resolution of min/max is 4 uS this->max += value/4; this->writeMicroseconds(this->readMicroseconds()); // update the current setting using the new min/max values }

You modified the way writeMicroseconds works that will break if used on a board with a clock that is not evenly divisible by 8. If you use the trim approach suggested above the standard library code should handle the trim without change.

mem, I love the idea of putting the reverse flag into the unused bit in the ServoPin_t structure. I will change my version, test it, and post it back here.

I have to admit that I do not follow your suggestion on the trim. And I also admit that part of my failure is with clock speed/8 and value/4. There is a whole area of prescale-ticks that confuses me. Why is the resolution of the min/max 4uS?

Continuing my discussion, with the awareness that some or all of my conclusions might be invalidated by my ignorance noted above, I do not see min and max being a solution to my trim needs. I want the trim to handle accurate centering of the servo. If I write 1500 to a servo, I want it to be straight. Due to how the servo horn is splined on to the servo shaft, placement of the horn is always off by some amount. Trim allows me to adjust for that attachment error.

min and max are end point adjustments. These limit the extreme values sent to a servo to prevent writing a number that falls outside of the workable range of the servo electronics. It also can be used to limit servo travel preventing a physical collision with the constructs attached to the servo. min and max do not affect the positioning of a center value like 1500.

I guess I do not see how this implementation of trim can break users with different clock speeds. If I set the trim value to 3, and write 1500 to writeMicroseconds, then the value is changed to 1503, and then it is further massaged by the TRIM_DURATION (adjusting for the overhead) and TICKS_PER_uS. In my run through of the code, I see this as equivalent to writing 1503 to the original unmodified writeMicroseconds.

I am completely willing to accept that I am wrong, or that I have a poor working knowledge of the timing internals of the relevant functions.

I would GREATLY enjoy any educational information you could share about this. I am trying to understand this. I do not see why the min and max are divided and multiplied by 4. Prescale is lost on me.

Oh, and I guess that however I implement this, I should put the reversing logic into readMicroseconds.

As with most software problems, there are many solutions. I hope the following helps

Lets start with a clarification of the endpoint adjustments – min and max. 4 microseconds was chosen because this is a good balance between providing adequate precision and range. Each 4 microsecond step in value for min or max gives a change of 0.2% of the pulse width range. Hobby servos typically are only accurate to around 1 degree, the best hobby servos I have seen are not more accurate than half a degree. 10 microseconds per degree (1000 microseconds for 100 degrees) is a typical value for a servo so steps smaller than 4 microsconds would be beyond the accuracy of the servo to respond. 4 microsecond steps give a range of 1024 microseconds for min and max. (min can range from 544 to 1568, max from 1376 to 2400) . If integers were used for min and max instead of bytes then twice the RAM would be needed for no functional benefit.

Now to the trim. I don't think writeMicroseconds is a good place to add a fudge factor. The function should do what it says – write a pulse of the given number of microseconds. A better place for trim is in the function that writes degrees. Here it would be logical to add a fudge factor so when writing 90 degrees the servo horn is exactly at 90 degrees. But if you don't like using degrees for servos, I suggest creating another function that takes a parameter to position the servo to a percentage of its travel. If you want maximum precision then the parameter scale could be in tenths of a percent, so 1000 is maximum rotation and 500 is half rotation.

Thank you for the description of min and max. I might be able to use this type of trickery on precision/range with my trim feature, and save another byte on the memory footprint.

I do not much care to use the degrees version of the servo write. I do not want to give up accuracy. I know that writing a servo pulse that differs by a small amount from the previous pulse might not result in any servo movement. I want that resolution limit to be on the servo mechanics and electronics, not created in my code. If I make progressive 1uS changes to the pulse, then the servo will move when it is able to detect the change. Going to degrees means I would be giving the equivalent of 10uS steps in my servo commands.

I could move my logic out of the writeMicroseconds and put it into a function that wraps that. It could apply the trim and reversing logic there. How much overhead is there in a function call? Enough to justify copying writeMicroseconds to a new function that duplicates all the writeMicroseconds logic and also uses my new logic? I am not horribly CPU bound (tests indicate 33% utilization so far), and this routine will be called about 900 times per second. Whether I have my logic in writeMicroseconds or in another wrapper function, the end result will be that a 1500 command will create a 1500uS pulse if none of my additional features are used.

There is actually another feature that I want to add to the servo; scaling. I have 30 of the same servos that I ordered at the same time. I tested each, and there is deviation in the servo movement per uS. I used a protractor to measure difference in servo movement with two pulses that differed by 1600uS. I expected 160 degrees of servo movement. What I got was 164 degrees through 174 degrees. I am also using 6 higher torque servos. They gave me 160 degrees. Since I want a predictable amount of movement for a given command, I want to assign a scaling factor to each servo. I can use the same map command I use to reverse the servo, but just modify the range by the scaling factor, and get the results I am looking for. For now, I will likely hard code the trims, reverses and scales in an initialization routine, but eventually I want a utility to adjust those factors and write them to the eeprom for use on each startup.

I will use the input given so far (thank you so much for that, btw) as well as any more input that is contributed later, and rework my changes to the servo library. I will post what I have after I have tested it.

Thanks for all the help, and keep it coming. :D

I understand that you don't want to use degrees but the most accurate hobby servos I have seen are not accurate enough to provide any real benefit if driven using higher precision than around 4 microseconds. Try a test with your best servos. See how many 1us steps it takes to move the servo. Also try writing any given number of microseconds forwards, wait, and the same backwards and see how accurate the return position is. Try the same test using steps of 4 us and see if it's any less accurate.

I would be interested to hear if using a precision of 1 microsecond offers any benefit over using say 4 microseconds.

The overhead of a wrapper function called 900 times a second is insignificant.

BTW, what servos are you using?

Oh, I completely agree that 1uS is below the resolution level of the servo.

But as I write 1uS increases to the servo starting at say, 1500, the servo might move at 1503 and 1507 and again at 1511. I would hate to be restricted to only writing the equivalent of 1500, 1510, 1520 and so on that degree math would limit me to. I would give up steps of resolution that the servo does read, even if I do not know the exact values where those steps lay. I just calculate the best value I can and let the servo choose its closest interpretation of that value.

The servos are standard Hitec hs311 and digital Spektrum ds821

Insignificant times for a wrapper function. Good to hear. I will not be afraid to use them.

Now, about the trim (and the soon to be added scale) attribute. Currently an int with the bigger footprint. If I use similar logic to the min and max, where I divide by 4 to store, and multiply by 4 to use, I get a larger range, at the cost of precision.

Since I want to use numbers with a + and – range, but store in a single byte, can I also add 128 before I store, and then subtract 128 when I read the value from memory?

If I use both methods, I think I can use a variable with a +or- 255 range, and store it in a single byte (with the mentioned loss of precision)

Make sense? Code example would probably help illustrate what I am asking about, but I am at work and have no access to the Arduino.

um, hrm...

Or is there a type declaration for an int8_t? ;)

I actually looked for this, and could not find a definition. uint8_t and int8_t. Where are they defined? The reference page mentions nothing about them.