Richtungsberechnung im kartesisches System

[Ritchie]

Hallo Zusammen,

ich bekomme die Krise:

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

Sieht jemand den Fehler:

****************************************************************************/
/*		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

[ogni42]

Ich _vermute_, dass Dein Weltkoordinatensystem nicht zu Deiner Winkelberechnung passt. Vorschlag: Teste mal, ob Dir die Zeile fltWinkel = atan2( fltDeltaY , fltDeltaX ) * 180.0 / PI; die Werte liefert, die Du für bestimmte DeltaY und DeltaX erwartest.

Stilvorschlag: Entweder immer Ungarische Notation benutzen oder nie (ich plädiere für nie, s. http://en.wikipedia.org/wiki/Hungarian_notation )

[Ritchie]

Hi,

gibt es den irgendwo ein Beispiel für das Ermitteln des korrekten Drehwinkels.

Derzeit habe ich immer das Problem, das ich falsche Werte bei überschreiten der Quadranten bekommen. Bevorzugt von 1.Q -3.Q
Oder von 1.Q. -> 4Q.

Gruss Ritchie

[PicNick]

Mich irritieren die "180" in deinem Code.
Normalerweise wird sowas im Bogenmaß gerechnet.

EDIT: Ich mach in solchen Fällen eigentlich immer zwei Rechenwege: ist DiffX < DiffY geh' ich über den Cosinus. Bei großen Winkeln verschwindet ja der Tanges im Nirwana, und der Rechner muß zu lange rechnen.

[Ritchie]

Hi,

die 180 sind zur Umrechnung auf Grad.

[PicNick]

Ah ja, 180/Pi *hirnklatsch*

[Ritchie]

Hallo Zusammen,

habe meinen Fehler gefunden.

Ursache: die Routinen arbeiten
a) mit Radiant, teilweise habe ich nicht alle Wert korrekt umgerechnet
b) atan2 arbeitet im Bereich von -PI -> PI,
ich hatte von 0- 2 PI gerechnet (Kompasswert umgerechnet)

Nachdem ich diese Probleme geändert hat, wird der Winkel korrekt berechnet.


Gruss RItchie