Grüßt euch!

Seit ungefähr nem halben Jahr habe ich jetzt auch einen Asuro und bin bis jetzt recht gut damit zurande gekommen, allerdings bin ich jetzt irgendwo an meine Kapazitäten gestoßen.
Ich habe vor den Asuro eine Figur (der Einfachheit halber mal einen 5-zackigen Stern - auch Pentagramm genannt O.o) malen zu lassen. Das hab ich auch schon hinbekommen (für Quelltext siehe unten). Jetzt soll er aber bei einem Hindernis, das er durch die Taster erkennt, eine bestimmte Strecke zurücksetzen und nochmal neu anfangen.
Und da haperts auch schon... wie bekomm ich das am einfachsten hin? ^^

Liebe Grüße, Michi

Code:
#include "asuro.h"
#define fahr {go(80);}		// Define: Vorwärtsfahren
#define dreh {turn(50);}	// Define: Drehen
#define stop {MotorSpeed(0,0);}

int trightspeed=150;	// Variable: Drehgeschwindigkeit rechter Motor (in Drehfunktion)
int tleftspeed=150;		// Variable: Drehgeschwindigkeit linker Motor (in Drehfunktion
int diff;				// Variable: Tickdifferenz zwischen links und rechts

int main(void)	// Hauptfunktion
	{	// Klammer - Hauptfunktion
	Init();
	Encoder_Init();	// Initialisierung der Odometrie

	void turn(int ticks)	// Drehfunktion, Einführung der Variable "ticks"
		{	// Klammer - Drehfunktion
		Encoder_Set(0,0);	// Nullsetzen des Odometriezählers
		MotorSpeed(tleftspeed,trightspeed);
		MotorDir(RWD,RWD);
		while(encoder[LEFT]<ticks) Msleep(1);
		}	// Klammer - Drehfunktion

	void go(int distance)	// Fahrfunktion, Einführung der Variable "distance"
		{	// Klammer Fahrfunktion
		Encoder_Set(0,0);	// Nullsetzen des Odometriezählers
		MotorDir(RWD,FWD);
		MotorSpeed(175,175);
		diff=encoder[LEFT]-encoder[RIGHT];	// Regler: Bestimmung der Tickdifferenz zwischen links und rechts
		MotorSpeed(175-diff,175+diff);	// Regler: Motorsteuerung
		while(encoder[LEFT]<distance) Msleep(1);
		}	// Klammer Fahrfunktion

	fahr;
	dreh;
	fahr;
	dreh;
	fahr;
	dreh;
	fahr;
	dreh;
	fahr;
	dreh;
	stop;

	while(1){}	// Endlosschleife
	return 0;	// Rückgabewert 0, Programm ist noch nicht beendet
	}	// Klammer Hauptfunktion
PS: Da fällt mir ein: Vogons (ich glaube zumindest, dass ich's von ihm hab, man möge es mir verzeihen, wenn nicht ^^) Regler zum Geradeausfahren funktioniert bei mir nicht, beide Odometrieeinheiten geben aber Werte aus, das hab ich schon geprüft.

/e: Wen es wundert, warum der linke Motor scheinbar verkehrt herum dreht... Ich hab den falschrum angeschlossen XD