Antikolisionsprogramm mit Tastern

[JensK]

ich habe mal ein programm mit meine Vater entworfen, wo der Asuro, wenn er irgendwo gegen fährt dem hinderniss ausweicht.

nur jetzt haben wir ein problem... alle Taster senden immer verschieden signale von sich an den hyperterminal... wie kann man das programm optimieren, so dass es funktioniert?
ich hoffe ihr könnt uns helfen

schonmal vielen dank für eventuelle Hilfe

MfG

JensK
wenn ihr die .hex braucht um es zu testen.. dann fragt einfach nach

/**************************************************************************************
*
* File Name:   test.c
* Project  :   ASURO
*
* Ver.     Date         Author   Comments
* -------  ----------   ------   ------------------------------
* 1.00	   17.04.2006   mko		 build
* 1.10	   17.04.2006   mko		 erweiterte asuro.h vom 27. September 2005 eingebunden
* 1.20	   20.04.2006   mko		 erweitert um Odometriedaten und doppeltes Auslesen der 
*								 Schalter
* 1.30	   02.03.2007   mko		 erweitert "geradeausfahren"
*
****************************************************************************************/


#include "asuro.h"
#define LA 0x20   // Taster links aussen
#define LM 0x10   // Taster links mitte
#define LI 0x8    // Taster links innen
#define RI 0x4    // Taster rechts innen
#define RM 0x2    // Taster rechts mitte
#define RA 0x1    // Taster rechts aussen
#define LRI 0xC   // Taster links und rechts innen
#define LAM 0x30  // Taster links aussen und mitte
#define LRM 0x12  // Taster links und rechts mitte
#define RAM 0x3   // Taster rechts aussen und mitte

#define LMIN 0x60 // Links läuft der Motor früher an als rechts
#define RMIN 0x60 // rechts etwas höher beginnen
#define LMAX 0x64 // Maximal 200, gleiche maximal Umdrehung für beide Seiten
#define RMAX 0x64 // Maximal 200

unsigned int i, j;
unsigned int lspeed, rspeed;
unsigned char taste1, taste2;
unsigned int odata[1];

int NachLinksAusweichen(void)
{
// LED einschalten
	BackLED(OFF,ON);
// Motor stoppen
	MotorSpeed(0,0);
// Rechtskurve zurück
	MotorDir(RWD,BREAK);
	MotorSpeed(120,0);
	Msleep(1000);
	MotorSpeed(0,0);
// LED ausschalten
	BackLED(OFF,OFF);
// Weiterfahren
	MotorDir(FWD,FWD);
	lspeed=LMIN;
	rspeed=RMIN;
	return 0;
}

int NachRechtsAusweichen(void)
{
// LED einschalten
	BackLED(ON,OFF);
// Motor stoppen
	MotorSpeed(0,0);
// Linkskurve zurück
	MotorDir(BREAK,RWD);
	MotorSpeed(0,120);
	Msleep(1000);
	MotorSpeed(0,0);
// LED ausschalten
	BackLED(OFF,OFF);
// Weiterfahren
	MotorDir(FWD,FWD);
	lspeed=LMIN;
	rspeed=RMIN;
	return 0;
}

int ZurueckSetzen(void)
{
// LED einschalten
	BackLED(ON,ON);
// Motor stoppen
	MotorSpeed(0,0);
// Linkskurve zurück
	MotorDir(RWD,RWD);
	MotorSpeed(LMAX,RMAX);
	Msleep(1000);
	MotorSpeed(0,0);
// LED ausschalten
	BackLED(OFF,OFF);
// Weiterfahren
	MotorDir(FWD,FWD);
	lspeed=LMIN;
	rspeed=RMIN;
	return 0;
}

int Aufgeben(void)
{
// LED einschalten
	BackLED(ON,ON);
// Motor stoppen
	MotorSpeed(0,0);
// LED ausschalten
	BackLED(OFF,OFF);
// Status LED ausschalten
	StatusLED(OFF);
}

int main(void)
{
	Init();
	
	// Vorwärts fahren
	MotorDir(FWD,FWD);
	// mit minimaler Geschwindigkeit beginnen
	lspeed = LMIN;
	rspeed = RMIN;
	do
	{
		// Eine Sekunde mit dieser Geschwindigkeit fahren 
		Msleep(1000);
		// Solange Geschwindigkeit erhöhen, bis maximal Geschwindigkeit erreicht ist
		if (lspeed < LMAX) lspeed++;
		if (rspeed < RMAX) rspeed++;
		MotorSpeed(lspeed,rspeed);
		OdometrieData(odata);  // Auslesen der Umdrehung
		PrintInt(odata[0]);    // Ausgabe auf serielle Schnittstelle
		PrintInt(odata[1]);    // Ausgabe auf serielle Schnittstelle
		taste1 = PollSwitch(); // Taster einmal abfragen
		taste2 = PollSwitch(); // Taster zweitesmal abfragen
		PrintInt(taste1);
		PrintInt(taste2);
		if (taste1 == taste2)  // beide Tastedrücke vergleichen
		{
			// Entscheiden, was zu tun ist
			if (taste1 == LA) 
				NachRechtsAusweichen();
			else if (taste1 == LM) 
				NachRechtsAusweichen();
			else if ((taste1 == LI) || (taste1 == RI))
				ZurueckSetzen();
			else if (taste1 == RM) 
				NachLinksAusweichen();
			else if (taste1 == RA) 
				NachLinksAusweichen();
			else
				Aufgeben();
		}
	} while (1);
	return 0;
}