// Ebene 5: PI-Regler für Geschwindigkeit und Hydraulikventile
#include "fba.h"
/*************
* PI-Regler *
*************/
int limitInt(long v) {
return limit<long>(v,32000);
}
Regler regler[4]; // wird (offenbar) per Konstruktor initialisiert!??
// Index 0: Lenkung, 1: Tempo, 2: Hubarm, 3: Löffel
Regler::Log Regler::log;
// Input: Fehler in Hundertstel-Winkelgrad
// Bspw. bei Istlenkwinkel -39° und Solllenkwinkel +39° = 7800
// D.h. Kp=1 bedeutet 1 PWM-Schritt für 2,56° Regelabweichung
// Output: Stellung des Hydraulikventils (gespreizt)
// Nicht integrieren wenn Ausgang auf Anschlag
// (Hinweis von Dr. Johannes Rudolph)
// 230825: Für Unterscheidung ob Slip-Stick oder veränderter Sollwert
// ist (mindestens) ein weiterer Parameter erforderlich!
char Regler::neu_soll(int e) {
auto li = log.current(); // Vier Integerwerte „zur freien Verfügung“
li.e = e;
bool plus = e>=0;
Eedata::PID&pi=eedata.pid[this-regler];
int min = pi.L[0+(plus<<1)]<<8, max = pi.L[1+(plus<<1)]<<8;
li.Σ = limit(add_sat(eΣ,e),min,max); // Überlaufsichere 16-Bit-Addition
int eω = eΩ;
li.Δ = add_sat(e,-eω); // Differenz-Anteil
if (!f.losgerissen && abs(li.Δ)>=pi.M) f.losgerissen=true;
eΩ = eω = (long(eω)*3+e)>>2; // Hier: Unendliche Impulsantwort, aber wenig filtern (schneller)
long y= (long)pi.Kp[plus]*li.e // Überlaufsichere 32-Bit-Addition
+(long)pi.Ki[plus]*shr6r(li.Σ) // 8 Bit rechtsschieben war für Fahrtregler ungeeignet. 17 Ergebnisbits.
+(long)pi.Kd[plus]*li.Δ; // zurzeit ungenutzt
int r = li.r = limitInt(y>>8); // ohne Spreizung aufzeichnen
log.savecurrent(); // Schreibzeiger erhöhen
// Da die Ausgangswirkung servotypisch nicht bei ±1 einsetzt
// (was für einen Regler elender Mist ist, Slip-Stick-Effekt)
// wird das Ausgangssignal gespreizt.
// Die Spreizwerte (im EEPROM) wurden experimentell ermittelt:
// Controller, fba3, Spannung 7..8 V (war unabhängig), aufgebockt.
// Im abgelegten Spreizbereich erfolgt „gerade so keine“ Reaktion.
if (r) {
r =limit<int>(r,f.vmax); // Geschwindigkeitsbegrenzung vor Spreizwert
r+=pi.Z[(r>=0)+(f.losgerissen<<1)]; // Spreizwert (vzb.)
}
if (r>=125) r=125; // Endgültig auf ±125 begrenzen
else if (r<=-125) r=-125;
else eΣ = li.Σ; // tatsächlich integrieren
return r;
}
void Regler::periodic() {
unsigned t=tout;
if (t) {
if (--t) {
tout=t; // rückschreiben
byte i=this-regler; // 0..3
// if (i!=1) {
// int n=
l2.set(i|L2::fRegler,
neu_soll(so*100-l4.lin[i]));
//-l2.pwms[i];
// TODO: Geschwindigkeit in Begrenzung der Regler-Summe einfließen lassen!
// Geschwindigkeit der PWM-Veränderung in Schritt/20ms
// TODO: Geschwindigkeit des Hydraulikzylinders einfließen lassen (D-Anteil)
// n=limit<int>(n,sp>0?sp:125);
// l2.set(i|L2::fAdd|L2::fRegler|1<<L2::bSlow,n);
// }else{
// char st = so*80/28; // Ungefähre Vorsteuerung (Leerlaufgeschwindigkeit bereits bei Servostellung 80)
// probeweise rein integrierend, ohne Zeitbegrenzung, langsam
// char e = so - idiv100r(l4.lin[1]); // Regelabweichung
// e = limit<char>(e,5); // langsam
// eΣ = limit<int>(eΣ+e,125);
// l2.set(i|L2::fRegler/*|1<<L2::bSlow*/,limit<int>(st+eΣ,125));
// }
}else reset(); // NaN anzeigen wenn Timeout erreicht wurde
}// sonst nichts tun
}
// <to> in Zehntelsekunden mit folgenden Sonderbedeutungen:
// 0: Regler beenden
// 0xFC: Zeit unendlich
// 0xFE: Zeit nicht beeinflussen
// 0xFF: Wie 0 aber ohne zugehörige PWM-Ausgänge zu beeinflussen
// (zum Rekursion vermeiden bei Aufruf aus L2::set())
void Regler::reset(byte to,bool cont) {
byte i=this-regler; // 0..3
if (!tout || !cont) {
eΣ = 0; // Das wichtigste zuerst: Integrierglied nullsetzen
eΩ = so*100-l4.lin[i]; // Altwert mit ungefilterter Regelabweichung starten
f.losgerissen = false;
if (!to || to==0xFF) {
if (to!=0xFF) { // Bei Aufruf aus L2::set() keine Rekursion!
if (i==0) {
// Katastrophal: Lenkregler auf Anschlag _und_ Fahrzeug in Fahrt:
// Hydraulikpumpe oder Steuerventil (oder sonstwas) kaputt! Nicht weiterfahren!
if (!to && cabs(l2.pwms[0])>=100 && l2.pwms[1]) {
l2.set(1,0); // ggf. Fahrtregler sowie Scheduler abwürgen
eedata.debug[11]++;
}
}
if (i!=1) l2.set(i|L2::fRegler,0); // Hydraulikventil schließen aber Fahrt beibehalten
}
so=cNaN;
to=0;
}
}
if (to==0xFD) tout=3*60*50; // Unendlich? Nein: 3 Minuten (9000 Ticks)
else if (to!=0xFE) tout=to*5; // Timeout nicht setzen
}
// liefert true wenn Regelziel erreicht
bool Regler::set(byte i,char so, char vmax) {
byte k = i&3;
Regler®=regler[k];
char b = reg.so;
// Falls Konstruktor nicht ausgeführt, hier nachholen
if (!reg.tout) reg.so = b = cNaN;
// if (!reg.sp) reg.sp = 10;
if (so==cNaN) {
if (b!=cNaN) reg.reset();
return true; // NaN als „Regelziel erreicht“ durchlaufen lassen
}
if (b==cNaN) b=idiv100r(l4.lin[k]); // Für Addition vom aktuellen Istwert (!!) ausgehen
int c = so;
if (i&fAdd) c+=b;
char min=eedata.fz.sp[0], max=eedata.fz.sp[1];
switch (k) {
case 0: min=eedata.lenkung.min();max=eedata.lenkung.max();break;
case 2: min=eedata.hubarm.min(); max=eedata.hubarm.max(); break;
case 3: min=eedata.löffel.min(); max=eedata.löffel.max(); break;
}
byte Δ = max-min; // großes Δ = ganzer Stellweg (in Winkelgrad oder cm/s)
c = limit<int>(c,min,max); // Niemals Ziel außerhalb Stellbereich
byte δ = c>=b ? c-b : b-c; // kleines δ = dieser Stellweg = erste Regelabweichung
if (!δ) return true; // Abkürzung: Regelung nicht erforderlich
reg.so = c; // Regler setzen oder ändern
if (vmax>0) reg.f.vmax = vmax; // Nur positive Werte akzeptieren, cNaN ist hier negativ
byte t = eedata.pid[k].T; // Maximale Regelzeit in Zehntelsekunden für maximalen Stellweg
if (!t) { // unendlich
reg.reset(0xFD,true); // Kode für unendlich
}else{
// TODO: reg.f.vmax in die Zeitbegrenzung einfließen lassen (t wird größer)
if (t>=10) t-=10; // Mindestzeit 1 s vor Skalierung abziehen
t = t*δ/Δ; // skalieren (= wird kleiner, alles vorzeichenlos)
t+=10; // Mindestzeit 1 s
if (t>0xFC) t=0xFC; // Nicht in Geheimkodes hineinrutschen
reg.reset(t,true); // Endliche Zeit setzen
}
return false;
}
void Regler::on20ms() { // Aufrufen wenn neue Messwerte in l4.lin vorliegen
for (byte i=0; i<elemof(regler); i++) regler[i].periodic();
}
| Detected encoding: UTF-8 | 0
|