-         

Ergebnis 1 bis 7 von 7

Thema: Richtungsberechnung im kartesisches System

  1. #1
    Erfahrener Benutzer Begeisterter Techniker
    Registriert seit
    07.11.2004
    Beiträge
    332

    Richtungsberechnung im kartesisches System

    Anzeige

    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
    Kaum macht man es richtig, schon funktioniert's ...

  2. #2
    Erfahrener Benutzer Roboter Genie
    Registriert seit
    21.10.2005
    Ort
    Erde
    Alter
    50
    Beiträge
    1.195
    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 )

  3. #3
    Erfahrener Benutzer Begeisterter Techniker
    Registriert seit
    07.11.2004
    Beiträge
    332
    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
    Kaum macht man es richtig, schon funktioniert's ...

  4. #4
    Super-Moderator Robotik Visionär Avatar von PicNick
    Registriert seit
    23.11.2004
    Ort
    Wien
    Beiträge
    6.836
    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.
    mfg robert
    Wer glaubt zu wissen, muß wissen, er glaubt.

  5. #5
    Erfahrener Benutzer Begeisterter Techniker
    Registriert seit
    07.11.2004
    Beiträge
    332
    Hi,

    die 180 sind zur Umrechnung auf Grad.
    Kaum macht man es richtig, schon funktioniert's ...

  6. #6
    Super-Moderator Robotik Visionär Avatar von PicNick
    Registriert seit
    23.11.2004
    Ort
    Wien
    Beiträge
    6.836
    Ah ja, 180/Pi *hirnklatsch*
    mfg robert
    Wer glaubt zu wissen, muß wissen, er glaubt.

  7. #7
    Erfahrener Benutzer Begeisterter Techniker
    Registriert seit
    07.11.2004
    Beiträge
    332
    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
    Kaum macht man es richtig, schon funktioniert's ...

Berechtigungen

  • Neue Themen erstellen: Nein
  • Themen beantworten: Nein
  • Anhänge hochladen: Nein
  • Beiträge bearbeiten: Nein
  •