@ehenkes
Habe das Gefühl das PollSwitch() bei Dir dauernd "anspringt".

Debug doch bitte mal:

1) Zeigt ASURO mit folgendem Ersatzcode das gleiche, von Dir beschriebene, Verhalten?
Code:
int avoid_Obstacle(int idx) {
	return 1;
}
2) Fährt ASURO mit folgendem Ersatzcode dauernd vorwärts (cruise)?
Code:
int avoid_Obstacle(int idx) {
	return 0;
}
3) Falls die Anworten "JA" waren, könnte folgender Ersatzcode bei PollSwitch() zu stabileren Ergebnissen führen.
Code:
int avoid_Obstacle(int idx) {
	static unsigned char count=0;
	static int old=0;
	int data=PollSwitch();

	count++;
	if (old!=data) count=0;
	old=data;
	return (count>4) ? data : 0;
}
4) Und, überhaupt, blinkt denn wenigstens die Statuslampe im Sekundentakt?

@robo.fr
Ist ja auch nur als weitere Alternative gedacht. Empfinde es aber als schöner wenn im Task kein:
"state=5;"
oder
"if (time_in_this_state < timeout_3) {...}"
vom eigentlichen Code der Taskaufgabe ablenkt.