Oje:
Code:
#include "RP6RobotBaseLib.h"

uint8_t i, servoposition;

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

void bumpersStateChanged(void)
{
	setLEDs(1);
	if(bumper_left || bumper_right)
	{
		changeDirection(BWD);
		move(100, BWD, DIST_MM(200), true);
		rotate(50, RIGHT, 120, true);
	}
	changeDirection(FWD);
	moveAtSpeed(100,100);
}

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

	changeDirection(FWD);
	moveAtSpeed(100,100);
	servoposition=0; 									// 0 ist sleep(8), 1 ist sleep(23)
	startStopwatch1(); 								// Stoppuhr starten

	while(1)
   {
		setLEDs(0b100100);
		task_RP6System();
		//sleep(50); // das Tasksystem muss man nicht so häufig aufzurufen

		if(getStopwatch1() > 2000) 				// zwei Sekunden sind vorbei
		{
			setStopwatch1(0); 						// Stoppuhr zurücksetzen
			setLEDs(0b010010);
			for(i=0; i<50; i++) 						// ein Sekunde lang Servo bewegen
      	{
         	PORTA |= 1;
         	if(servoposition) sleep(23); else sleep(8);
         	PORTA &= ~1;
         	sleep(100); 							// Impulspause ist sleep() und Task zusammen!
         	task_RP6System(); 					// beim Servoschwenken weiterfahren
      	}
      	if(servoposition) servoposition=0; 	// andere Servoposition wählen
				else servoposition=1;
		}
   }
   return(0);
}
Man sollte eben mit dem Tasksystem kein sleep() verwenden ;)