Das ist der code, den ich bisher geschrieben hab, aber irgendwie funktioniert FahreEinzeln(..) nicht richtig. Die zurückgelegten Strecken sind sehr sehr sehr viel kürzer als angegeben. Fahre() funktioniert allerdings ganz gut, obwohl diese funktion nach dem selben prinzip geht, wie FahreEinzeln(..). Vielleicht überseh ich auch irgendwas...

Code:
#include "asuro.h"

#define false 0
#define true 1
#define SCHWARZ 0
#define WEISS 1
#define LINKS 0
#define RECHTS 1
#define LGRW 540
#define RGRW 530

typedef short bool;

//Gradzahl in Bogenmaß umrechnen
double dtor( double deg )
{
    double r = 32*deg/180;// PI*reifenabstand*winkel/180°
	return r;
}

// eine bestimmte Strecke mit einem Rad fahren
void FahreEinzeln( double mm , int grenzw , unsigned char dir , short rad );

//eine bestimmte Strecke mit beiden Rädern fahren
void Fahre( double mm , unsigned int lgrenzw , unsigned int rgrenzw , unsigned char dir);

void InitTinte()
{
	FahreEinzeln( 5.91 , LGRW , RWD , LINKS );
	FahreEinzeln( 5.91 , LGRW , FWD , LINKS );
}

//-----------HAUPTPROGRAMM----------------
int main(void)
{
	Init();
	InitTinte();
	//Gleichseitiges Dreieck mit seitenlänge 10cm (100mm) fahren
	Fahre( 100.0 , LGRW , RGRW , FWD );
	FahreEinzeln( dtor( 300.0 ) , RGRW , FWD , RECHTS );
	Fahre( 100.0 , LGRW , RGRW , FWD );
	FahreEinzeln( dtor( 300.0 ) , RGRW , FWD , RECHTS );
	Fahre( 100.0 , LGRW , RGRW , FWD );

	while(1);
	return 0;
}
void Fahre( double mm , unsigned int lgrenzw , unsigned int rgrenzw , unsigned char dir)
{
	unsigned int rodo,lodo,rodo_old,lodo_old;
	unsigned int lcounter = 0;
	unsigned int rcounter = 0;
	unsigned int data[2];
	unsigned char lspeed = 140;
	unsigned char rspeed = 140;

	OdometrieData( data );

	//Momentaner Wert links
	if( data[LINKS] < lgrenzw )
		lodo_old = SCHWARZ;
	else
		lodo_old = WEISS;

	//Momentaner Wert rechts
	if( data[RECHTS] < rgrenzw )
		rodo_old = SCHWARZ;
	else
		rodo_old = WEISS;

	//Losfahren
	MotorDir( dir , dir );
	MotorSpeed( lspeed , rspeed );

	while( 3*lcounter < mm || 3*rcounter < mm )
	{
		OdometrieData( data );

		//links Auswerten
		if( data[LINKS] < lgrenzw )
			lodo = SCHWARZ;
		else
			lodo = WEISS;

		//rechts Auswerten
		if( data[RECHTS] < rgrenzw )
			rodo = SCHWARZ;
		else
			rodo = WEISS;

		//Farbwechsel links prüfen
		if( lodo != lodo_old )
		{
			lcounter += 1;
			lodo_old = lodo;
		}

		//Farbwechsel rechts prüfen
		if( rodo != rodo_old )
		{
			rcounter += 1;
			rodo_old = rodo;
		}

		//Ungleichmäßigkeiten ausgleichen
		if( lcounter+1 < rcounter )
			rspeed = 0;
		else if( rcounter+1 < lcounter )
			lspeed = 0;
		else
		{
			rspeed = 140;
			lspeed = 140;
		}

		//Falls eine Seite Ziel erreicht, stehenbleiben
		if( 3*lcounter >= mm )
			lspeed = 0;
		if( 3*rcounter >= mm )
			rspeed = 0;

		MotorSpeed( lspeed , rspeed );
	}
	MotorSpeed( 0 , 0 );
}

void FahreEinzeln( double mm , int grenzw , unsigned char dir , short rad )
{
	unsigned int data[2];
	unsigned int counter = 0;
	bool odo,odo_old;

	OdometrieData(data);

	//Momentaner Wert
	if( data[rad] < grenzw )
		odo_old = SCHWARZ;
	else
		odo_old = WEISS;

	//Richtung und Geschwindigkeit einstellen
	if( rad == LINKS )
	{
		MotorDir( dir , BREAK );
		MotorSpeed( 140 , 0 );
	}
	else
	{
		MotorDir( BREAK , dir );
		MotorSpeed( 0 , 140 );
	}

	//Strecke fahren
	while(3*counter < mm )
	{
		OdometrieData(data);
		//Auswerten
		if( data[rad] < grenzw )
			odo = SCHWARZ;
		else
			odo = WEISS;

		//Farbwechsel prüfen
		if( odo != odo_old )
		{
			counter += 1;
			odo_old = odo;
		}
	}

	//Anhalten
	MotorSpeed( 0 , 0 );
	MotorDir( BREAK , BREAK );
}
mfg Lukas

P.S.: die Idee mit dem Servo ist nicht schlecht. mal schauen vielleicht mach ich das auch noch mal.