Quelltext /~heha/mb-iwp/Bergwerk/fba-fw-230825.zip/koord.cpp

#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();
}
Vorgefundene Kodierung: UTF-80