ServoTimer2 - drives up to 8 servos

Forum 2005-2010 (read only) Hardware Interfacing
1

The following code was posted in a giant thread called 3 axis auto stabilized platform, but it may be difficult to find in the hundreds of posts there. I will put the code in the playground with a more complete writeup when I can find the time, but for now here is the code. Please do post your comments and suggestons – as well as information on the projects you build using the code.

Have fun!

The library uses Timer2 to drive up to 8 servos using interrupts so no refresh activity is required from within the sketch.
The usage and method naming is similar to the Arduino software servo library Arduino Playground - Servo
except that pulse widths are in microseconds not degrees.

A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method.
The servo is pulsed in the background to the value most recently written using the write() method

Note that analogWrite of PWM on pins 3 and 11 is disabled when the first servo is attached

The methods are:

ServoTimer2 - Class for manipulating servo motors connected to Arduino pins.

attach(pin ) - Attaches a servo motor to an i/o pin.
attach(pin, min, max ) - Attaches to a pin setting min and max values in microseconds
default min is 544, max is 2400

write() - Sets the servo pulse width in microseconds.

read() - Gets the last written servo pulse width in microseconds.

attached() - Returns true if there is a servo attached.

detach() - Stops an attached servos from pulsing its i/o pin.

The library takes about 824 bytes of program memory and 32+(1*servos) bytes of SRAM.
The pulse width timing is accurate to within 1%

p.s. here is my list of potential enhancements:

  • add method to read and write angle (in as an alternative to microseconds) axis
  • A variant that uses timer1 – offering greater precision and the potential to drive more than eight servos.
2

The following two files should be placed in a directory called ServoTimer2 in the hardware/Libraries directory

ServoTimer2.h

/*
  ServoTimer2.h - Interrupt driven Servo library for Arduino using 
/* 
  This library uses Timer2 to drive up to 8 servos using interrupts so no refresh activity is required from within the sketch. 
  The usage and method naming is similar to the Arduino software servo library http://www.arduino.cc/playground/ComponentLib/Servo
  except that pulse widths are in microseconds.
 
  
  A servo is activated by creating an instance of the Servo class passing the desired pin to the attach() method.
  The servo is pulsed in the background to the value most recently written using the write() method

  Note that analogWrite of PWM on pins 3 and 11 is disabled when the first servo is attached

  The methods are:

   ServoTimer2 - Class for manipulating servo motors connected to Arduino pins.

   attach(pin )  - Attaches a servo motor to an i/o pin.
   attach(pin, min, max  ) - Attaches to a pin setting min and max values in microseconds
    default min is 544, max is 2400  
 
   write()     - Sets the servo pulse width in microseconds.

   read()      - Gets the last written servo pulse width in microseconds. 

   attached()  - Returns true if there is a servo attached. 

   detach()    - Stops an attached servos from pulsing its i/o pin. 
  

The library takes about 824 bytes of program memory and 32+(1*servos) bytes of SRAM. 
The pulse width timing is accurate to within 1%

 */

// ensure this library description is only included once
#ifndef ServoTimer2_h
#define ServoTimer2_h

#include <inttypes.h>
typedef uint8_t boolean;
typedef uint8_t byte;

#define MIN_PULSE_WIDTH       750        // the shortest pulse sent to a servo  
#define MAX_PULSE_WIDTH      2250        // the longest pulse sent to a servo 
#define DEFAULT_PULSE_WIDTH  1500        // default pulse width when servo is attached
#define FRAME_SYNC_PERIOD   20000        // total frame duration in microseconds 
#define NBR_CHANNELS 8                   // the maximum number of channels, don't change this 

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

typedef struct {
  ServoPin_t Pin;
  byte counter;
  byte remainder;
}  servo_t;

class ServoTimer2
{
  public:
      // constructor:
      ServoTimer2();

      uint8_t attach(int);     // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure
                               // the attached servo is pulsed with the current pulse width value, (see the write method) 
      uint8_t attach(int, int, int); // as above but also sets min and max values for writes. 
    void detach();
    void write(int);         // store the pulse width in microseconds (between MIN_PULSE_WIDTH and MAX_PULSE_WIDTH)for this channel
    int read();                    // returns current pulse width in microseconds for this servo
      boolean attached();      // return true if this servo is attached 
 private:
       uint8_t chanIndex;      // index into the channel data for this servo

};


