Charlieplex 20 LED mit Arduino Uno\AVR; Interrupt langsam

Frohe Ostern allen Foristen,

ich will mein aktuelles Projekt vorstellen. Ich möchte ca. 20 LED 
charlieplexen, mit dem Arduino Uno.
Desweiteren soll der Uno 6 Schalter und 3 Taster auswerten und 6 Servos 
steuern.
Brauchen werde ich das für ein Gleisbildstellwerk auf der Modellbahn.

Im Moment habe ich Probleme die Interruptroutine zu optimieren. Diese 
verbraucht zuviel Rechenzeit. Die
Grenze liegt bei 66666 Hz. Vielleicht stellt dies ja schon das Optimum 
dar? Aber, vielleicht hat ja
jemand Interesse und hilft mir auf die Sprünge mit Tips.

Diese Codestelle gefällt mit nicht sonderlich:

1

    //Column0

2

    if(column0_desired_pwm_value == pwm_tick){

3

      ddrByte &= ~column0;

4

      portByte &= ~column0;

5

    }else if(column_on){            //Dieser Code wiederholt sich in jeder Column

6

      ddrByte |= column0 | controlLine;    //

7

      portByte |= column0;          //

8

      portByte &= ~controlLine;        //

9

    }

Ich habe schon versucht diese Bedingung extra zu schreiben, aber ohne 
Erfolg. Entweder es leuchten
immer alle LED oder es werden andere LED mit angesteuert. Das liegt 
sicher daran, dass diese Bedingung
im else Zweige abhängig von der Vorbedingung ist.

Ich hänge mal den Code, Bilder, Schaltplan und Verdrahtungsplan mit an.
Wenn es mir nicht hilft, vielleicht anderen.