#pragma once
#include <avr/io.h>
#include <avr/interrupt.h>// cli, sei
#include <string.h> // memset
typedef unsigned char byte;
#define nobreak [[fallthrough]]
#define elemof(x) (sizeof(x)/sizeof(*(x)))
#define NOINIT __attribute__((section(".noinit")))
// 4 Register für den C-Compiler sperren
register unsigned saveR24 asm("r2");
register unsigned saveR30 asm("r4");
#define t0cnt GPIOR1 // zum Zählen der Pulse an OC0B
#define spdCnt GPIOR2 // Impulszähler an INT1
/**************************
* Schicht 0: Echtzeituhr *
**************************/
extern struct Uhr{ // sizeof = 6
byte ms10; // 10-ms-Schritte (0..99)
uint32_t losec;// Sekunden (typisch seit 1.1.1970 0:00)
char hisec;// Platz für Überlauf sowie für Prähistorie
void operator++(); // 50 Hz zählen (Prä-Inkrement)
operator byte*() {return (byte*)this;}
void reset() {memset(this,0,sizeof*this);}
}uhr;
static_assert(sizeof uhr==6);
// Die 5 Bytes für ganze Sekunden genügt für die Kalenderjahre -15462 .. 19402
// 4 Bytes (vzb.) hingegen reichen nur bis zum 19. Januar 2038.
/*********************
* Schicht 1: EEPROM *
*********************/
template<byte N>struct CalibData{
byte n; // mindestens 2, maximal N
struct A{ // muss Array Of Struct sein für variable N
int16_t adcvalue; // streng monoton steigend!
int8_t per125; // monoton steigend oder fallend
}a[N]; // Muss letztes Element von CalibData sein
int16_t toLin(int16_t);// Wandelt A/D-Wert in Gradangabe (Hundertstelgrad)
inline A const&first() const {return a[0];}
inline A const&last() const {return a[n-1];}
inline char min() const {char a=first().per125, b=last().per125; return a<b?a:b;}
inline char max() const {char a=first().per125, b=last().per125; return a>b?a:b;}
static_assert(N>=2);
};
extern struct Eedata{ // Gesamtlänge 0x70 = 112
byte lamps; // [1] @ 0
byte i2caddr; // [1] @ 1
CalibData<11>lenkung; // [34] @ 2
CalibData<3> hubarm, // [10] @24
löffel; // [10] @2E
int t0,rsv[3]; // [8] @38 0-°C-Wert für Temperatursensor; frei
struct PID{ // [32] @40 // Alle Werte doppelt, für Minus- und Plus-Abweichung
char Kp[2], // Proportional-Faktor (minus-plus)
Ki[2], // Integrier-Faktor (minus-plus)
Kd[2], // Differenzier-Faktor (minus-plus)
Z[4], // Spreizung der Hydraulik (minus-plus Losreißen, minus-plus InFahrt)
L[4]; // Limit für Integrier-Word (min-max-minus, min-max-plus)
byte T, // Maximalzeit in 1/10 s, 0 = unendlich (für Fahrtregler)
M; // Schwellwert zum Erkennen des Losreißens
}pid[4]; // Reglerdaten für Geschwindigkeit und Hydrauliken
struct Fz{ // @80
char sp[2]; // [2] @80 Maximalgeschwindigkeit rückwärts/vorwärts in cm/s
unsigned onestep; // [2] @82 Fahrweite in 0,1 µm für 1 Motordrehimpuls
unsigned q[4]; // [8] @84 Winkeländerung in π*2^-23 für 10°, 20°, 30° und maximalen Lenkwinkel
int rsv[2]; // [4] @8C
}fz; // Fahrzeugkonstanten
byte debug[32]; // [32] @90
// Debug-Zähler
// 0 Power-On-Reset (Einschalten)
// 1 Externes Reset (vom Programmiergerät)
// 2 Brown-Out-Reset (Spannung nicht ganz weg)
// 3 Watchdog-Reset (nie)
// 4 RxD: Falsche Prüfsumme
// 5 RxD: Zeitlücke detektiert (erstmal kein Fehler)
// 6 RxD: Pufferüberlauf
// 7 RxD: Falsches Längenbyte (<4 oder >32)
// 8 ADC: Nicht früh genug fertig (nie)
// 9 PWM: Zeitproblem in ISR (Murks! Tritt auf! Interrupts >400µs gesperrt!?)
// 10 PWM: Letztes pwmidx
// 11 Regler: Notbremse vom Lenkregler
// 12 I²C: Unverarbeiteter Status
// 13 I²C: TWSR beim letzten unerwarteten Status
// 14 I²C: „SCL Low Timeout“: I²C-Neustart (Tritt auf bei Taktfehlern vom RPi auf.)
// 15 I²C: Status == 0xF8 (Dummy)
// 16 I²C: Lesedaten unvollständig gelesen (ungenutzt)
// 17 I²C: Antwortpuffer leer oder EEPROM nicht bereit: Sende 0xFF
// 18 I²C: Schreibpuffer voll
inline void read(); // Statische Konstruktoren fressen mehr Flash
void update();
operator byte*() {return (byte*)this;}
}eedata;
static_assert(sizeof eedata==176); // 230418: Umstrukturierung!
#define eedebug (eedata+0x90)
/* Ungefähre Fahrzeugkennwerte (Radlader):
* Zählimpulse pro Motorumdrehung: 4? (max. 400 Hz)
* Erste Getriebeübersetzung: 4:1
* Differenzialgetriebeübersetzung: 10:1
* Raddurchmesser: 140 mm, Umfang 440 mm
*/
/****************************
* Schicht 2: PWM-Generator *
****************************/
// Mit dem '4017 kommt man (bei gleich bleibender Wiederholrate von 50 Hz)
// nur auf 9 Kanäle; der zehnte Ausgang „schluckt“ den Rest der Zeit.
// Alle (auch die negativen) Flanken werden per Mikrocontroller-Hardware generiert
// und in der ISR von Flanke zu Flanke vorberechnet.
// Dafür muss die maximale Interruptsperrzeit unter 500 µs liegen.
template<class T> static constexpr T limit(T c, T minv, T maxv) {
return c<minv ? minv : c>maxv ? maxv : c;
}
template<class T> static constexpr T limit(T c, T maxv) {
return limit<T>(c,-maxv,maxv);
}
extern struct L2{
char pwms[9];
byte pwmok; // PWM generieren solange !=0, dekrementiert mit 1 Hz
// … von 190 bis 10, dann mit 50 Hz
static void countdown();
enum{ // Flags (High-Nibble) für Kanalbyte (Low-Nibble)
fAdd =0x10, // Addieren statt setzen (absichtlich Bit 4!)
fRegler=0x20, // Vom Regler: Regler nicht abschalten (Nur für Kanalindex 0..3)
bSlow =6, // Bit 7~6: Stellgeschwindigkeit begrenzen (Servomotoren schonen)
}; // 0b00 = unbegrenzt, 0b01 = 5/20ms, 0b10 = 10/20ms, 0b11 = 20/20ms
void set(byte,char); // setzt Kanal auf Wert, cNaN übergehend,
// und setzt pwmok auf 10+60×3 = 190 (3 Minuten; Totmann-Funktion)
// Wird aufgerufen von:
// * Original-Controller: L3::onIbusRx() -> L3::toServo() (nur bei Änderung)
// * Regler
// * I²C-Bus
// * Pampelmuse
// * A/D-Wandler (nur auf Kanal 1 = Fahrmotor wirkend):
// - Bei Rückwärtsfahrt und Rückfahrtaster-Kontakt: Stopp
// - Bei Vorwärtsfahrt und Ultraschall-Entfernung < 5 cm: Stopp
// Bei aktivem Notaus passiert nichts!
static void t1init(); // in isr.S
// Nachträglich eingebaut wurde der Impulsgenerator und die Pulslängenerfassung
// zum Tracking des Magic Controllers, d.h. der Schaltzustände von Licht und Zündung
// Während der Rundumleuchte ein Einzelpuls zum Weiterschalten genügt,
// benötigt der Magic Controller eine „Mindestlänge“ = einige kurze (1 ms)
// bzw. lange (2 ms) Pulse für den Schaltvorgang.
// Dazu, am Kanal 7, eine „lange Mindestlänge“ für die Blinker-Schaltebene.
// Gesteuert wird das (via SPI oder I²C) über eine Hintertür im Level 4.
static byte k7hold;
static void k7hold_handler();
static byte autopuls[6]; // Automatischer Pulsgenerator für alles was „zählt“
// 0 Kanal 6- (Zündung)
// 1 Kanal 7- (Licht Fahrzeug)
// 2 Kanal 7+ (Licht Fahrerhaus (am Muldenkipper nichts zu sehen))
// 3 Kanal 7-- (lang) (Blinkmodus)
// 4 Kanal 7++ (lang) (Blinken ein/aus (am Radlader nur Geräusch))
// 5 Kanal 8+ (Rundumleuchte)
static void autopuls_handler(); // Aufruf alle 20 ms von Hauptschleife
}l2;
static_assert(sizeof l2==10);
namespace notaus{
inline void force() { // Aufruf bei Notaus von Pampelmuse oder I²C
PORTD&=~(1<<2);
DDRD |= 1<<2; // Wired-And am Not-Aus-Eingang
}
inline bool forced() {
return DDRD&1<<2;
}
inline void release() {// Aufruf bei … Signal von Fernbedienung?? Nee!
DDRD &=~(1<<2);// Wired-And „enteisen“
PORTD|= 1<<2;
}
inline bool released() {
return PIND&1<<2;
}
}//namespace
/****************************
* Schicht 3: Fernbedienung *
****************************/
// Der Controller unterstützt (sendet) 10 Kanäle und hat:
// - 6 Analog-Geber (2 Sticks und 2 Drehräder),
// - 2 Tasten (Unterseite)
// - 2 Schalter mit 2 Stellungen
// - 2 Schalter mit 3 Stellungen
extern struct L3{
char values[10];
L3();
// static inline unsigned checksum(const byte*);
// static inline bool set_checksum(byte*);
static inline void onSensorQuery();
void checkIbusRx();
void toServo() const; // „kopiert“ l3 nach l2 einzelbyteweise
static byte fbaus; // Fernbedienung unwirksam („geraubt“) wenn !=0
}l3;
static_assert(sizeof l3==10);
/********************************************************
* Schicht 4: A/D-Wandler, Impulszähler, Linearisierung *
********************************************************/
// Futter für I²C-Adresse 0xB6/0xB7: I²C-Daten Level 3
extern struct L4{
int16_t lin[7];// Lenkung in 1/100°,
// Fahrgeschwindigkeit in 0,1 mm/s (Anzeige cm/s),
// Hubarm/Ladefläche in 1/100°,
// Löffel/Verwindung in 1/100°,
// Traktionsakkuspannung in 1/100 V,
// Chiptemperatur in 1/100 °C,
// Ultraschallsensor in 0,1mm (Anzeige cm)
byte flags; // 0 Anstoß-Schalter [Nur Muldenkipper]
// 2:1 0 Kein Not-Aus
// 1 Not-Aus vom Schalter
// 2 Not-Aus von Pampelmuse
// 3 Not-Aus vom I²C-Controller
// 3 Zündung ein/aus (Kanal 6-) 0..1
// 4 Blinken (Kanal 7+ lang) 0..1
// [Radlader: Nur Geräusch. Muldenkipper: Blinklichter links+rechts]
// 7:5 Status der Rundumleuchte (Kanal 8+)
// 0 aus
// 1 normale Rundum-Funktion (beim Einschalten)
// 2 langsam rundum
// 3 blinkend
// 4 blitzend
// 5..7 sollte nicht vorkommen
byte lamps; // Bit 0..2: Lampenstatus Fahrzeug (Kanal 7-) 0..5
// Bit 3..4: Blinkstatus der Fahrzeuglampen (Kanal 7- lang) 0..2
// Bit 5..7: Lampenstatus Führerhaus (Kanal 7+) 0..4
// Der Magic Controller merkt sich (leider) den Schaltzustand,
// daher Mitführung im EEPROM
uint16_t ad[5],// ADC0: Poti
// ADC1: Poti
// ADC2: Poti
// ADC3: Traktionsakkuspannung
// ADC8: Temperatur
ustime, // Zeit vom Ultraschallsensor
i1time; // Differenzzeit der Capturewerte vom INT1
byte i1num, // Anzahl INT1
i1to; // Timeout (3..0 à 20 ms) für brauchbaren Capturewert
uint32_t kmz; // Kilometerzähler in Impulsen vom Motor
static void adcstart();
char save(); // liefert vzb. Tachoticks
void on20ms(char);
}l4;
static_assert(sizeof l4==36);
// Im Gegensatz zu #define ohne Seiteneffekte (Mehrfachberechnung von x),
// so nicht ganz korrekt für Gleitkommazahlen (-0 wird nicht zu +0) => fabs()
template<class T> static constexpr T abs(T x) {
return x<0 ? -x : x;
}
inline byte cabs(char x) {asm volatile(
" sbrc %0,7 \n"
" neg %0 \n"
:"+r"(x));
return x;
}
/*
inline char cadd_sat(char a, char b) {
a+=b;
asm(" brvc 1f \n" // ohne Überlauf nichts tun
" ldi %0,0x80 \n" // lade mit iNaN, Begrenzung später
" brcs 1f \n"
" com %0 \n" // lade mit iInf
"1: \n"
:"+d"(a));
return a;
}
*/
inline int add_sat(int a, int b) {
a+=b;
asm( // Solche Tricksereien wirklich nur nach „+=“-Operator!
" brvc 2f \n" // ohne Überlauf nichts tun
" ldi %A0,0 \n"
" ldi %B0,0x80\n" // lade mit iNaN, Begrenzung später
" brcs 1f \n"
" com %A0 \n"
" com %B0 \n" // lade mit iInf
"1: lds r0,%1 \n"
" inc r0 \n"
" sts %1,r0 \n"
"2: \n"
:"+d"(a):"i"(eedebug+19));
return a;
}
inline long add_sat(long a, long b) {
a+=b;
asm(" brvc 2f \n" // ohne Überlauf nichts tun
" ldi %A0,0 \n"
" ldi %B0,0 \n"
" ldi %C0,0 \n"
" ldi %D0,0x80\n" // lade mit lNaN, Begrenzung später
" brcs 1f \n"
" com %A0 \n"
" com %B0 \n"
" com %C0 \n"
" com %D0 \n" // lade mit +lInf
"1: lds r0,%1 \n"
" inc r0 \n"
" sts %1,r0 \n"
"2: \n"
:"+d"(a):"i"(eedebug+19));
return a;
}
// Kleine Divisionsroutine ÷100 für <a> im Bereich ±12700 mit Rundung
// 90 Takte
inline char idiv100r(int a) {
byte r=8;
asm(
" tst %B0 \n"
" clt \n"
" brpl 1f \n"
" com %B0 \n" // Komplement statt Negation führt zu Rundung gegen +∞
" com %A0 \n" // 50 => 1, -50 => 49 => 0
" set \n"
"1: subi %A0,lo8(-50) \n"
" sbci %B0,hi8(-50) \n" // Mit halbem Divisor aufrunden
"0: lsl %A0 \n" // Null einschieben
" rol %B0 \n"
" brcs 1f \n" // Hier niemals Carry
" cpi %B0,100 \n"
" brcs 2f \n"
"1: subi %B0,100 \n"
" inc %A0 \n" // Low-Teil = Divisionsergebnis, High-Teil = Rest
"2: dec %1 \n"
" brne 0b \n"
" brtc 1f \n"
" neg %A0 \n" // Ergebnisbyte negieren
"1: \n"
:"+d"(a):"r"(r));
return a; // Rest verwerfen
}
inline int MulDiv(int a, int b, int c) {
return (long)a*b/c; // Achtung! Division rundet Richtung 0!
}
inline unsigned MulDivU(unsigned a, unsigned b, unsigned c) {
return a*(unsigned long)b/c; // Auch diese Division rundet Richtung 0!
}
template<byte N>inline int MulShr(int a, int b) {
return (long)a*b>>N; // Achtung! Rundet Richtung -∞, also wie floor()!
}
template<byte N>inline int MulShrR(int a, int b) {
return (long)a*b+(1<<N-1)>>N;
} // halben Shiftwert vorher addieren, das rundet „richtig“.
// Beim Barrelshifter in ASM wäre das ein "adc 0" danach.
// 6x rechtsschieben und runden; hier in 11 Takten
inline int shr6r(int a) {
asm( // %B0 %A0 r0 C
" mov r0,%A0 \n" // fefcba98 76543210 76543210
" mov %A0,%B0 \n" // fefcba98 fedcba98 76543210
" rol r0 \n" // fefcba98 fedcba98 6543210x 7
" rol %A0 \n" // fefcba98 edcba987 6543210x f
" sbc %B0,%B0 \n" // ffffffff edcba987 6543210x
" rol r0 \n" // ffffffff edcba987 543210xx 6
" rol %A0 \n" // ffffffff dcba9876 543210xx e
" rol %B0 \n" // fffffffe dcba9876 543210xx
" rol r0 \n" // fffffffe dcba9876 43210xxx 5
" adc %A0,r1 \n" // inkrementiere wenn Bit 5 = 1
" adc %B0,r1 \n"
:"+d"(a));
return a;
}
// NEU 230321: Geschwindigkeitsmessung mittels Pulsabstandsmessung;
// Trigger nur noch auf 1 Flanke, Nutzung von umlaufendem Timer1
extern unsigned i1ts;
/***********
* i2c.cpp *
***********/
class I2C{
public:
static void init();
static void poll();
};
/**********************************
* Schicht 5: Regler (regler.cpp) *
**********************************/
constexpr int8_t cInf = 0x7F, cNaN = 0x80;
constexpr int16_t iInf = 0x7FFF, iNaN = 0x8000;
constexpr int32_t lInf = 0x7FFFFFFF, lNaN = 0x80000000;
int limitInt(long);
// I²C-Daten Level 5
extern struct Regler{
char so; // Regler-Soll (in ganzen Grad bzw. cm/s)
struct F{
byte vmax:7; // Regler-Speed = maximaler Ausgabewert für PWM vor Spreizung, 0 = 125
bool losgerissen:1;
// operator byte&() {return *(byte*)this;}
F(byte v, bool b):vmax(v),losgerissen(b) {}
}f;
unsigned tout; // in 20 ms
int eΣ; // Summierter Fehler
int eΩ; // Letzter Fehler (gefiltert wegen Rauschen)
char neu_soll(int);
void reset(byte to=0,bool cont=false); // Restart für to!=0
void periodic();
static void on20ms(); // Aufruf alle 20 ms
enum{
fAdd=0x10, // Addition auf Regelziel, wenn aktiv, sonst auf Istwert
}; // Regler startet nicht wenn Istwert bereits erreicht (!)
static bool set(byte,char,char); // liefert true wenn Regelziel erreicht
Regler():so(cNaN),f(125,false) {} // Scheint auch im Array korrekt initialisiert zu werden!
struct LogItem{
int e,Σ,Δ,r;
};
static struct Log{ // Wenn mehrere Regler gleichzeitig arbeiten gibt's Kuddelmuddel.
byte writeidx,readidx;
LogItem items[16]; // ausreichend für 300 ms
LogItem& current() {return items[writeidx];}
void savecurrent();
byte emit(byte*,byte=60); // max. 60 Byte zum I²C-Puffer ausspucken
}log;
}regler[4];
static_assert(sizeof regler==8*4);
/***************************************************************
* Schicht 6: Odometrie (v.a. zur Funktionskontrolle, odo.cpp) *
***************************************************************/
extern struct Odo{
long x,y; // Position des Rechenpunktes im Koordinatensystem (Linkssystem), Einheit 0,1 µm
long α; // Winkellage des (geradeausgelenkten) Fahrzeugs, umlaufend, 24 Bit = 360°
int q; // 1/Kurvenradius: Addierglied für α
int curλ;
void on20ms(char n); // Aufruf alle 20 ms
void lenkTo(int λ); // ermittelt q durch Interpolation aus eedata.fz.q
static long lsin(long w,int α);
static long lcos(long w,int α) {return lsin(w,α+16384);}
}odo;
static_assert(sizeof odo==16);
/*******************************************************************
* Schicht 7: Koordinierte, geregelte Mehrfachbewegung (koord.cpp) *
*******************************************************************/
struct L7{ // Beschreibt 1 Segment
int8_t soll[4]; // Sollwerte der 4 Regler (cNaN = belassen)
int8_t speed[4]; // Geschwindigkeiten für die Regler
long s; // Fahrstrecke in Ticks; 0 = keine Strecke
unsigned w; // Warten in 20 ms
static L7*prepare() __attribute__((warn_unused_result));
static void push();
// static void purge(); // Alle Aufträge löschen
static void purge(byte); // Im aktuellen Auftrag kanalspezifisch Auftrag beenden
static void on20ms(char);
static const byte&füllstand() {return fill;}
static bool full();
private:
static byte fill;
static byte ri;
static byte wi();
static void eat(); // Ältesten (aktuell verarbeiteten) Auftrag löschen
static unsigned lastuv; // Letzte vorzeichenlose Geschwindigkeit
};
// Damit kann man während der Fahrt
// bremsen/beschleunigen/Kurven fahren/Hubarm/Löffel betätigen
// und (mit mathematischer Analyse) eine Zielfahrt vornehmen
/*******************
* Schicht 9: Load *
*******************/
struct Load;
extern struct Loadavg{
struct LoadLong{
byte min, max; // Minimum wird nicht benutzt
unsigned cnt;
unsigned long time;
void on20ms(Load&);
inline void on20ms(volatile Load&);
}isr,sleep;
unsigned t1l,t1h; // Gesamt-Takte
void on20ms();
void prepare(); // vor Abholen
void reset(); // nach Abholen via I²C oder SPI+RFM69
}loadavg;
static_assert(sizeof loadavg==20);
| Detected encoding: UTF-8 | 0
|