Ich hätt' gesagt, das Konzept ist verkorkst. Den 16 Bit Timer1 auf 8 Bit PWM zu kastrieren und dann mit dem 8 Bit Timer2 zu versuchen über Pulseout eine Servosteuerung hinzumurksen ist suboptimal.
Man hätte Timer2 für die PWM nehmen können, um mit Timer1 im Modus 14 oder 15 eine völlig auf Hardware basierende Servoansteuerung ohne jedes Gewackel zu bauen. Auch 'ne ISR kann man dranpappen, wenn man 'nen Zeitgeber braucht.
ICR1 oder OCR1A muss als Top verwendet werden, bei 39999 gibt's 20ms, das Servo kann dann mit Werten von 2000-4000 für Compare1A oder ..B von 1-2ms eingestellt werden.