Hallo Zusammen,

ich bekomme die Krise:

meine Routine arbeite nicht korrekt, wenn ich
ein Ziel im 3. Quadranten habe.

Sieht jemand den Fehler:

Code:
****************************************************************************/
/*		Berechne die neue Ausrichtung und wende den Roboter in die Richtung	 */
/****************************************************************************/

void Pit_NeueRichtung(Behaviour_t *data)
{
unsigned 	char		bMotorStatus;										// Status des letzten Kommando
float						fltDeltaX,fltDeltaY;								// Differenzpos. in der Karte
float						fltDrehWinkel;										// Tatsaechlicher Drehwinkel += Links -=Rechts 
float						fltDrehBogenStrecke;								// Strecke des Bogen
int						intSpeedLinks,intSpeedRechts;					// Geschwindigkeitsvorgaben
static float			fltWinkel;											// Zwischenwert fuer Drehwinkel
static   int			intDrehrichtung;									// Drehrichtung
static	unsigned int	intAnzahlDrehpulse;							// Anzahl der Drehimpulse
static	int			intDrehungAktiv=0;

if (intDrehungAktiv == 0)													// Ist eine Drehung aktiv
	{
	fltDeltaX = Roboter.AktZielPosX - Roboter.AktPosX;				// Berechne Differenz zum Ziel
	fltDeltaY = Roboter.AktZielPosY - Roboter.AktPosY;				// 

	fltWinkel = atan2( fltDeltaY , fltDeltaX ) * 180.0 / PI;		// Berechne kürzesten Winkel in WinkelWert
	intDrehrichtung=false;													// Drehrichtung auf links rum (standard) belegen

	if ( fltWinkel < 0 )														// Ist der Zielwinkel im 3 oder 4 Quadrant ?
		{
		fltWinkel*= -1;														// Winkel in Quadrant 1 oder 2 legen
		intDrehrichtung=true;												// Drehwinkel ist rechts rum zu sehen
		}
	
	Roboter.SollStrecke = sqrt((fltDeltaX * fltDeltaX) + 
										(fltDeltaY * fltDeltaY) ) * WEGPRONAVIGATIONSSTRECKE;		// Zu fahrende Strecke

											
	fltDrehWinkel= fabs(fltWinkel - Roboter.AktWinkel);				// Ermittle Winkeldifferenz (eigentlich Drehwinkel)
		
	fltDrehBogenStrecke= fltDrehWinkel * ((RADABSTANDINCM * PI) / 180.0);	// Berechne Drehstrecke
	intAnzahlDrehpulse = (int) (fltDrehBogenStrecke / WEGPROIMPULSINCM);		// Endcoder Impulse der Strecke

	if (fltDrehWinkel < 4.0 )												// Drehwinkel erreicht ? / oder innerhalb der Toleranz
		{
		Roboter.SollWinkel=fltWinkel;										// Sollwinkel vermerken
		Roboter.AktWinkel=fltWinkel;
		return_from_behaviour(data);										// Dann Verhalten abschalten
		return;																	// Funktion hier abbrechen
		}
	intDrehungAktiv=1;														// Drehen einleiten
	}
else
	{
	bMotorStatus=Motor.IsCommandActive();								// Erfrage Status von MPIC
	if ( !(bMotorStatus & STATUS_KOMMANDO) )
		{
		if ( intDrehungAktiv == 2 )										// Wurde Drehung bereits eingeleitet
			{
			intDrehungAktiv=0;												// Drehung fertig
			Roboter.SollWinkel=fltWinkel;									// Sollwinkel vermerken
			Roboter.AktWinkel=fltWinkel;
			return_from_behaviour(data);									// Dann Verhalten abschalten
			return;																// Funktion hier abbrechen
			}

		intDrehungAktiv=2;													// Drehung aktiviert
		if ( intDrehrichtung == false)
			{
			intSpeedLinks  = (0x80 - Roboter.AktSpeedLinks);		// Geschwindigkeit setzen
			intSpeedRechts = (0x80 + Roboter.AktSpeedRechts);		// Rechts rum drehen
			}
		else
			{
			intSpeedLinks  = (0x80 + Roboter.AktSpeedLinks);		// Geschwindigkeit setzen
			intSpeedRechts = (0x80 - Roboter.AktSpeedRechts);		// links rum drehen
			}

		Motor.Steps( intAnzahlDrehpulse, intAnzahlDrehpulse );	// Schrittwerte
		Motor.Speed( intSpeedLinks, intSpeedRechts );				// Geschwindigkeit setzen
		bMotorStatus=Motor.IsCommandActive();							// Erfrage Status von MPIC
		Motor.Go();
		while( bMotorStatus == Motor.IsCommandActive() )			// Warte bis Kommando angenommen
			usleep(200);    													// sleep fuer 200 ms
		}
	}
}
Für jede Hilfe wäre ich dankbar.

Gruss Ritchie