#include "fba.h"
/*********************************
* Fahrt um bestimmte Wegstrecke *
*********************************/
static L7 l7[16] NOINIT; // Ringpuffer
byte L7::ri,L7::fill;
unsigned L7::lastuv;
byte L7::wi() {byte r=ri+fill; if (r>=elemof(l7)) r-=elemof(l7); return r;}
bool L7::full() {return fill>=elemof(l7);}
void L7::eat() {if (!fill) return; --fill; byte i=ri+1; if (i>=elemof(l7)) i=0; ri=i;}
L7*L7::prepare() {
if (fill>=elemof(l7)) return 0; // nichts frei
L7&el = l7[wi()];
memset(el.soll,cNaN,8); // Regler mit NaN initialisieren
memset(&el.s,0,6); // Alles andere mit 0 initialisieren
return ⪙
}
void L7::push() {++fill;}
//void L7::purge() {lastuv=fill=0;}
// Ziel dieses „selektiven purge“ ist, dass während einer Fahrt
// mit Streckenbegrenzung das Lenken möglich bleibt,
// damit man sich per 3D-Marker und Level5-Zugriff
// durch die Tore „schlängeln“ kann.
// Hingegen der Zugriff auf den Fahrtregler oder den Fahrservo
// beendet eine begrenzte Fahrt, und die Fahrt geht unendlich.
// (Global per Totmannfunktion auf 3 Minuten begrenzt,
// aber auch das Betätigen von Hydrauliken und Licht
// setzt den Totmannzähler zurück.)
void L7::purge(byte k) { // k = 0..5
if (!fill) return;
if (k==4) k=1; // gleichgeschaltet
L7&el = l7[ri];
if (k<4 && el.soll[k]!=cNaN) {
regler[k].reset(); // beenden falls laufend
el.soll[k]=cNaN; // Beim nächsten L7::on20ms() "complete"
}
if (k==1) {
el.s=0; // Fahrt beenden
lastuv=0;
}
if (k==5) el.w=0;
}
// Integer-Wurzelzieh-Algorithmus ohne Rest
// Quelle: Englische Wikipedia
static unsigned ulsqrt(unsigned long n) {
unsigned long c = 0, d = 1UL << 30;
while (d>n) d>>=2; // Erst mal „schnell“ Startbitposition finden
while (d) {
auto k = c+d; // Addition Bit + linksverschobenes Ergebnis
c >>= 1;
if (n >= k) {
n -= k;
c += d; // Bit setzen
}
d >>= 2;
}
return c; // Hier sollte <c> und <n> nur noch 16 Bit groß sein.
}
// Im Gegensatz zum restlichen Programm, das wegen der Anzeige
// in Hundertstel (= Dezimal-Festkommaformat) rechnet,
// wird hier im Binär-Festkommaformat Q8.8 und ähnlich gerechnet.
// Basiseinheiten sind Sekunde und Zentimeter.
// Alle derartigen Festkommazahlen sind hier vorzeichenlos.
// Berechnet die Geschwindigkeit am Wegpunkt s,
// wenn von x=0 und v=0 mit Beschleunigung a beschleunigt wird.
// Hier für den zielgenauen Bremsvorgang.
// Die Lehrbuchformel: v = √(2as)
// Hier: s in Puls, a in 50cm/s² Q1.6, Ergebnis v in cm/s Q9.7
// Im EEPROM steht in fz.onestep der Umrechnungsfaktor in 0,1 µm/Puls
static unsigned v_after(unsigned s, byte a) {
// Wie in fernbed.cpp:118: Umrechnung in cm, aber als Q16.8, nicht Q16.0
unsigned long s2 = s;
s2*=168; // 168 ≈ 2^24/100000, passt in 24 Bit
s2>>=8; // passt in 16 Bit
s2*=eedata.fz.onestep; // in cm als Q16.16
s2>>=8; // in cm als Q16.8, passt in 24 Bit
s2*=a/*<<1*/; // Q16.8 × Q2.6 = Q18.14
return ulsqrt(s2*a); // √Q18.14 = Q9.7
}
// Die Abweichung sollte aus √50 ~ 7 herrühren, ist aber kleiner.
void L7::on20ms(char wég) {
if (!fill) return;
bool complete=true;
L7&el = l7[ri];
long s = el.s;
// Weg-Abarbeitung im Trapezprofil, ohne Folgeauftragverkettung
// Aufrufrate garantiert 50 Hz, 20 ms (wichtig für Beschleunigung und Abbremsen)
if (s) {
char vmax = el.soll[1], amax = el.speed[1];
if (vmax==cNaN) vmax=0; // ohne Geschwindigkeitangabe (besser) vorwärts fahren
if (!vmax) vmax=16; // Defaultgeschwindigkeit 16 cm/s
if (amax<0) amax=-amax;
if (amax<0) amax=0; // niemals cNaN, das gibt Riesenärger
if (!amax) amax=16; // amax positiv, Defaultbeschleunigung 12,5 cm/s²
// vmax-Einheit: cm/s Q8.0
// amax-Einheit: cm/s/20ms = 50cm/s² Q1.6, amax=126 ist 94 cm/s², amax=1 ist <1 cm/s²
if (s<0) {s=-s; vmax = -vmax;}// s positiv, vmax gibt Richtung an
if (s<0) s=0; // falls ein Rindvieh lNaN angegeben hat: Nicht fahren!
vmax = limit<char>(vmax,eedata.fz.sp[0],eedata.fz.sp[1]);
s-= cabs(wég);
if (s<0) s=0;
unsigned uv = lastuv; // „Istwert“ in cm/s als Q8.8
uv += amax<<2; // Beschleunigungsversuch (Q8.8 + Q3.8)
unsigned uvmax = abs(vmax)<<8;// Maxwert in cm/s als Q8.8 (unten Nullbits)
if (uv>uvmax) uv = uvmax; // Erste Begrenzung: Plateauphase
if (s<65536) {
uvmax = v_after(s,amax);
// Irgendwo ist noch ein Denk/Rechenfehler!! Bremsrampe viel flacher als Beschleunigung
// Mit den falschen Schiebeweiten in v_after und uvmax<<2 statt <<1 haut es gut hin.
if (uvmax<1<<8+4) { // Q9.7 passt in Q7.7?
uvmax<<=2;/*BUGBUG*/ // In Q8.8 umwandeln
if (uv>uvmax) uv = uvmax; // Zweite Begrenzung: Bremsphase
}
}
lastuv = uv; // ablegen für nächste Runde
char v = uv>>8;
if (s && !v) ++v; // Notwendige Bewegung erzwingen
if (vmax<0) v=-v;
if (v) Regler::set(1,v,125); // Regler in cm/s setzen
else{
regler[1].reset(); // Regler stoppen (hält bei Kanal 1 nicht an)
l2.set(1|L2::fRegler,0); // anhalten ohne purge() (wäre fatal)
}
el.soll[1] = vmax; // Korrigierte Geschwindigkeit + Richtung retten
el.speed[1] = amax; // Korrigierte Beschleunigung retten
el.s = s; // Restweg verkürzen, ab hier stets positiv
if (!s) el.soll[1]=cNaN; // Keine anschließende Reglerkontrolle
}
if (s) complete=false;
// Regler kontrollieren
for (byte k=0; k<4; k++)
if (!(s && k==1) // Bei Weg-Abarbeitung ist Regler 1 unter Kontrolle des Rampenprofil-Generators
&& el.soll[k]!=cNaN
&& !Regler::set(k,el.soll[k],el.speed[k])) // Regelziel setzen (zumeist unverändert)
complete=false;
// Warten
unsigned w = el.w;
if (w && !--w) complete=false;
el.w = w;
if (complete) eat();
}
| Detected encoding: UTF-8 | 0
|