Richtungsberechnung im kartesisches System
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
