Irgendwie wird's nicht wirklich besser, ich breche hier mal ab:
Code:
 #include "RP6RobotBaseLib.h"

uint8_t i, servoposition;

void acsStateChanged(void)
{
   setLEDs(statusLEDs.byte | 0b001001);
   if(obstacle_right && obstacle_left)		// beide Seiten?
      rotate(60, RIGHT, 100, true);
   else if(obstacle_left)						// nur links
   {
		setLEDs(statusLEDs.byte & ~1);
		rotate(80, RIGHT, 50, true);
	}
  	else if(obstacle_right)             	// bleibt nur noch rechts übrig
  	{
     	setLEDs(statusLEDs.byte & ~8);
		rotate(80, LEFT, 50, true);
	}
	
	setLEDs(statusLEDs.byte & ~0b001001);
   changeDirection(FWD);                  // weiterfahren
   moveAtSpeed(100,100);
}

void bumpersStateChanged(void)
{
   setLEDs(statusLEDs.byte | 0b001001);
   if(bumper_left || bumper_right)
   {
      changeDirection(BWD);
      move(100, BWD, DIST_MM(200), true);
      rotate(50, RIGHT, 120, true);
   }
	setLEDs(statusLEDs.byte & ~0b001001);
   changeDirection(FWD);
   moveAtSpeed(100,100);
}

int main(void)
{
   initRobotBase();
   setLEDs(0b111111);
   DDRA |= 1;
   BUMPERS_setStateChangedHandler(bumpersStateChanged);
   ACS_setStateChangedHandler(acsStateChanged);
   powerON();
   enableACS();
   setACSPwrMed();

   mSleep(500);
   setLEDs(0b000000);

   changeDirection(FWD);
   moveAtSpeed(100,100);
   servoposition=8;
   startStopwatch1();                         // Stoppuhr starten

   while(1)
   {
      task_RP6System();

      if(getStopwatch1() > 2000)             // zwei Sekunden sind vorbei
      {
         setStopwatch1(0);                   // Stoppuhr zurücksetzen
         if(servoposition==8)                // Servoseite anzeigen
				setLEDs((statusLEDs.byte & ~16) | 2);
			else
			   setLEDs((statusLEDs.byte & ~2) | 16);

         for(i=0; i<50; i++)                 // ein Sekunde lang Servo bewegen
         {
            PORTA |= 1;														// Impuls senden
				cli(); // vorsichtshalber atomar
				timer=0; // timer wird in ISR alle 100µs inkrementiert
				sei();
				while(timer < servoposition) task_RP6System();

            PORTA &= ~1;													// Impulspause
				cli(); // vorsichtshalber atomar
				timer=0; // timer wird in ISR alle 100µs inkrementiert
				sei();
				while(timer < (200-servoposition)) task_RP6System();
         }

         if(servoposition==8) servoposition=23; // andere Servoposition wählen
            else servoposition=8;
      }

      if((getStopwatch1() % 125) == 0)                         // Betriebsanzeige
		{
			if(statusLEDs.byte & 4)
				setLEDs((statusLEDs.byte & ~4) | 32);              // (Spielerei :)
			else                                                  // krass!
			   setLEDs((statusLEDs.byte & ~32) | 4);
		}
   }
   return(0);
}