Ich hab gestern Versucht mein erstes "Richtiges" Programm für mein RP6 zu schreiben aber leider will das Programm nicht so wie ich will.

Das Programm sollte wenn es richtig funktioniert einen Gegenstand folgen.
(Der Gegenstand wird über die ACS Sensoren erkannt)

Das Problem ist zur Zeit das er nicht nach Rechts fahren will wenn sich das Hindernis nach Rechts bewegt.

Ich hab hier mal den Code Hochgeladen

Code:
#include "RP6RobotBaseLib.h"

void acsStateChanged(void)
{	
	
	if(obstacle_right)
	//Hinderniss rechts
	moveAtSpeed(100, 0);
	
	if(obstacle_left) 
	//Hinderniss links
	moveAtSpeed(0, 100);
	
	if(obstacle_right && obstacle_right ) 
	// Mitte
	moveAtSpeed(100, 100);
	
		statusLEDs.LED6 = obstacle_left && obstacle_right; // Mitte ?
		statusLEDs.LED3 = statusLEDs.LED6;
		statusLEDs.LED5 = obstacle_left; // Hinderniss links
		statusLEDs.LED4 = (!obstacle_left); // LED5 invertiert!
		statusLEDs.LED2 = obstacle_right; // Hinderniss rechts
		statusLEDs.LED1 = (!obstacle_right); // LED2 invertiert !
		updateStatusLEDs();  
	}
	
	
	void bumpersStateChanged(void)
{
	if(bumper_left)
	moveAtSpeed(0,0);
	setACSPwrOff(); 
	
	if(bumper_right)
	moveAtSpeed(0,0);	
	setACSPwrOff(); 
	
}
	
	
		
int main(void)
{
	
	initRobotBase();
	
	//ACS Event Handler reg.
	ACS_setStateChangedHandler(acsStateChanged);
	
	powerON(); // ACS Empfänger einschalten (und Encoder etc.)
	
	
	//Event Handler registrieren:
	BUMPERS_setStateChangedHandler(bumpersStateChanged);
	
	
	while(true)
	{
		//Aus der Hauptschleife ständig die motionControl Funktion aufrufen - sie regelt die Motorgeschwindigkeit :
		task_motionControl();
		task_ADC(); // Wird wegen den Motorstromsensoren aufgerufen!
		
		task_ACS(); // ständig die task_ACS Funktion aufrufen !
		
		task_Bumpers(); // ständig aus der Hauptschleife aufrufen
		
		setACSPwrLow(); // ACS auf mittlere Sendeleistung stellen.
	}
	
	return 0;
}

MFG V!P3R