// the following ServoArrayT2 class is not implemented in the first version of this library
class ServoArrayT2
{
  public:
      // constructor:
      ServoArrayT2();

      uint8_t attach(int);     // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure
                               // channels are assigned consecutively starting from 1
                               // the attached servo is pulsed with the current pulse width value, (see the write method) 
    void detach(int);        // detach the servo on the given channel
      void write(int,int);     // store the pulse width in microseconds (between MIN_PULSE_WIDTH and MAX_PULSE_WIDTH)for the given channel
    int read(int);                    // returns current pulse width in microseconds for the given channel
      boolean attached(int);   // return true if the servo on the given channel is attached 
 private:
       uint8_t chanIndex;      // index into the channel data for this servo

};

#endif
3

servoTimer2.cpp

/* ServoTimer2.cpp*/
extern "C" {
  // AVR LibC Includes
  #include <inttypes.h>
  #include <avr/interrupt.h>
  #include "WConstants.h"
}
#include <wiring.h>
#include "ServoTimer2.h"
static void initISR();   
static void writeChan(uint8_t chan, int pulsewidth);

#define FRAME_SYNC_INDEX   0             // frame sync delay is the first entry in the channel array
#define FRAME_SYNC_PERIOD  20000         // total frame duration in microseconds 
#define FRAME_SYNC_DELAY   ((FRAME_SYNC_PERIOD - ( NBR_CHANNELS * DEFAULT_PULSE_WIDTH))/ 128) // number of iterations of the ISR to get the desired frame rate
#define DELAY_ADJUST       8             // number of microseconds of calculation overhead to be subtracted from pulse timings   

static servo_t servos[NBR_CHANNELS+1];    // static array holding servo data for all channels

static volatile uint8_t Channel;   // counter holding the channel being pulsed
static volatile uint8_t ISRCount;  // iteration counter used in the interrupt routines;
uint8_t ChannelCount = 0;          // counter holding the number of attached channels
static boolean isStarted = false;  // flag to indicate if the ISR has been initialised

ISR (TIMER2_OVF_vect)
{ 
  ++ISRCount; // increment the overlflow counter
  if (ISRCount ==  servos[Channel].counter ) // are we on the final iteration for this channel
  {
      TCNT2 =  servos[Channel].remainder;   // yes, set count for overflow after remainder ticks
  }  
  else if(ISRCount >  servos[Channel].counter)  
  {
      // we have finished timing the channel so pulse it low and move on
      if(servos[Channel].Pin.isActive == true)           // check if activated
          digitalWrite( servos[Channel].Pin.nbr,LOW); // pulse this channel low if active   

        Channel++;    // increment to the next channel
      ISRCount = 0; // reset the isr iteration counter 
      TCNT2 = 0;    // reset the clock counter register
      if( (Channel != FRAME_SYNC_INDEX) && (Channel <= NBR_CHANNELS) ){           // check if we need to pulse this channel    
          if(servos[Channel].Pin.isActive == true)         // check if activated
             digitalWrite( servos[Channel].Pin.nbr,HIGH); // its an active channel so pulse it high   
      }
      else if(Channel > NBR_CHANNELS){ 
         Channel = 0; // all done so start over               
      } 
   }  
}

