Hallo

Am Ende von task_TeilErkannt() wird z=0 gesetzt, das bedeutet für task_fahren() dann natürlich fahren:

void task_fahren(void)
{if(z==0)
{
moveAtSpeed(50,50);
writeString_P("Fahren\n");}
.....

Das z=0 sollte wohl besser im else-Zweig sein und die 5 Schlafsekunden würde ich ersatzlos streichen weil er dann 5 Sekunden weiterdüst obwohl das hinderniss erkannt wurde? Fehlt hier nicht noch ein MoveAtSpeed():

powerON();
changeDirection(BWD);
task_TeilErkannt();

Vermutlich wäre es so richtiger:

powerON();
changeDirection(BWD);

while(true)
{task_servo();
task_TeilErkannt();
task_fahren();
task_RP6System();
}

Gruß

mic