Zur while-Schleife habe ich geschrieben: Wenn du alle 20mSec zuerst dein Servo, dann die Motorsteuerung und dann die Helligkeitsmessung startest, ist das doch schnell genug. Also deinen Timer1 zum starten nehmen.
Code:
while(1)
  {
if(count){
  count=0;
  IR_Servo = IR_Servoposition();
  Motorsteuerung();
  helligkeitsmessung();
 }
  }
return 0;  //wird nie erreicht
}
So würde ich die Aufrufe machen.
Zu den Variablen, entweder global oder als volatile kennzeichnen.
Stromversorgung: Das ist so ein Fall wo man ein Oszi braucht um so Grenzfälle festzustellen, ein normales Meßgerät ist nicht schnell genug. Wenn die Software nichts mehr bringt das Servo mal probeweise mit einem eigenen Regler versorgen.