Tasterauswertung und Kollisionserkennung Problem

[spomue]

Hallo liebe Asuro Begeisterte,

ich spiele schon einwenig mit dem Asuro herum doch ein Problem bekomm ich einfach nicht hin.
Zum einen habe ich ein kleines Programm das die Taster auswertet und dementsprechend umdreht. Zum anderen habe ich mir das Kollisionsprogramm aus eurem Forum genommen.
Beide funktionieren getrennt voneinander super. Versuche ich beide nur Schritt für Schritt zusammen zubringen habe ich beim Messen der Geschwindigkeit meine großen Probleme.
Ich habe mir das Tasterprogramm genommen und die Geschwindigkeitsmessung eingebunden. Es wird noch nicht reagiert auf eine Änderung.

1. Die gemessenen Geschwindigkeiten sind im Vergleich zum einzel Programm sehr viel kleiner( normal sind es 190, im Tasterprogramm sind es 150).
2. Nachdem ich eine Taste gedrückt habe wird als Geschwindigkeit nur noch 0 angegeben.

Warum ist das so? was habe ich vergessen?

Bitte helft mir. Ich weiß einfach nicht mehr weiter.

Hier mein Programm und die header- Datei habe ich auch angehangen.

#include "asuro.h"  // bitte die neue Datei benutzen
                    // asuro.h und asuro.c vom 31.03.05

#define aus {MotorState(BREAK,BREAK);}
#define go {MotorSpeed(150,150);MotorState(FWD,FWD);}
#define backturn { MotorSpeed(50,-50);MotorState(RWD,RWD);msleep(500);}
#define backward { MotorSpeed(-50,-50);MotorState(RWD,RWD);msleep(300);}
#define rightturn { MotorSpeed(50,-50);msleep(300);}
#define leftturn { MotorSpeed(-50,50);msleep(300);}

static volatile uint8_t switched=0;
uint8_t status_led = 0;

inline void msleep(uint16_t time)
{
   uint16_t i;
   for(i=time;i>0;i--)
      Sleep(72);
}

void status(void){
	if (status_led == 0){
		status_led = 1;
		StatusLED(GREEN);
	}
	else if (status_led == 1){
		status_led = 0;
		StatusLED(RED);
	}
	return;
}

void write(char* text)
{
   uint8_t i=0;
   while((text[i]!='\0')&&(i<255)) i++;
         
   SerWrite((unsigned char*)text,i);
}

void writeint(int16_t zahl)
{
   char text[7]={'\0'};
   itoa(zahl,text,10);
   write(text);
}

int speed(void)
{
   int rightspeed,leftspeed;
   int leftold,rightold;

   EncoderReset();      // reset encoder


   leftold=encoder[LEFT];
   rightold=encoder[RIGHT];
   msleep(300);
   leftspeed=encoder[LEFT]-leftold;
   rightspeed=encoder[RIGHT]-rightold;

   write("/n/r  speed Left,Right ");
   writeint(leftspeed);
   writeint(rightspeed);
   return leftspeed+rightspeed;
}

ISR(INT1_vect)
{
	StopSwitch();	
	switched=1;
}

int main(void)
{
	int16_t num_of_switch;
	int v;
	
	Init();
	EncoderInit();
	StatusLED(OFF);
	write("\n\rASURO OK\n\r");

	go;
	msleep(500);

	while(1)
	{
		BackLED(OFF,OFF);
		StartSwitch();

		while(switched == 0){
			status();
			v=speed();
      		write("\n\r speed ");
      		writeint(v);
		}
		

		// Wenn Taster gedrückt wurde
		if(switched == 1){
			write("\n\r auf taster gefahren ");
			num_of_switch = PollSwitch ();
						
			if(num_of_switch < 5){				
				BackLED(OFF,ON);
				backward;
				rightturn;
				go;
				num_of_switch = 0;
				msleep(500);
			}
			
			if(num_of_switch > 5) {
				BackLED(ON,OFF);
				backward;
				leftturn;
				go;
				num_of_switch = 0;
				msleep(500);
			}
		switched = 0;
		}
	}
   return 0;
}

Danke für eure Hilfe.