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* :oops:

[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