Odometrie

[H3llGhost]

[quote="Gerry1973"]

[...]

#include "asuro.h"

int main(void) {
	unsigned int odo_max_l,odo_max_r,odo_min_l,odo_min_r,odo_mitte_l,odo_mitte_r,data[2];
	unsigned char auf_l,auf_r,speed,speed_wert,speed_l,speed_r,zaehler_l,zaehler_r,schwing,rueckw,dreh_l,dreh_r,taste;;
	unsigned long dauer1,dauer2;
	Init();
	MotorDir(FWD,FWD);
	auf_r=auf_l=0;
	zaehler_r=zaehler_l=rueckw=dreh_l=dreh_r=0;
	odo_max_l=odo_max_r=odo_min_l=odo_min_r=300;
	speed=120;
	speed_l=speed_r=speed;
	speed_wert=(speed/20);
	MotorSpeed(speed_l,speed_r);  

	while(1) {
		
		for(schwing=0;schwing<30;schwing++) {
			dauer1=Gettime();
			odo_max_l--;
			odo_max_r--;
			odo_min_l++;
			odo_min_r++;
			OdometrieData(data);
			if (data[0]>odo_max_l) {odo_max_l=data[0];}
			if (data[1]>odo_max_r) {odo_max_r=data[1];}
			if (data[0]<odo_min_l) {odo_min_l=data[0];}
			if (data[1]<odo_min_r) {odo_min_r=data[1];}
			odo_mitte_l=(odo_max_l+odo_min_l)/2;
			odo_mitte_r=(odo_max_r+odo_min_r)/2;
			if ((auf_l==0) && (data[0]>(odo_mitte_l+5))) {
				auf_l=1;
				zaehler_l++;
			}
			else if ((auf_l==1) && (data[0]<(odo_mitte_l-5))) {
				auf_l=0;
			}
			if ((auf_r==0) && (data[1]>(odo_mitte_r+5))) {
				auf_r=1;
				zaehler_r++;
			}
			else if ((auf_r==1) && (data[1]<(odo_mitte_r-5))) {
				auf_r=0;
			}
		
			dauer2=Gettime();
			while ((dauer1+5)>dauer2) {dauer2=Gettime();}
		}
		
		if (zaehler_l<speed_wert) {speed_l=speed_l+speed_wert;}
		else if (zaehler_l>speed_wert) {speed_l=speed_l-speed_wert;}
		
		if (zaehler_r<speed_wert) {speed_r=speed_r+speed_wert;}
		else if (zaehler_r>speed_wert) {speed_r=speed_r-speed_wert;}
		
		zaehler_l=zaehler_r=0;
		
		if (PollSwitch()>0) {
			taste=PollSwitch();
			if (taste==1) {dreh_l=(800/speed);}
			if ((taste==2) || (taste==4)) {dreh_r=(1200/speed);}
			if (taste==31) {dreh_r=(800/speed);}
			if ((taste==8) || (taste==16)) {dreh_l=(1200/speed);}
		}
		
		if (dreh_l>0) {
			MotorDir(RWD,BREAK);
			dreh_l--;
		}
		else if (dreh_r>0) {
			MotorDir(BREAK,RWD);
			dreh_r--;
		}
		else {
			MotorDir(FWD,FWD);
		}
		
		MotorSpeed(speed_l,speed_r);
	}
	return 0;
}

Ich habe schon einige Programme für Odometrie ausprobiert, leider hat mein ASURO immer einen Rechtsdrall.
Woran könnte das liegen?