Asuro Problem Init()

[mich-beck]

Habe ein kleines Programm für den asuro geschrieben, mit welchem er Abgründen und Hindernissen ausweichen kann. Das funktioniert mit diesem Quelltext auch wunderbar.
Das Problem ist:
Ich habe versucht die Init() außerhalb der while-schleife aufzurufen, was dazu führte, dass der asuro nur noch ausweicht. Also er fährt ständig rückwärts->kurve->rückwärts->kurve...
Manchmal fährt er dann doch gerade aus, bis er auf ein hinderniss oder einen Abgrund trifft, und dann wiederholt er das Ausweichmanöver wieder einige Male.
Kann mir das mal jemand erklären? Wo liegt mein Fehler im Programm?


Edit_1:

Das Problem hat sich gelöst, hatte ein Problem mit der Sleep-Funktion, da ich die neuen librarys verwende.
Jetzt ist aber das nächste Problem aufgetaucht:
Das Prog funktioniert nur, wenn ich die BackLEDs einschalte.
Wenn nicht, fährt der Asuro permanent das Ausweichmanöver.

HILFE!!!!!!!!


#include "asuro.h"
#define STOP 20
/*asuro kann einem abgrund mittels der liniensensoren ausweichen und hindernissen mittels der kollisionstaster*/

int main(void)
{
Init();
while(1)
{
//***************************deklaration der variablen***************************************** ***
unsigned int lineData[2]; //array für die daten der liniensensoren
unsigned char i; //laufvariable für die for-schleifen
unsigned char taste=0; //nimmt den wert der kollisionstaster auf
//***************************programm*************** **********************************************

FrontLED(ON);
Msleep(0.3); //kurze pause, damit linedata die richtigen werte erfasst
LineData(lineData);

PollSwitch();
taste = PollSwitch();
BackLED(ON,ON);

if ((lineData[0] > STOP) && (lineData[1] > STOP) && (taste==0)) //vorwärts falls kein hinderniss und kein abgrund
{
MotorDir(FWD,FWD);
MotorSpeed(150,150);
PollSwitch();
}
else if ((lineData[0] < STOP) || (lineData[1] < STOP) || (taste>0)) //wenden falls abgrund oder hinderniss
{
MotorDir(BREAK,BREAK);
MotorSpeed(0,0); //stop
Msleep(2);
for(i=0; i<210; i++) //kurz zurueck
{
MotorDir(RWD,RWD);
MotorSpeed(130,130);
Msleep(2);
}
MotorSpeed(0,0);

for(i=0; i<140; i++) //wenden
{
MotorDir(FWD,RWD);
MotorSpeed(140,140);
Msleep(2);
}

}

//Sleep(200);
}
return 0;
}