Hallo an alle

Ich wollte fragen ob ihr vielleicht meinen code anschaun könnt. Das ganze funktioniert nicht ganz so wie ich will

Ich habe folgendes Problem:

wenn ich den Asuro einschalte und die Motoren sich drehen und mir die Zählerstände über das Hyperterminal zurückschreben lasse, dann seh ich das es mir nicht jeden Farbwechsel mitzählt, sondern nur jeden 20. oder 30.

Wenn ich die Motoren von den Zahnrädern wegnehme, und von Hand langsam drehe dann zählt er jeden Farbwechsel, aber sobald ich ein bisschen schneller drehe, überspringt er einige Farbwechsel.

Das Programm soll den Asuro eigentlich bei der Geradeausfahrt eigentlich Regeln. Die Regelung funktioniert auch, nur er zählt zu langsam.

Code:
#include "stdlib.h"
#include "asuro.h"

void SerWert(unsigned int iWert)
{
    unsigned char cWert[6] = "     ";

    itoa(iWert, cWert, 10);
    SerWrite(cWert, 5);
	SerWrite(", ", 2);
}

int main(void)
{

     /*Merker Links und REchts*/
     unsigned int ML = 0;
	 unsigned int MR = 0;
     
     /*Zähler für farbwechsel*/
     unsigned int zaehlerl = 0;
     unsigned int zaehlerr = 0;
        
	/*geschwindigkeit der Motoren*/
     int speed = 100; 			//Grundgeschwingkeit
     int speedl = 0;			//Geregelte Geschwindigkeit links
     int speedr = 0;			//Geregelte Geschwindigkeit Rechts
	 
     /* odometrie Datenspeicher */
     unsigned int data[2]={0,0};
	 
     /*Schwellwerte*/
     unsigned int SO = 500;
     unsigned int SU = 200;
	 
     /*Differenz*/
     int diff = 0;
	 
     /*Verstärkungsfaktor*/
     int VFaktor = 2;
     
      
     
     Init();
     
     StatusLED(GREEN);
	 
	 // Merker Links auf Anfangswert setzten
	if (data[0] > SO)
		{
		ML=1;
		}
		
	if (data [0] < SU)
		{
		ML=2;
		}
		
	 // Merker Rechts auf Anfangswert setzten	
	if (data [1] > SO)
		{
		MR=1;
		}
		
			
	if (data [1] < SU)
		{
		MR=2;
		}
	
	
     
     while(1) {
     
        MotorDir(FWD,FWD);
        MotorSpeed(speedl,speedr);
        OdometrieData(data); // links = 0, rechts = 1 
  
        
		// Odometrie Auswertung Links
        if ((data[0]) > SO && (ML==2))		// wenn Helligkeitswert links > Schwelle oben und Merker links ist 2
				{
				zaehlerl ++; ML=1;		// dann Zähler +1 und setze Merker auf 1
				}
			
        if ((data[0]) < SU && (ML==1))		// wenn Helligkeitswert links < Schwelle unten und Merker links ist 1
				{
				zaehlerl ++; ML=2;		// dann Zähler +1 und setze Merker auf 2
                } 
          
        // Odometrie Auswertung Rechts
        if ((data[1]) > SO && (MR==2))		// wenn Helligkeitswert rechts > Schwelle oben und Merker rechts ist 2
				{
				zaehlerr ++; MR=1;		// dann Zähler +1 und setze Merker auf 1
				}
			
        if ((data[1]) < SU && (MR==1))		// wenn Helligkeitswert rechts < Schwelle unten und Merker rechts ist 1
				{
				zaehlerr ++; MR=2;		// dann Zähler +1 und setze Merker auf 2
                } 
				
				
               
          // Zälerdifferenz ermitteln
          diff = zaehlerl - zaehlerr;
                     
          
          speedl = speed - VFaktor * diff;
          speedr = speed + VFaktor * diff;
          
        SerWert(zaehlerl);
		SerWert(zaehlerr);
		SerWert(ML);
		SerWert(MR);
		SerWert(data[0]);
		SerWert(data[1]);

        
        SerWrite("\n\r", 2);		
		
		}
		
	return 0;
}
hat jemand eine Idee??

Vielen Dank im Vorraus

Gruß Schleicha