RC servo AVR - saturation on motor ..

#ifndef GUARD_SERVO_H
#define GUARD_SERVO_H
//-------------------------------------------------------------------
//#include "globaldefs.h"
//-------------------------------------------------------------------
#ifndef SERVO_INIT
#define SERVO_INIT

	#define SERVO_PORT  PORTB
	#define SERVO_DDR   DDRB
	#define N_SERVOS    2
	#define SERVO_FRAME 18000 
	#define SERVO_TIME_DIV (SERVO_FRAME / N_SERVOS)

	#define US2TIMER1(us) ((us) * (uint16_t)(F_CPU / 1E6))
	// Servo times (this is Futaba timing).
	#define SERVO_MIN   1000 // µs
	#define SERVO_MAX   2000 // µs
	#define SERVO_MID   (SERVO_MIN + SERVO_MAX) / 2
	#define TRUE 1
	#warning "No global defs"
	const static uint8_t servoOutMask[N_SERVOS] = {
		0b00000001, // PX0
		0b00000010, // PX1
	};
	
#endif
//-------------------------------------------------------------------

volatile uint16_t servoTime[N_SERVOS];
void servoSet(uint8_t servo, uint16_t time /* µs */);
void servoTest (void);
void ServoInit(void);

//-------------------------------------------------------------------
void ServoInit (void)
{
int i;
for(i = 0; i < N_SERVOS; i++) {
	servoTime[i] = US2TIMER1(SERVO_MID);
    }
SERVO_DDR |= _BV(PIN0);
SERVO_DDR |= _BV(PIN1);

// Setupt a first compare match
OCR1A = TCNT1 + US2TIMER1(100);
// start timer 1 with no prescaler
TCCR1B = (1 << CS10);      
// Enable interrupt
TIMSK |= (1 << OCIE1A);

}
//-------------------------------------------------------------------
void servoSet(uint8_t servo, uint16_t time /* µs */)
{
    uint16_t ticks = US2TIMER1(time);
    cli();
    servoTime[servo] = ticks;
    sei();
}
//-------------------------------------------------------------------
void servoTest (void)
{
uint16_t time;
uint8_t w;
// Test update of servo 0
for(time = SERVO_MIN; time <= SERVO_MAX; time += 50) {
servoSet(0, time);
servoSet(1, time);
for(w = 0; w < 25; w++) {
	_delay_ms(10);
	}
}
}
//-------------------------------------------------------------------
ISR(TIMER1_COMPA_vect)
{
    static uint16_t nextStart;
    static uint8_t servo;
    static short outputHigh = TRUE;
    uint16_t currentTime = OCR1A;
    uint8_t mask = servoOutMask[servo];
   
    if (outputHigh) {
        SERVO_PORT |= mask;
        // Set the end time for the servo pulse
        OCR1A = currentTime + servoTime[servo];
        nextStart = currentTime + US2TIMER1(SERVO_TIME_DIV);
    } else {
        SERVO_PORT &= ~mask;
        if (++servo == N_SERVOS) {
            servo = 0;
        }
        OCR1A = nextStart;
    }
    outputHigh = !outputHigh;
}
//-------------------------------------------------------------------
#endif