links/rechts Antriebverhältnis für Kreisbogenfahrt

[Sternthaler]

Hallo zusammen,

genau zu dem mathematischen Ergebnis bin ich auch schon vor ein 'paar' Tagen beim Asuro gestoßen.
Ich hatte das mal in EXCEL gegossen und berücksichtigt auch die Geradeausfahrt. Ist nun im Anhang.

Der oben angegeben Ausdruck "Raddurchmesser) / 3" ist bei mir in einem RadFaktor gelandet und steht im 2ten blau markierten Feld im EXCEL-Kopf. (Rechts die beiden blauen Felder sind nur der Faktor in Werte unter 255 gerechnet. Bei der Geometrie vom Asuro kommt man so mit einer schnellen Byte-Rechnerei aus.)

Interessant ist noch das rechts stehende grüne Feld.
Dort kann man einen Prozentwert eintragen, der Fehler bei der Ermittlung der Drehgeber-Impulse 'schönrechnet'. (Bei meinem Asuro komme ich da mit einem Wert von 97 zum gewünschten Ergebis.)

Um beide Motoren zu regeln, damit sie die Vorgaben in gleicher Zeit abfahren, hatte ich, auch schon vor ein 'paar' Tagen, den waste-Linienverfolgungsregler so angepasst, das der sich darum kümmert.
Hier mal der Reglercode:

/*****************************************************************************
   FUNKTION:   MotorPID
   Aufgabe:    Funktion zur Reglung der Motoren.
               Diese Funktion bzw. die eigendliche Berechnung fuer die
               Reglung wurde im Forum unter www.roboternetz.de von waste
               entwickelt.
               Von Sternthaler ist die Moeglichkeit hinzugefuegt, die
               Reglerberechnung fuer unterschiedliche Regelaufgaben zu
               nutzen.
               Die Parameter steuern, welche Sensoren zur Reglung benutzt
               werden sollen. Zum einen koennen es die Liniensensoren sein
               um eine Linienverfolgung zu realisieren, zum anderen kann
               der Regler zur Ueberwachung der Raddecoder genutzt werden.
   Parameter:  
*****************************************************************************/
         unsigned char  MotorPID (
         unsigned char  regler,
                  char  speed,
                  int   links,
                  int   rechts)
{
                  int   LWert = 0, RWert = 0;
                  int   absLinks = 0, absRechts = 0;
                  float faktor;
static            int   x, x1, x2, x3, x4;
static            int   xalt, drest, isum;
                  int   kp = 0, kd = 0, ki = 0;
                  int   yp, yd, yi, y, y2;
                  int   LSpeed,	RSpeed;
         unsigned char  LDir,   RDir;
         unsigned char  use_regler = TRUE;

   switch (regler)
   {
   case PID_LINIE:
      links = rechts = 1;                 // erzwingt vorwaertsfahrt
      LineData ();                        // Liniensensoren
      LWert = sens.linie [LINKS_DUNKEL] - sens.linie [RECHTS_DUNKEL];
      RWert = sens.linie [LINKS_HELL]   - sens.linie [RECHTS_HELL];
      /* DIESE PARAMETER WURDEN VON waste IM FORUM UNTER
         https://www.roboternetz.de
         ENTWICKELT.
      */
      kp = 5;              // Parameter kd enthält bereits Division durch dt
      ki = 5;
      kd = 70;
      break;
   case PID_ODOMETRIE:
      if (links == 0 || rechts == 0)
         use_regler = FALSE;
      else
      {
         absLinks  = abs (links);
         absRechts = abs (rechts);
         /* Odometrie-Zaehler so justieren, dass fuer eine Kurvenfahrt
            die Tic-Anzahl auf beiden Seiten identisch aussehen.
            Die Seite auf der weniger Tic's zu fahren sind wird auf die
            hoehere Anzahl 'hochgerechnet'.
         */
         if (absLinks < absRechts)
         {
            faktor = (float)absRechts / (float)absLinks;
            LWert = sens.rad_tik [LINKS] * faktor;
            RWert = sens.rad_tik [RECHTS];
         }
         else
         {
            faktor = (float)absLinks / (float)absRechts;
            LWert = sens.rad_tik [LINKS];
            RWert = sens.rad_tik [RECHTS] * faktor;
         }
         kp = g_kp;
         ki = g_ki;
         kd = g_kd;
      }
      break;
   }

   LSpeed = (int)(speed - hw.motor_diff / 2);   //Wunschgeschwindigkeit vorgeben
   RSpeed = (int)(speed + hw.motor_diff / 2);   //Hardware beruecksichtigen

   if (use_regler == TRUE)
   {
      /* AB HIER IST DIE BERECHNUNG VON waste IM FORUM UNTER
         https://www.roboternetz.de
         ENTWICKELT WORDEN.
      */
      x1 = RWert - LWert;                 // Regelabweichung

      x = (x1 + x2 + x3 + x4) / 4;        // Filtert die 4 letzten Werte
      x4 = x3; x3 = x2; x2 = x1;          // Pipe ueber die letzten 4 Werte

      isum += x;                          // I-Anteil berechnen
      if (isum >  16000) isum =  16000;   // Begrenzung: Überlauf vermeiden
      if (isum < -16000) isum = -16000;
      yi = isum / 625 * ki;

      yd = (x - xalt) * kd;               // D-Anteil berechnen und mit nicht
      yd += drest;                        // berücksichtigtem Rest addieren
      if (yd > 255) drest = yd - 255;     // Eventuellen D-Rest merken
      else if (yd < -255) drest = yd + 255;
      else drest = 0;

      yp = x * kp;                        // P-Anteil berechnen

      y = yp + yi + yd;                   // Gesamtkorrektur
      y2 = y / 2;                         // Aufteilung auf beide Motoren
      xalt = x;                           // x merken

      if (y > 0)                          // Abweichung nach rechts
      {
         LSpeed += y2;                    // links beschleunigen
         if (LSpeed > 255)                // wenn Wertebereich ueberschritten
         {
            y2 += (LSpeed - 255);         // dann Rest rechts berücksichtigen
            LSpeed = 255;                 // und Begrenzen
         }
         RSpeed -= y2;                    // rechts abbremsen
         if (RSpeed < 0)                  // Auch hier Wertebereich
         {
            RSpeed = 0;                   // beruecksichtigen
         }
      }
      if (y < 0)                          // Abweichung nach links
      {
         RSpeed -= y2;                    // rechts beschleunigen
         if (RSpeed > 255)                // wenn Wertebereich ueberschritten
         {
            y2 -= (RSpeed - 255);         // dann Rest links berücksichtigen
            RSpeed = 255;                 // und Begrenzen
         }
         LSpeed += y2;                    // links abbremsen
         if (LSpeed < 0)                  // Auch hier Wertebereich
         {
            LSpeed = 0;                   // beruecksichtigen
         }
      }
   }

   /* Und wieder (fast) waste
   */
   if (links >0) LDir = FWD; else if (links <0) LDir = RWD; else LDir = BREAK;
   if (rechts>0) RDir = FWD; else if (rechts<0) RDir = RWD; else RDir = BREAK;

   if (LSpeed < 20) LDir = BREAK;         // richtig bremsen
   if (RSpeed < 20) RDir = BREAK; 
   MotorDir   (     LDir,         RDir);
   MotorSpeed (abs (LSpeed), abs (RSpeed));

   return 0;
}

