Ich kann dir leider nur bedingt weiterhelfen!
Wie man die Zeit zählt weiß ich leider auch überhaupt nicht!
Hier erstmal ein Programmcode zum Gerradeausfahren mit Odometrie (Aus dem Asuro Buch )!
Code:
#include "asuro.h"

#define TRIGGERLEVEL 600
#define HYSTERESIS 10
#define LOW 0
#define HIGH 1

int main(void)
{
	unsigned int data[2];
	signed int status[2]={0,0};
	signed int difference=0;
	unsigned long int distance=0;
	signed int speed;
	int i;
	Init();
	MotorDir(FWD,FWD);
	while(1) {
		
		
		// Odometrie auslesen
		OdometrieData(data);
		
		// Links von niedrig auf hoch?
		if ((status[0]==LOW) && (data[0]>TRIGGERLEVEL+HYSTERESIS)) {
		status[0]=HIGH;
		difference++;
		distance++;
		}
		
		// Links von hoch auf niedrig?
		if ((status[0]==HIGH) && (data[0]<TRIGGERLEVEL-HYSTERESIS)) {
		status[0]=LOW;
		difference++;
		distance++;
		}
		
		// Rechts von niedrig auf hoch?
		if ((status[1]==LOW) && (data[1]>TRIGGERLEVEL+HYSTERESIS)) {
		status[1]=HIGH;
		difference--;
		}
		
		// Rechts von hoch auf niedrig?
		if ((status[1]==HIGH) && (data[1]<TRIGGERLEVEL-HYSTERESIS)) {
		status[1]=LOW;
		difference--;
		}
		
		// Wenn difference über dem erlaubten Wert ist
		if (difference<-speed) difference=-speed;
		if (difference>speed) difference=speed;
		
		// Status LED nach der Odometrie leuchten lassen
		StatusLED(status[0]+status[1]*2);
		
		// difference auf Motoren verteilen
		if (difference>0) MotorSpeed(speed-difference,speed);
		else MotorSpeed(speed,speed+difference);
	}
	return 0;
}
Jetzt würde die Variable distance nach jedem Odometrietick den Wert um 1 erhöhen (Bei mir wären das 40 Ticks pro Umdrehung! Der Radumfang beträgt ca. 116mm) jetzt muss man nur noch gucken, dass man zählt wie viele Ticks es jede Sekunde gibt!