Algorithmen zur Bahnplanung

**Sternthaler** · 27.11.2007, 01:45

Hallo alle zusammen,
ja, es wird so langsam Weihnachten.

Genau aus diesem Grund, und weil der erste Ansatz nun im Asuro steckt, hier mal auf die schnelle das ganze Zeug.

Die rechnende Funktion als Ausschnitt aus asuro_st.c:

Code:

/*****************************************************************************
   FUNKTION:   NaviCalc
   Aufgabe:    Funktion zur Berechnung der Navigationswerte.
               Diese Funktion wird im ADC-Interrupt aufgerufen um die aktuellen
               Positions-Koordinaten und Richtung vom Asuro anhand der Daten
               der Odometrie zu berechnen.
               Ausgewertet werden die globalen Variablen count36kHz_l .._r
               Die Ergenisse werden in einer globalen Datenstruktur fuer die
               Anwendung zur Verfuegung gestellt.
   Parameter:  seite    Angabe, an welcher Rad-Seite der Tik gerade
                        aufgetreten war.
   Global:     count36kHz_l
               count36kHz_r
   V011     26.11.2007  Sternthaler
                        Funktion in erster Version
*****************************************************************************/
         unsigned char  NaviCalc (
         unsigned char  seite)
{
static            double speed_last_l = 0;
static            double speed_last_r = 0;
                  double speed_l;
                  double speed_r;
                  double speed, omega, zeit;
                  double omega_zeit = 0, speed_zeit = 0;
static            double phase_x = 0;
static            double phase_y = 1;
static            double pos_x = 0;
static            double pos_y = 0;

   if (seite == RECHTS)
   {
      speed_l      = speed_last_l;
      speed_r      = 10789.0 / count36kHz_r;
      speed_last_r = speed_r;
      zeit         = count36kHz_r / 36000.0;
      count36kHz_r = 0;
   }
   else
   {
      speed_l      = 10789.0 / count36kHz_l;
      speed_r      = speed_last_r;
      speed_last_l = speed_l;
      zeit         = count36kHz_l / 36000.0;
      count36kHz_l = 0;
   }

   /*
      Mittelere Geschwindigkeit
   */
   speed = (speed_r + speed_l) / 2;

   /*
      Drehwinkel. Die 10.36 sind der Radabstand.
   */
   omega = (speed_r - speed_l) / 10.36;

   /*
      Neue Phasenwerte berechnen
   */
   omega_zeit = omega * zeit;
   phase_x = phase_x - phase_y * omega_zeit;
   phase_y = phase_y + phase_x * omega_zeit;

   /*
      Und endlich auch die neuen X- und Y-Positonen
   */
   speed_zeit = speed * zeit;
   pos_x = pos_x + phase_x * speed_zeit;
   pos_y = pos_y + phase_y * speed_zeit;

   MD_RAD_POS_HIST ((int)(pos_x * 100), (int)(pos_y * 100));
   
   return 0;
}

Ich weiß. Noch wird mit Fließkomma gerechnet. Noch sind nicht alle notwendigen Rechenschritte vorhanden. Und auch bei der Berücksichtigung der beiden Tik-Zeiten-Zähler ist bestimmt noch ein dicker Fehler vorhanden.
Aber, das Ergebnis ist schon nicht allzu schlecht.

Die besten Zahlen stecken zur Anschauung im Excel-Diagramm.

@mare_crisium
Deine neueste Version V06 ist noch nicht gelesen. Ich schätze, dass ich somit um das Schätzen dann demnächst auch nicht herrumkommen werde. Ich schätze aber, dass dies auch wieder etwas dauern wird

.