Hallo zusammen.

Vieleicht finde ich einen Crack unter euch, der mir sagt, warum mein ASURO nich mit einer konstanten geschwindigkeit fahren will. Habe schon mal was zusammengeschrieben, aber leider macht er lustige Sachen anstatt konstant zu fahren

Ich wäre sehr dankbar wenn mir jemand ein paar tipps geben könnte.


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


void speed(unsigned char wegl)
{ 
unsigned char speed;
int wegl;
speed=wegl;
if(wegl<80)speed++;
else{speed--;}
return (speed);
}

int main(void) 
{ 
	unsigned char sped, flagl=0, flagr=0; 
	unsigned int data[2]; 									//Speicher Bereitstellen
	int wegl, wegr, diff, time, timeb;
	int speedLeft,speedRight;
	
	Init();
	MotorDir(RWD,RWD);
	StatusLED(GREEN); 
	speedLeft = speedRight = speed;
	
	
	while(TRUE)
	{
		wegl=0; wegr=0;
		for(time=1; time<500; time++)				// Zeitschleife 1 Sekunde
		{
			OdometrieData(data); 
			if ((data[0] < 550) && (flagl == 1)) {flagl = 0; wegl++;} 
			if ((data[0] > 650) && (flagl == 0)) {flagl = 1; wegl++;} 
			if ((data[1] < 550) && (flagr == 1)) {flagr = 0; wegr++;} 
			if ((data[1] > 650) && (flagr == 0)) {flagr = 1; wegr++;} 
			diff=wegr-wegl; 
			if (diff>0) speedRight--;             		     // fahre geradeaus 
				else if (diff<0) speedLeft--; 
				else 
				{
				speedRight=speed; 
				speedLeft=speed;
				} 
			if (speedRight<0) {speedRight=0;} 
			if (speedLeft<0) {speedLeft=0;} 
		
				MotorSpeed(speedLeft,speedRight); 
				SerWrite(wegl, sizeof(wegl));
				Sleep(76);									// 1 Millisekunde pause	
		}
		
		if(PollSwitch()>0) 
		{
			for (timeb=1;timeb<500;timeb++)		
			{
				if(PollSwitch()>0) 
				{
				MotorDir(BREAK,BREAK);
				BackLED(ON,ON);
				Sleep(76);
				}
			}
		}
		
	}
 
   return 0; 
}