ServoTimer2::ServoTimer2()
{
   if( ChannelCount < NBR_CHANNELS)  
      this->chanIndex = ++ChannelCount;  // assign a channel number to this instance
   else
      this->chanIndex = 0;  // todo      // too many channels, assigning 0 inhibits this instance from functioning 

uint8_t ServoTimer2::attach(int pin)
{
      if( isStarted == false)
       initISR();    
      if(this->chanIndex > 0)  
      {
       //debug("attaching chan = ", chanIndex);
       pinMode( pin, OUTPUT) ;  // set servo pin to output
       servos[this->chanIndex].Pin.nbr = pin;  
       servos[this->chanIndex].Pin.isActive = true;  
      } 
      return this->chanIndex ;
}

void ServoTimer2::detach()  
{
    servos[this->chanIndex].Pin.isActive = false;  
}

void ServoTimer2::write(int pulsewidth)
{      
   writeChan(this->chanIndex, pulsewidth); // call the static function to store the data for this servo          
}

int ServoTimer2::read()
{
  unsigned int pulsewidth;
   if( this->chanIndex > 0)
      pulsewidth =  servos[this->chanIndex].counter * 128 + ((255 - servos[this->chanIndex].remainder) / 2) + DELAY_ADJUST ;
   else 
       pulsewidth  = 0;
   return pulsewidth;   
}
 
boolean ServoTimer2::attached()
{
    return servos[this->chanIndex].Pin.isActive ;
}

static void writeChan(uint8_t chan, int pulsewidth)
{
   // calculate and store the values for the given channel
   if( (chan > 0) && (chan <= NBR_CHANNELS) )   // ensure channel is valid
   { 
      if( pulsewidth < MIN_PULSE_WIDTH )                // ensure pulse width is valid
          pulsewidth = MIN_PULSE_WIDTH;
      else if( pulsewidth > MAX_PULSE_WIDTH )
          pulsewidth = MAX_PULSE_WIDTH;       
      
        pulsewidth -=DELAY_ADJUST;                   // subtract the time it takes to process the start and end pulses (mostly from digitalWrite) 
      servos[chan].counter = pulsewidth / 128;      
      servos[chan].remainder = 255 - (2 * (pulsewidth - ( servos[chan].counter * 128)));  // the number of 0.5us ticks for timer overflow         
   }
}

static void initISR()
{   
      for(uint8_t i=1; i <= NBR_CHANNELS; i++) {  // channels start from 1    
         writeChan(i, DEFAULT_PULSE_WIDTH);  // store default values          
      }
      servos[FRAME_SYNC_INDEX].counter = FRAME_SYNC_DELAY;   // store the frame sync period       

      Channel = 0;  // clear the channel index  
      ISRCount = 0;  // clear the value of the ISR counter;
      
      /* setup for timer 2 */
      TIMSK2 = 0;  // disable interrupts 
      TCCR2A = 0;  // normal counting mode 
      TCCR2B = _BV(CS21); // set prescaler of 8 
      TCNT2 = 0;     // clear the timer2 count 
      TIFR2 = _BV(TOV2);  // clear pending interrupts; 
      TIMSK2 =  _BV(TOIE2) ; // enable the overflow interrupt        
        
      isStarted = true;  // flag to indicate this initialisation code has been executed
}
4

ServoTimer2 test sketch

// this sketch cycles three servos at different rates 

#include <ServoTimer2.h>  // the servo library

// define the pins for the servos
#define rollPin  2
#define pitchPin 3
#define yawPin   4

ServoTimer2 servoRoll;    // declare variables for up to eight servos
ServoTimer2 servoPitch; 
ServoTimer2 servoYaw; 
 
void setup() {
  servoRoll.attach(rollPin);     // attach a pin to the servos and they will start pulsing
  servoPitch.attach(pitchPin); 
  servoYaw.attach(yawPin); 
}


// this function just increments a value until it reaches a maximum 
int incPulse(int val, int inc){
   if( val + inc  > 2000 )
      return 1000 ;
   else
       return val + inc;  
}

void loop()
{ 
 int val;
  
   val = incPulse( servoRoll.read(), 1);
   servoRoll.write(val);

   val =  incPulse( servoPitch.read(), 2);
   servoPitch.write(val);
   
   val = incPulse(servoYaw.read(), 4);
   servoYaw.write(val);
   
   delay(10);   
}
5

Helo,
I get these errors using this library

ServoTimer2.cpp: In constructor 'ServoTimer2::ServoTimer2()':
ServoTimer2.cpp:59: error: a function-definition is not allowed here before '{' token
ServoTimer2.cpp:73: error: a function-definition is not allowed here before '{' token
ServoTimer2.cpp:78: error: a function-definition is not allowed here before '{' token
ServoTimer2.cpp:83: error: a function-definition is not allowed here before '{' token
ServoTimer2.cpp:93: error: a function-definition is not allowed here before '{' token
ServoTimer2.cpp:98: error: a function-definition is not allowed here before '{' token
ServoTimer2.cpp:114: error: a function-definition is not allowed here before '{' token
ServoTimer2.cpp:132: error: expected `}' at end of input
ServoTimer2.cpp: At global scope:
ServoTimer2.cpp:10: warning: 'void initISR()' declared 'static' but never defined
ServoTimer2.cpp:11: warning: 'void writeChan(uint8_t, int)' declared 'static' but never defined
ServoTimer2.cpp:23: warning: 'isStarted' defined but not used

ServoTimer2::ServoTimer2()
{
   if( ChannelCount < NBR_CHANNELS)
      this->chanIndex = ++ChannelCount;  // assign a channel number to this instance
   else
      this->chanIndex = 0;  // todo      // too many channels, assigning 0 inhibits this instance from functioning

uint8_t ServoTimer2::attach(int pin)
{

How do I solve this?

Erik

6

Hi Erik, it looks like a bracket got dropped off that post, the fragment should look like this:

ServoTimer2::ServoTimer2()
{
   if( ChannelCount < NBR_CHANNELS)  
      this->chanIndex = ++ChannelCount;  // assign a channel number to this instance
   else
      this->chanIndex = 0;  // todo      // too many channels, assigning 0 inhibits this instance from functioning 
} // <- put the closing bracket here

Good luck!

7

Hey MEM,

One question:

Is the ServoTimer2 library compiling on 0012 now? I had it working on 0011 but would not run on 0012.

I just corrected the .cpp file also.

Can ST2 be used with ServoDecode to command the servos?

Jim

8

Yes, the code above (with the missing bracket added) should compile under 0012. If its not for you, let me know what error messages you are getting.

Yes, ServoTimer2 works with ServoDecode.

You may be able to find some examples burried in this giant thread:http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1204020386

9

Hey MEM,

Yes sir, I am glad to report that ST2 does in fact compile and run on 0012 now.. The missing } was the issue before. The following code I got from the Giant thread and it compiles fine but keeps getting the message that there is no decoder input. If I run the ServoDecode test code only, the test code compiles and runs just fine. This co-mingled code seems to stumble on the capturing a valid run state.

Code:

// ServoDecodeTest
#include <ServoDecode.h>
#include <ServoTimer2.h> // the servo library

// define the servo pins
#define servo1Pin 2 // the pin the servo is attached to
#define servo2Pin 3

ServoTimer2 servo1; // declare variables for the servos
ServoTimer2 servo2;

void setup()
{
Serial.begin(38400);
servo1.attach(servo1Pin);
servo2.attach(servo2Pin);
ServoDecode.begin();
}

void loop()
{
int pulsewidth; //[MAX_CHANNELS];

if( ServoDecode.getState()!= READY_state) {
Serial.println("The decoder is not ready");
}
else {
// decoder is ready, do somthing with the channel pulse widths
if( ServoDecode.GetChannelPulseWidth(1) > 1500){ // run script 1 if pulse width greater than 1500 ;
int argumentA = (2250 - ServoDecode.GetChannelPulseWidth(2)) / 200; // some values proportional to stick position
boolean argumentB = ServoDecode.GetChannelPulseWidth(2) < 1500; // true if channel 2 less than than 1500
runScript1(argumentA, argumentB);
}
}
delay(100); // update 10 times a second
}

void runScript1(int argumentA, boolean argumentB){
// do something here to control servo...
servo1.write(2000);
delay(5000); // note that all servo are pulsed by the interrupt handler with their current value every 20ms
if(argumentA)
servo2.write(2000);
delay(argumentB * 10); //delay a time determined by the transmitter stick position
servo1.write(1000);
servo2.write(1000);
}

I understand the logic flow, but the mechanics of coding seem to get convoluted when I try to write some code. The code logic is:

Get receiver output on Arduino pin 8
Decode the PPM into channel pulsewidths
Convert the pulsewidths to servo friendly values
do this as rapidly as possible( using interrupts if necessary)
pass the pulses to the servo outpins on manual :-/
pass control to Arduino as option :-/

Thanks in advance :slight_smile:

10

The values from ServoDecode.GetChannelPulseWidth should be servo friendly – they would be the same value as the servo would get if it was plugged directly into the channel.

Perhaps try the following logic to get you started:

In the else part of your loop:
int chan1 = ServoDecode.GetChannelPulseWidth(1);
int chan2 = ServoDecode.GetChannelPulseWidth(2);
Serial.println(chan1);
Serial.println(chan2);
Servo1.write(chan1);
Servo2.write(chan2);

Once you confirm that works, you can start manipulating the values you get from ServoDecode before sending them to the servos.

11

Thanks, Mem. I'm going to try this library and code on my robot arm as soon as I free up some pins (I'm using switches to control three of the servos - once I get my micro joysticks I can reduce the control lines to one per axis).

12

Good afternoon MEM,

Thank you once again for your assistance.

The code now runs as expected and better in fact, than the attempts I made using SoftwareServo. With this code you provided and my sketch, the servos move now just as if they were connected directly to the receiver with NO JITTER at the center throw position. Next step is to create some mixing schemes to move the servos as needed and then to use Gyros to assist with the servo placements in assist mode. :smiley: :smiley:

Thank you once again.

Jim

13

Hi Jim, good to hear that you are making progress :slight_smile:

Looking forward to hearing more about your project when you have the servo mixing coded.

14

Hi Guys,

Im currently trying to get this program reading a potentiometer value. Ive mashed it together with the following program:

// Controlling a servo position using a potentiometer (variable resistor)
// by Michal Rinott <people.interaction -ivrea.it /m.rinott>

#include <Servo.h>

Servo myservo1; // create servo object to control a servo
Servo myservo2; // create servo object to control a servo

int potpin0 = 0; // analog pin used to connect the potentiometer
int potpin1 = 1; // analog pin used to connect the potentiometer

int val1; // variable to read the value from the analog pin
int val2; // variable to read the value from the analog pin

void setup()
{
Serial.begin(9600);
myservo1.attach(9); // attaches the servo on pin 9 to the servo object
myservo2.attach(10); // attaches the servo on pin 9 to the servo object
}

void loop()
{
int test = 1;
val1 = analogRead(potpin0); // reads the value of the potentiometer (value between 0 and 1023)
val2 = analogRead(potpin1); // reads the value of the potentiometer (value between 0 and 1023)

val1 = map(val1, 0, 1023, 27, 160); // scale it to use it with the servo (value between 0 and 180)
val2 = map(val2, 0, 1023, 27, 160); // scale it to use it with the servo (value between 0 and 180)
myservo1.write(val1); // sets the servo position according to the scaled value
myservo2.write(val2); // sets the servo position according to the scaled value

if (test)
{
Serial.print(val1);
Serial.print(" ");
Serial.print(val2);
Serial.println();
}
delay(15); // waits for the servo to get there
}

And the result is:

// this sketch no longer cycles three servos at different rates

#include <ServoTimer2.h> // the servo library

// define the pins for the servos
#define rollPin 2
#define pitchPin 3
#define yawPin 4

ServoTimer2 servoRoll; // declare variables for up to eight servos
ServoTimer2 servoPitch;
ServoTimer2 servoYaw;

int potpin0 = 0; // analog pin used to connect the potentiometer
int potpin1 = 1; // analog pin used to connect the potentiometer
int potpin2 = 2; // analog pin used to connect the potentiometer

int val0; // variable to read the value from the analog pin
int val1; // variable to read the value from the analog pin
int val2; // variable to read the value from the analog pin

void setup() {
servoRoll.attach(rollPin); // attach a pin to the servos and they will start pulsing
servoPitch.attach(pitchPin);
servoYaw.attach(yawPin);
}

void loop()
{
int test = 1;
val0 = analogRead(potpin0); // reads the value of the potentiometer (value between 0 and 1023)
val1 = analogRead(potpin1); // reads the value of the potentiometer (value between 0 and 1023)
val2 = analogRead(potpin2); // reads the value of the potentiometer (value between 0 and 1023)

val0 = map(val0, 0, 1023, 27, 160); // scale it to use it with the servo (value between 0 and 180)
val1 = map(val1, 0, 1023, 27, 160); // scale it to use it with the servo (value between 0 and 180)
val2 = map(val2, 0, 1023, 27, 160); // scale it to use it with the servo (value between 0 and 180)

servoRoll.write(val0); // sets the servo position according to the scaled value
servoPitch.write(val1); // sets the servo position according to the scaled value
servoYaw.write(val2);

if (test) //If int test = 1 will display the value of the pots
{
Serial.print(val0);
Serial.println();
Serial.print(val1);
Serial.print(" ");
Serial.print(val2);
Serial.println();
}
delay(15); // waits for the servo to get there
}

It reads the pot value but doesnt output to the servo so im thinking it doesnt work in the same way as the other program.

Im a programming novice so any help would be greatly appreciated at this point :slight_smile:

15

The input to ServoTimer2 is pulse width in microseconds, not degrees.

Replace the map functions in your sketch with the following code:

#define MIN_PULSE  750
#define MAX_PULSE  2250

 val0 = map(val0, 0, 1023, MIN_PULSE , MAX_PULSE );     // scale it to use it with the servo  
 val1 = map(val1, 0, 1023, MIN_PULSE , MAX_PULSE );     // scale it to use it with the servo  
 val2 = map(val2, 0, 1023, MIN_PULSE , MAX_PULSE );     // scale it to use it with the servo
16

Success! Thankyou Mem :slight_smile:

I will post the full code when ive filled it out.

17

Hi mem,
I 'd like to use this library for driving my version of PanoDuino http://phil007.com/readme/?cat=8

In the function writeChan i notice that the actual precision is 0.5us

"servos[chan].counter = pulsewidth / 128;
servos[chan].remainder = 255 - (2 * (pulsewidth - ( servos[chan].counter * 128))); // the number of 0.5us ticks for timer overflow"

Would it be possible to modify the library so that the pulsewidth can be given in 0.5us? like 2000 to 4000 or even a higher precision.

Also the "attach(pin, min, max ) - Attaches to a pin setting min and max values in microseconds" is not implemented YET, are you planning to write this?

18

Hi erik,

Although the timer itself has a precision of 0.5 microseconds, the overall precision will be less than that because of the overhead of the interrupt handling. So in reality I don't think you would get any greater accuracy if the pulse width was given to half a microsecond. Also, I don't think that even the best hobby servos have an accuracy of anywhere near 0.1% ( 0.5 microseconds represents less than 0.1 % the typical servo range) so no real world performance would be gained. It would make the marketing department happpy though (if I had a marketng department) :wink:

That attach method is on my todo list – sadly I have been able to get around to it yet.

19

Right mem,
the servo errors usually are pretty large and mostly due to the quality of the potmeter, especially the linearity, in a normal situation this is not noticed.
My servos are Hitec HS785HB and are actually winches used for sailing boat models and the rotate 1080 degrees (3.5 rotations) instead of the usual 90 degrees, this with standard 1000-2000us, with 592-2688 i get over 8 rotations.
this later reduced by 3 or 5 with the extra gear.
i can see almost every change by 1 us.
the pitch servo lifts the camera only from 0-180 degrees by supplying 1234-1792us, a difference of 558us, resulting in 0.32 degrees.
i would like to get a 1 degree but with right now i can be of by 0.16 degrees, with 0.5us precision this would only by a very acceptable 0,08 degrees.
the range of the yaw servo is 360 degrees so the error is doubled.

I understand that the precision is more than good enough in standard applications.
Could you point me out where modifications are needed in order to have the 0.5us drive?
erik

20

You could experiment by trying the modified code

// modifed writeChan to take pulsewidth in 0.5 microseconds units (2000 - 4000 gives widths of 1000us to 2000us)
static void writeChan(uint8_t chan, int pulsewidth){
   if( (chan > 0) && (chan <= NBR_CHANNELS) )   // ensure channel is valid
   { 
      if( pulsewidth < 2 * MIN_PULSE_WIDTH )                // ensure pulse width is valid
          pulsewidth = 2 * MIN_PULSE_WIDTH;
      else if( pulsewidth > 2 * MAX_PULSE_WIDTH )
          pulsewidth = 2 * MAX_PULSE_WIDTH;       
      
        pulsewidth -= 2 * DELAY_ADJUST;                   // subtract the time it takes to process the start and end pulses (mostly from digitalWrite) 
      servos[chan].counter = pulsewidth / 256;      
      servos[chan].remainder = 255 - ( (pulsewidth - ( servos[chan].counter * 256)));  // the number of 0.5us ticks for timer overflow      
   }

I have not compiled or run the code so it may need some work. My guess is that the interrupt overheads mentioned in my earlier post will not actually give you the accuracy you want. If you do decide you like the 0.5us pulsewidths you will want to change the read function as well.

If you need more discussion on this to get it going - start a new thread so users of the standard library don't get sidetracked.