Problem mit der Servoansteuerung

Hallo,

ich habe bereits gestern etwas gepostet, aber dieses Problem konnte ich 
beheben. Ich programmiere, bzw. ich bastel mir gerade ein Programm für 
einen Schülerwettbewerb zusammen.
Meine Gruppe und ich haben uns vorgenommen eine Abblendmaschine zu 
programmieren. Leider happert es bei unseren Kenntnissen.
Der Schülerwettbewerb wird von einer Firma gesponsort, den Namen werde 
ich hier nicht erwähnen.
Diese Firma hat uns bereits einige Befehle im vorraus mitgegeben, damit 
uns das Programmieren nicht so schwer fällt. Die Vertreter meinten, dass 
sie nicht voraussetzen können, dass wir die Hochsprache C können.
Uns wurde gesagt, wir müssen nicht wissen, wie man diese Befehle 
programmiert, aber wir sollten bei der Präsentation erklären können, 
welche Funktion der Befehl hat und schildern können, wie der Befehl 
funktioniert. (Z.B. Waitus -> Zähler, der im 1MHz Takt von 1 bis 1000 
zählt)

Zum Ablauf unseres Programmes: Wir verwenden zwei Lichtsensoren und 
vergleichen diese per Analogwert. Ist z.B. Analogwert 1 größer als 
Analogwert 2, dann sollen die Super-Hellen-LEDs abgeschalten und die 
normale LEDs angeschalten werden. Wir haben uns noch überlegt, diese 
LEDs abzurichten, d.h. wir haben zusaätzlich 2 Servomotoren verwendet. 
Ein Servomotor lenkt die LEDs Richtung Boden ab, der andere zur Seite.

Unser Problem ist, dass wir die Servomotoren nicht ansteuern können. Wir 
haben bisher im Datenblatt kein Pin gefunden, der PWM-Signale ausgeben 
kann. Wir haben schließlich jemanden gefragt, ob man das auch anders 
regeln könnte.
Dieser meinte, sofern wir ihn nicht falsch verstanden haben, wenn man 
ein Port als Ausgang definiert und ihm "1" und "0" Impulse sendet, 
könnte der Servomotor selbst das Tastverhältnis berechnen und die 
Frequenz selbstbestimmen.

Ich habe unser Programm mitgeschickt, damit vielleicht jemand kurz 
drüber schaut und uns ein Tipp gibt, wie man es umschreiben könne, damit 
die Servos doch noch funktionieren.
Ich erwarte, dass niemand uns die Lösung "in den Arsch schiebt", sondern 
einfach nur, dass man ein paar Tipps bekommt wie es funktionieren 
könnte.

Unser Microcontroller hat negative Ein- und Ausgänge.

#include "sfr_r813.h"

void waitus(unsigned int us)

  {
  unsigned int i;
  //us/=2;
  for (i=us;i>0;i--) {
      asm("nop");
    }

  }

void waitms(unsigned int ms)

  {
  unsigned int i;
  //ms *= 10;
  for (i=ms;i>0;i--) {
      waitus(996);    /*996 statt 1000ms um Fehler auszugleichen*/
    }

  }
void init(void)
  {
  prc0 = 1;             /* Protect off */
  cm13 = 1;             /* Xin Xout */
  cm15 = 1;             /* XCIN-XCOUT drive capacity select bit : HIGH 
*/
  cm05 = 0;             /* Xin on */
  cm16 = 0;             /* Main clock = No division mode */
  cm17 = 0;
  cm06 = 0;             /* CM16 and CM17 enable */
  asm("nop");           /* Waiting for stable of oscillation */
  asm("nop");
  asm("nop");
  asm("nop");
  ocd2 = 0;             /* Main clock change */
  prc0 = 0;             /* Protect on */
  }

int analogwert(int chn)
  {
  int adcresult;
  if (chn>=8||chn<0) return 0;
  chn = 7-chn;
  adcon0 = chn;       //channel
  adcon1 = ((1<<3)|(1<<5));  //10-bit Mode
  adst = 1;      //ADC conversion start
  while (adst==1);    //Ende der conversion abwarten
  //adcresult = (ad);

  return ad;
  }

void main(void)
{
init ();

    pd1 = 0x0F;
  pd0 = 0x00 ;



    while (1)
    {
    if ( analogwert(6) >= analogwert(7) )

// Lichtsensoren vergleich (Aussführung,wenn Umgebungssensor größer 
// ist als  Gegenlichtsensor. Es können Werte von 0 - 1023 ausgeben 
werden.)
//analogwert (6) ist der Gegenlichtsensor und analogwert (7) ist der 
Umgebungssensor.
{

    // Abblendlicht
    p1_0 = 0;
    p1_1 = 0;
    // Aufblendlicht
    p1_2 = 1;
    p1_3 = 1;

      // Servo 1 (auf den Boden abrichten)

      /*while (  analogwert(6) <= analogwert(7)  )
      {
      p0_1 = 0;
      waitus(1500);
      p0_1 = 1;
      waitus(18500);
      }
      //Servo 2 (zur Seite abrichten)
      /*  p0_2 = 0;
        waitms(18);
        p0_2 = 1;
        waitms(2);   */
    }
    else
    {

    // Abblendlicht
    p1_0 = 1;
    p1_1 = 1;
    // Aufblendlicht
    p1_2 = 0;
    p1_3 = 0;


      /*while(  analogwert(6) <= analogwert(7)  )
                       // Servo 1 (auf die Straße aufrichten)
      {
      p0_1 = 0;
      waitus(18500);
      p0_1 = 1;
      waitus(1500);
      }
      // Servo 2 (auf die Seite richten)
      /*  p0_2 = 0;
        waitus(18500);
        p0_2 = 1;
        waitus(1500);  */
    }
}
}


Die While-Schleifen habe ich in /*...*/ gesetzt, damit ersichtlich ist, 
wo die Fehler vermutlich liegen könnten.

Wenn uns jemand weiterhelfen könnte, wäre wir sehr erfreut.

MfG Toni