Hallo an alle!!

habe bei einem programm probleme
Wollte das programm RP6_BASE_SERVO_01 bearbeiten:
Code:
// Uncommented Version of RP6Base_Servo_01.c
// written by D. Ottensmeyer
// ------------------------------------------------------------------------------------------

#include "RP6RobotBaseLib.h"

#define SERVO_OUT			SDA

#define LEFT_TOUCH			550
#define RIGHT_TOUCH			254
#define PULSE_ADJUST 		4
#define PULSE_REPETITION	19

void bumpersStateChanged(void)
{
	if(bumper_left)  
	{
		moveAtSpeed(0,0);  
		setLEDs(0b010000);
		startStopwatch1();
	}
	if(bumper_right)
	{
	    moveAtSpeed(0,0);
		setLEDs(0b010001);
		startStopwatch1();
	}	
}

void blink(void)
{
	if(getStopwatch1() > 500)
	{
		statusLEDs.LED2 = !statusLEDs.LED2;
		statusLEDs.LED5 = !statusLEDs.LED5;
		updateStatusLEDs();
		setStopwatch1(0);
	}
}

void initSERVO(void)
{
	DDRC |= SERVO_OUT;
	PORTC &= ~SERVO_OUT;
	startStopwatch1();
}

void pulseSERVO(uint8_t position)
{
	cli();
	PORTC |= SERVO_OUT;
	delayCycles(LEFT_TOUCH);
	while (position--) 
	{
		delayCycles(PULSE_ADJUST);
	}
	PORTC &= ~SERVO_OUT;
	sei();
}

void task_SERVO(void)
{static uint8_t pos;
	if (getStopwatch1() > PULSE_REPETITION) 
	{
		pulseSERVO(pos);
		if (getStopwatch2() > 48) {
			pos++;
			if (pos > RIGHT_TOUCH) {pos = 0;}
			setStopwatch2(0);
		}
		setStopwatch1(0);
	}
}

int main(void)
{
	initRobotBase();
	setLEDs(0b010001);
	mSleep(2500);
	
	BUMPERS_setStateChangedHandler(bumpersStateChanged);
	
	
	powerON();
	
	
	
	initSERVO();
	
	
	while(true)  
	{
	    task_SERVO();
		task_RP6System();
		
		
	    setLEDs(0b010001);
		move(100, FWD, DIST_MM(300), true);
		rotate(50, RIGHT, 180, true);
		move(200, FWD, DIST_MM(300), true);
		rotate(50, LEFT, 180, true);
	   
		
	}
	return 0;}
dass robby nach vorne fährt wendet, und an die start pos. zurück kehrt!!
aber sich das SERVO die ganze zeit nach links dreht und sich langsam zurück nach rechts stellt!

Das original servo prog. funktioniert einwandfrei!!!
wenn ich die fahrt-, und bumper funktionen dazu schreibe,
zuckt der SERVO mal kurz dann is ende, die fahrt und bumper befehle
werden aber ab gearbeitet!!!!!

Was also tuen ausser Weinen????


ps: Prog. anfänger!!b


DANKE SCHON MAL FÜR DIE HILFE