Ihr sollt doch selbst nachdenken! Naja, egal.

Hier ist er (nicht so toll programmiert; lieg dadran, dass seeeehr oft was geändert wurde, weil es nicht so richtig geklappt hat).

Code:
#include "asuro.h"

unsigned char speed, flagl=FALSE, flagr=FALSE;
unsigned int data[2];
unsigned int wegl, wegr, diff, schnittl, schnittr;
int speedLeft,speedRight;

int vorne_frei(void)
{
	int temp1, temp2, temp3;
	temp1 = PollSwitch(); Sleep(7);
	temp2 = PollSwitch(); Sleep(7);
	temp3 = PollSwitch(); Sleep(7);
	if((temp1 * temp2 * temp3) == 0)
	{
		return 1;
	}
	else
	{
		return 0;
	}
}

void start(void)
{
   Init();
   
   StatusLED(GREEN);
}

void setze_speed(int geschwindigkeit)
{
	int i;
	speedLeft = geschwindigkeit;
	speedRight = geschwindigkeit;
	
	MotorDir(BREAK,BREAK); Sleep(72);
	
	if(geschwindigkeit > 0)
	{
	   MotorDir(FWD,FWD);
	}
	else
	{
		geschwindigkeit *= -1;
		MotorDir(RWD,RWD);
	}
	   schnittl = 0;
	   schnittr = 0;
	   
	   MotorSpeed(speedLeft,speedRight);
	   
	   for(i = 1; i < 100; i++) 
	   {
			OdometrieData(data);
			schnittl += data[0];
			schnittr += data[1];
			Sleep(72);
	   }
	   schnittl /= 100;
	   schnittr /= 100;
}

void geradeaus(void)
{
	   int logl, logr, log;
	   wegl=0; wegr=0;
	   logl = 0;
	   logr = 0;
	   log = 0;
	   while(log < 100)
	   {
		  OdometrieData(data);
		  logl += data[0];
		  logr += data[1];
		  wegl=0; wegr=0;
		  if ((data[0] < schnittl) && (flagl == TRUE)) {flagl = FALSE; wegl++; SerWrite("l",1);}
		  else if ((data[0] > schnittl) && (flagl == FALSE)) {flagl = TRUE; wegl++; SerWrite("L",1);}
		  if ((data[1] < schnittr) && (flagr == TRUE)) {flagr = FALSE; wegr++; SerWrite("r",1);}
		  else if ((data[1] > schnittr) && (flagr == FALSE)) {flagr = TRUE; wegr++; SerWrite("R",1);}
		  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);
		  Sleep(7);
		  log++;
	   }
	   SerWrite("|",1);
	   schnittl = logl / 100;
	   schnittr = logr / 100;
}

int main(void)
{ int x;
   start();
   while(1)
   {
	setze_speed(200);
	while(vorne_frei() == 1)
	{
		geradeaus(); StatusLED(GREEN);
	}
   MotorDir(RWD,RWD);
   MotorSpeed(200,100);
   for(x = 1; x <= 1000; x++)
	{
		Sleep(72);
	}
   }
   return 0;
}
Gruß,
Yoshi