#include "fba.h"
#include <math.h> // M_PI
/******************
* Odometriedaten *
******************/
Odo odo NOINIT;
void Odo::lenkTo(int λ) { // Ziel-Winkel
unsigned Δλ = abs(λ-curλ);
if (Δλ<100) return; // Nicht rechnen bei weniger als 1° Bewegung
curλ = λ;
char cλ = idiv100r(λ); // Mit ganzen Grad weiterrechnen
byte λa = cabs(cλ), // vorzeichenlos
λb = λa/10, // 0..3
λ0 = λb*10, // Intervall-Untergrenze
λ1 = λ0+10, // Intervall-Obergrenze
λmin = cabs(eedata.lenkung.first().per125),
λmax = cabs(eedata.lenkung.last().per125);
if (λmax<λmin) λmax = λmin; // größeren von beiden
if (λ1 > λmax) λ1 = λmax; // Intervall verkleinern
unsigned
q0 = λb ? eedata.fz.q[λb-1] : 0,
q1 = eedata.fz.q[λb],
qa = q0 + MulDivU(λa-λ0,q1-q0,λ1-λ0);
q = cλ<0 ? -qa : qa; // Neuer Kurvenfaktor
}
/* Experimentelle Bestimmung von Q in der Tabelle eedata.fz.q:
Dieser ergibt sich aus den Tachoimpulsen TI(λ)
für einen Vollkreis bei gegebenem Lenkwinkel λ:
2π = TI/Q → Q = 2π/TI
Q ist in etwa proportional zum Lenkwinkel λ, deshalb genügen hier 4 Testkreise.
Hier: Vollkreis = 65536 => Q = 65536/TI.
*/
long Odo::lsin(long w,int α) {
constexpr float π = M_PI;
return w*::sin(α*π/32768); // TODO: Derzeit einzige Stelle mit float
}
// Warum 0,1 µm als Basis-Wegstrecke?
// 1. Eagle-Maßeinheit
// 2. Wertebereich ±200 m, das reicht gut
// 3. Bei kleinen Winkeln wenig Rundungsfehler
void Odo::on20ms(char n) { // Tachozählimpulse und Lenkwinkel zu neuer Position
if (!n) return;
long w = (long)n*eedata.fz.onestep;
x+= lcos(w,α>>8);
y+= lsin(w,α>>8);
α+= n*q; // Zweierkomplement-Überläufe sind gewollt!
}
| Detected encoding: UTF-8 | 0
|