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:
Code:
/*****************************************************************************
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:
Code:
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);
}
}
Zitieren
Hoffentlich liegt das Ziel auch am Weg 
Lesezeichen