Ich hatte schonmal vor geraumer Zeit solch ein Programm geschrieben...
Hier der Quellcode:
Code:
//Programm zum Umfahren von Hindernissen mithilfe der Infrarot-Hinderniserkennung von waste
//zusammengestellt und programmiert von julien

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

int main(void)
{ 
   float puls;
   unsigned int Rmin = 1024, Rmax = 0, Lmin = 1024, Lmax = 0, Rmitte = 512, Lmitte = 512, lspeed=150, data[2], richt=0, wegr=0, wegl=0, auswl=0, auswr=0, schritt=0, vorhe=0r;  
   unsigned char flagl=FALSE, flagr=FALSE, hindernis=FALSE;
   Init(); 
   DDRD |= (1 << DDD1);      // Port D1 als Ausgang 
   PORTD &= ~(1 << PD1);      // PD1 auf LOW 
   puls = 1; 
   while(1) 
   {
   while(hindernis==FALSE){
      if (PIND & (1 << PD0)){
          StatusLED(GREEN);   // kein Hindernis
		 } else {
		 vorher=(int)(puls);
		 hindernis=TRUE;
		 StatusLED(RED);      // Hindernis erkannt
		}
	 puls = 1.02 * puls;      // Pulsbreite wird um 2% erhöht 
     if (puls > 10){
      if (PIND & (1 << PD0)) hindernis=0;
      puls = 1;
	  exit;
     }
    OCR2 = 255 - (int)(puls);
   }
   
   if(hindernis==TRUE){ //"gucken", wo noch ein freier Weg ist und diesen dann fahren
   if(schritt==0){ //nach rechts gucken
    MotorDir(FWD,RWD);
    Msleep(500);
   }
   if(schritt==1){ 
    auswr=(int)(puls);
   }
   if(schritt==2){ //nach links gucken
    MotorDir(RWD,FWD);
    Msleep(1000);
   }
   if(schritt==3){
    auswl=(int)(puls);
	MotorDir(FWD,RWD);
	Msleep(500);
   }
   if(schritt==4){ //richtigen weg fahren
     if(auswr<vorher) richt=1;
	 if(auswl<vorher) richt=2;
	 if(auswr==auswl) richt=3;
	 hindernis=FALSE;
	 schritt=0;
	}
   if(schritt<4) schritt++;
   }
   
   if(hindernis==FALSE){ //kein Hindernis in Sicht - geradeaus fahren
   richt=0;
   schritt=0;
           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; 
      wegl++; 
      } 
      if ((data[0] > Lmitte) && (flagl == FALSE)) { 
      flagl = TRUE; 
      wegl++; 
      } 
      if ((data[1] < Rmitte) && (flagr == TRUE)) { 
      flagr = FALSE; 
      wegr++; 
      } 
      if ((data[1] > Rmitte) && (flagr == FALSE)) { 
      flagr = TRUE; 
      wegr++; 
      }
	if(wegl<wegr) lspeed++;
	if(wegl>wegr) lspeed--;
	if(wegl>=65000){ wegr=wegr-65000; } //zurücksetzen, damit kein Überlauf nach
	if(wegr>=65000){ wegl=wegl-65000; } //längeren Fahrten passiert
	}
	if(richt==0) MotorDir(FWD,FWD); //geradeaus
    if(richt==1) MotorDir(RWD,FWD); //links
	if(richt==2) MotorDir(FWD,RWD); //rechts
	if(richt==3) MotorDir(RWD,RWD); //zurück
    MotorSpeed(lspeed,150);
  }
   return 0; 
}
Erklärung: wenn ein Hindernis vor ihm erkannt wird, guckt er nach links und dann nach rechts und ermittelt somit, in welcher richtung das Hindernis entfernter ist. Dann fährt er in diese Richtung. Ist kein Ausweg zu erkennen, fährt er zurück.

Leider funktioniert es nicht ganz richtig. Deshalb ist es sehr verbesserungswürdig...
Kann mir bitte jemand bei diversen Verbesserungsarbeiten helfen?

MfG
Julien