Hallo hai1991
Oh Mist, da gebe ich mausi_mick den Tipp seine Variablen durch Defines zu ersetzen, und die von dir gemachten Defines habe ich übersehen.
Ja, dein Hinweis löst genau das was ich übersehen hatte.
Grüß dich mare_crisium,
was machst du bei den Asurianern 
? Hast du nun auch einen?
Und wie immer ein guter Vorschlag von dir. (Ist ja ein echter Regler)
Ähm, in deinem Text schreibst du, dass hier aktuell nicht geregelt würde.
- while((difl < 10) && (difr < 10))
- {
--- OdometrData(dalr);
--- Auswerten der beiden ODO-Werte aus dalr[2] und ermitteln einer
--- Geschwindigkeitsänderung
--- ...
Das ist doch auch ein Regler? (Vorgabe / Messwerte / Abweichung -> Ergibt neue Stellwerte)
Und ein schuldbewustes: Ja, Ja, ich sollte eigentlich X, Y und Omega berechnen .
Und noch mal etwas zu hai1991.
Deine Suche nach einem Regler für den Asuro wird dich unweigerlich zu wastes linienverfolgendem PID-Regler führen.
Genau den habe ich schon seit vielen, vielen Äonen zum Kurven-/Geradefahren genutzt.
Ein paar experimentelle Anpassungen an den Reglerparametern (wie mare_crisium es Dummys wie mir, bei solchen Dingen empfiehlt). Ein weiterer kleiner Umbau darin erlaubt es auch noch, dass man nicht auf das Ende der Fahrt warten muss solange das Hauptprogramm das Ding regelmäßig alle 2ms aufruft.
Dazu aber nur den Codeausschnitt vom Regler. Schließlich geht es hier um deinen Ansatz.
Code:
/*****************************************************************************
FUNKTION: MotorPID
Aufgabe: Funktion zur Reglung der Motoren.
Diese Funktion bzw. die eigentliche 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 = 65;
ki = 5;
kd = 90;
}
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
}
}
}
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;
}
Diese Funktion muss alle 2ms vom Hauptprogramm aus aufgerufen werden, da waste damals diesen Zeitfaktor in seinen Berechnungen zur Reglerdimensionierung angesetzt hatte.
Oder innerhalb der Funktion wird ein while() mit Msleep(2) eingebaut.
Die struct-Variable sens enthält die Messwerte der Sensoren, die per Interrupt immer aktuelle Werte enthalten. Sie 'verbrauchen' innerhalb der Funktion keine Rechenzeit.
Ein Aufruf erfolgt mit Tik-Angaben bei den Parametern links und rechts. Eine Umrechnung von Strecke bzw. Winkel und Radius und Vorwärts/Rückwärts muss vorher erfolgen.
Tut mir leid, wenn ich euch das bis jetzt verschwiegen habe. Aber dein Ansatz hai1991 soll ja nicht durch 'Fertigfutter' beeinflusst werden fand ich.
@mausi_mick
Dann lass den Krimi doch zu Hause
Gruß Sternthaler
Zitieren
.
Lesezeichen