Okay, hab es jetzt mal ein bisschen abgeändert. So fährt er geradeaus. Okay, ein bisschen lenkt er hin und her - aber egal.
Code:
int RZaehler;
int UebergaengeR=0;
int MittelwertR;
int MittelwertL;
int LZaehler;
int UebergaengeL=0;
long Zaehler=0;
int Lmin=1023;
int Lmax=0;
int Rmin=1023;
int Rmax=0;
MotorDir(FWD,FWD);
MotorSpeed(LSpeed,RSpeed);
for (Zaehler=0; Zaehler<100; Zaehler++)
{
unsigned int data[2];
OdometrieData(data);
if (data[0] > Lmax)
Lmax=data[0];
if (data[0] < Lmin)
Lmin=data[0];
if (data[1] > Rmax)
Rmax=data[1];
if (data[1] < Rmin)
Rmin=data[1];
LongSleep(1);
}
MittelwertL=(Lmin+Lmax)/2;
MittelwertR=(Rmin+Rmax)/2;
int i;
UebergaengeL = UebergaengeR = 0;
LZaehler = HELL;
RZaehler = HELL;
for (i = 0; i< 500; i++)
{
unsigned int data[2];
OdometrieData(data);
if (data[0] < MittelwertL && LZaehler == DUNKEL)
{
UebergaengeL = UebergaengeL + 1;
LZaehler = HELL;
}
else if (data[0] > MittelwertL && LZaehler == HELL)
{
UebergaengeL = UebergaengeL + 1;
LZaehler = DUNKEL;
}
if (data[1] < MittelwertR && RZaehler == DUNKEL)
{
UebergaengeR = UebergaengeR + 1;
RZaehler = HELL;
}
else if (data[1] > MittelwertR && RZaehler == HELL)
{
UebergaengeR = UebergaengeR + 1;
RZaehler = DUNKEL;
}
LongSleep(1);
}
if (UebergaengeR < UebergaengeL)
{
RSpeed = RSpeed + 1;
LSpeed = LSpeed - 1;
StatusLED(RED);
}
else if (UebergaengeR > UebergaengeL)
{
RSpeed = RSpeed - 1;
LSpeed = LSpeed + 1;
StatusLED(GREEN);
}
else if (UebergaengeR == UebergaengeL)
{
StatusLED(YELLOW);
}
Übrigens: Die Kalibrierung wiederhol ich, falls plötzlich ne Lampe angeht und dann die gesamten Daten falsch sind.
Lesezeichen