Folgende Variablen sind zu berücksichtigen:
- sens.rad_tik [LINKS | RECHTS ] : Aktuelle Zählerstände der Drehgeber
- g_kp, g_ki, g_kd : Globale PID-Werte. (Bei meinem Asuro 65, 5, 90
- hw.motor_diff : Kann 0 sein. Berücksichtigt unterschiedliche Motoren

Die benutzten Defines sollten selbsterklärend sein

Ach so, wichtig wäre es noch zu erwähnen, dass die Reglerfunktion alle 2 ms aufgerufen werden sollte. Das Timing ist von waste mal so berechnet worden.

Viel Erfolg mit der Kurvenfahrt.
Sternthaler

P.S.: Hier auch noch mal eine Aufrufstelle der Regler-Funktion:

         if (v_fahren == TRUE)
         {
            MotorPID (
               PID_ODOMETRIE,    // Reglerwahl
               150,              // Mittlere Geschwindigkeitsvorgabe
               v_weg_l,          // Anzahl Takte linkes  Rad (Odometrie)
               v_weg_r);         // Anzahl Takte rechtes Rad (Odometrie)

            if (sens.rad_tik [LINKS]  >= v_weg_l  &&
                sens.rad_tik [RECHTS] >= v_weg_r)
            {
               v_fahren = FALSE;
               v_weg_l = 0;
               v_weg_r = 0;
               MotorSpeed (0,     0);
               MotorDir   (BREAK, BREAK);
            }
         }