Control multiple Servo using avr

I want to try to control about 20 servos ( for a robot ) using avr with timer1 by directly toggling the output. This is what I have been able to come up till now. But when I try to add the last two servos , all the servos fail. I am assuming this is a timing issue. But I do not know how to correct this . I searched online , but could not find any specific answers.

If any one could point to a good source where they talk about controlling multiple servos it would be very helpfull

int main(void)
{
    DDRB = 0xFF;
    DDRA = 0xFF;

    TCCR1A |= 1<<WGM11; 
    TCCR1B |= 1<<WGM12 | 1<<WGM13 | 1<<CS10;
    TIMSK |= 1 << OCIE1A;

    ICR1 = 19999;  // Clear on reaching this point. 
    uint16_t count = MIN_VAL;
    uint16_t count2 = 2000; 
    uint16_t c_rp = MIN_VAL;
    uint16_t c_rp2 = 1000; 
    uint16_t c_rp3 = MIN_VAL;



    sei();
    while(1)
    {
        // Control PORT B PINS 
        // TCNT1 counts upwards and on reaching count 
        // an then the pin goes low
        // SERVO 1
        if( (  PORTB & (1<<PIN) ) && TCNT1 >= count)
        {
            PORTB &= ~(1<<PIN);
            if (count < MAX_VAL)
                count  += INC_VAL; // CHANGING SERVO POSITION
            else
                count = MIN_VAL; 

        }

        // SERVO 2
        if( (  PORTB & (1<<PIN2) ) && TCNT1 >= count2)
        {
            PORTB &= ~(1<<PIN2); // CONSTANT SERVO POSITION

        }


        // Control PORT A PINS 
         // SERVO 3
        if( (  PORTA & (1<<RP) ) && TCNT1 >= c_rp)
        {
            PORTA &= ~(1<<RP);
            if (c_rp < MAX_VAL)
                c_rp  += INC_VAL;
            else
                c_rp = MIN_VAL; 

        }

          // SERVO 4
        if( (  PORTA & (1<<RP2) ) && TCNT1 >= c_rp2)
        {
            PORTA &= ~(1<<RP2);

        }

 }


ISR (TIMER1_COMPA_vect) // ISR called when TCNT1 equals ICR1
{
    PORTB |= (1<< PIN ) | (1<< PIN2) ;
    PORTA |= (1 << RP ) | (1 << RP2) | (1<< RP3);
}

Solution

I can get at least 12 with this shameless selfpromotion. It might point you in the right direction though: https://github.com/pengumc/servocontroller/blob/ef4d7c69e96375feeab30072e7b7f7975616cc87/src/ServoControllerI2C.c