ZitierenHallo mare_crisium,
oh kopfschüttel,
klar einen senkrechten Vektor kann man ja genau so ausrechnen. (Ist ganz einfach, wenn man einen gut erkläten Tritt in den Hintern bekommt.)
Mit den Einheiten bin ich einverstanden. Auch bei uns läßt sich der Küchentisch noch locker in cm ausdrücken, ohne den "unsigned char"-Bereich zu verlassen.
Mit den Rr und Rl bzw. N(rl) und n(rl) komme ich zu dieser Uhrzeit noch nicht klar. Im Moment sehe ich in (1) nur, dass du (PI * Rr / Nr) ausklammerst aus den beiden Geschwindigkeiten vr und vl. Weiters Grübeln ist da bei mir angesagt.
Zielpositions-Abweichung: Gut gebrüllt Löve. Ähh, gut erklärt. Ist mir nun auch klar geworden. Danke.
Zum Reglungskonzept im Asuro kann ich nur auf den Beitrag von waste hier hinweisen. Den von waste erstellten Code kann man sehr schön als allround-Code nutzen. Das Geheimnis bleibt natürlich in den PID-Parametern für die entsprechende Aufgabe.
Hier ist mal meine aktuelle Version vom Regelcode:
Ist so angepasst, dass sowohl der ehemalige von waste programmierte Zweck zur Linienverfolgung, als auch eine Reglung für ein Geradeausfahren, nun möglich sind. Klar, der Aufrufer muss ein abgestimmtes Timing haben, sonst stimmen die zeitabhängigen PID-Parameter nicht zur Regelungsaufgabe. Da waste im 2ms-Raster rechnete, sollte das Ding halt alle 2ms aufgerufen werden.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; } /* Diese Parameter wurden von Sternthaler durch probieren ermittelt. */ kp = 65; ki = 6; 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 } } } /* 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; }
Aktuell geht es in dieser Version nicht, dass beide Regler gleichzeitig nutzbar sind, da innerhalb der Funktion die genuzten static-Variablen nur einmal vorhanden sind. Erzeugt man ein Array für die Variablen mit Variable/Parameter regler als Index, sollte das auch erledigt sein.
Ich sehe gerade, dass noch ein bisschen 'Beiwerk' fehlt.
- Variable hw.motor_diff kann mit 0 ersetzt werden.
- Variablen sens.linie[0-3] sind die automatisch im Interrupt ermittelten Messwerte der Liniensensoren für links/Rechts Hell-/Dunkelmessung
- Variablen sens.rad_tik[0-1] sind die, auch im Interrupt, ermittelten Hell-/Dunkelwechsel an den Odometriescheiben.
(Bei Bedarf poste ich den kompletten Code)
Mal sehen, wann wir google mit einem Asuro auf dem Mond überraschen
Eine Gute Nacht wünscht Sternthaler
/ 
und hatte dann Zeit genug, mir was zu Deiner Programmstruktur auszudenken 
. Ich hänge das kommentierte Programm als pdf-Datei an.
Lesezeichen