Kollisionstaster zu langsam

Hallo zusammen
Ich habe ein Problem beim Programmieren.
Asuro soll geradeausfahren und gleichzeitig auf ein Hinderniss reagieren.
Der Kollisionstaster reagiert zwar, leider erst nach einer kurzen Zeit.

Hier ist mein Programm


#include "asuro.h"
#include <stdlib.h>


int main(void)
{
unsigned char speed=150, flagl=FALSE, flagr=FALSE;
unsigned char taste=0;
unsigned int data[2];
int wegl, wegr, diff;
int speedLeft,speedRight;
Init();

MotorSpeed(150,150);
speedLeft = speedRight = speed;
wegl=0; wegr=0;

while(1)
{
taste=PollSwitch();
if (taste>0)
{
MotorSpeed(0,0);
StatusLED(RED);
}
else
{
MotorSpeed(150,150);
StatusLED(GREEN);
while(wegl<333)
{
OdometrieData(data);
if ((data[0] < 550) && (flagl == TRUE)) {flagl = FALSE; wegl++;}
if ((data[0] > 650) && (flagl == FALSE)) {flagl = TRUE; wegl++;}
if ((data[1] < 550) && (flagr == TRUE)) {flagr = FALSE; wegr++;}
if ((data[1] > 650) && (flagr == FALSE)) {flagr = TRUE; wegr++;}
diff=wegr-wegl;
if (diff>0) speedRight--; // fahre geradeaus
else if (diff<0) speedLeft--;
else {speedRight=speed; speedLeft=speed;}
if (speedRight<0) {speedRight=0;}
if (speedLeft<0) {speedLeft=0;}
MotorSpeed(speedLeft,speedRight);
}
}
}
}

kann mir jemand Helfen?

Zunächst mal:
Code sollte man immer mit dem Button [code] psoten, damit er nicht die ganze Seite belegt.

Meiner Vermutung zur Funktion: Dein Prgramm bleib in der while-Schleife hängen

while(wegl<333)

bis der weg 333 erreicht ist.

Die Wegzählung gibt es übrigens als interrupt-Routine schon in der sourceforge-lib, entstanden in diesem Thread:

https://www.roboternetz.de/phpBB2/viewtopic.php?t=11256

Danke für deine Antwort
Beim mehrmaligen Drücken der Taster funktioniert das Programm.
Dies ist wie du schon gesagt hast auf die Weglänge zurückzuführen.

Kennst Du diese Methode https://www.roboternetz.de/phpBB2/viewtopic.php?t=23163 für die Kollisionserkennung ?
Die funktioniert normalerweise besser als mit den Tastern.