Positionsbestimmung von Asuro

[julien]

Ich hab jetzt ein Programm geschrieben. Das war ein Haufen Arbeit. Ich hoffe, dass es vom Prinzip her funktionieren würde. Ich weiß nicht, woran der Fehler liegt. Die Status LED flackert nur, daher behaupte ich, dass da ein Fehler im Programm sein muss.

#include "asuro.h"
#include <math.h>
int main(void){
	float w=0,r=0,puls=1;
	unsigned int obj=0, objx[500], objy[500], Rmin = 1024, Rmax = 0, Lmin = 1024, Lmax = 0, Rmitte = 512, Lmitte = 512, data[2], wegr=0, wegl=0, richt=1, x=0, y=0, i=0; 
    unsigned char flagl=FALSE, flagr=FALSE, eingetr=FALSE;
	Init();
   DDRD |= (1 << DDD1);      // Port D1 als Ausgang 
   PORTD &= ~(1 << PD1);      // PD1 auf LOW 
   puls = 1;
	while(1){
	      if (PIND & (1 << PD0)){
         StatusLED(GREEN);   // kein Hindernis
		 } else {
		if(eingetr==FALSE){
		objx[obj]=x;
		objy[obj]=y;
		obj++;
		eingetr=TRUE;
		}
		richt=2;
		StatusLED(RED);      // Hindernis erkannt
		}
   puls = 1.02 * puls;      // Pulsbreite wird um 2% erhöht 
   if (puls > 10){
   if (PIND & (1 << PD0)){ richt=1; eingetr=FALSE; }
   puls = 1; }
   OCR2 = 255 - (int)(puls);
	if(richt==0) MotorDir(BREAK,BREAK);
    if(richt==1) MotorDir(FWD,FWD);
    if(richt==2) MotorDir(BREAK,FWD);
    MotorSpeed(150,150);
	
	for(i=0;i<=obj;i++){
	if((x<=objx[i]+10) && (y<=objy[i]+10)) richt=2;
	}
//-----------------------Schritte zählen-----------------------------------
	  OdometrieData(data); // 0. links, 1. rechts
        // max links
        if (data[0] > Lmax)
            Lmax += (data[0] - Lmax) / 2; 
        // min links 
        if (data[0] < Lmin)
            Lmin -= (Lmin - data[0]) / 2;
        // max rechts
        if (data[1] > Rmax)
            Rmax += (data[1] - Rmax) / 2;
        // min rechts
        if (data[1] < Rmin)
            Rmin -= (Rmin - data[1]) / 2;
         Rmitte=(Rmax+Rmin)/2;
         Lmitte=(Lmin+Lmax)/2;
      if ((data[0] < Lmitte) && (flagl == TRUE)) {
      flagl = FALSE;
      }
      if ((data[0] > Lmitte) && (flagl == FALSE)) {
      flagl = TRUE;
      wegl++;
      }
      if ((data[1] < Rmitte) && (flagr == TRUE)) {
      flagr = FALSE;
      }
      if ((data[1] > Rmitte) && (flagr == FALSE)) {
      flagr = TRUE;
      wegr++;
      }
//------------------------------------------------------------------------
//------------------------POSITIONSBESTIMMUNG-----------------------------
w=1.607143*(wegl-wegr); //winkel
r=((wegl+wegr)/2)*w;
x=(int)(r*cos(w));
y=(int)(r*sin(w));

	}
	return(0);
}