/* 6-fach-Schrittmotorsteuerung mit einfacher 3-Achs-Parallelkinematik
*
* tabsize = 8, encoding = utf-8 (ohne Windows-Header BOM)
* Änderungen (+dazu, *geändert, -Bugfix):
+090803 automatische Belüftung des Vakuumerzeugers (Greifer 1 & 2)
+100812 Kaskadierung der Portexpander um weitere 16 Digitalausgänge
+100820 "@"-Schnittstellenkommando
+100823 "~"-Schnittstellenkommando
*101102 "A"-"F" liefern ganze Motor-Struktur, nicht nur Position
*101220 Erhöhung der CPU-Taktfrequenz von 8 auf ca. 15 MHz
Vergrößerung der Motor-Klasse von 8 auf 32 Byte
Neue Kommandos ≥80h mit nachfolgendem Längenbyte und Datenblock
+110106 Ziel- und Referenzfahrt komplett
+110107 CPU-Last-Berechnung und -Ausgabe (Min, Max, Avg)
*110110 Baudrate jetzt 38400 Baud (benötigt HiSerial für COM3..COM6)
+110204 Debug-Funktionen zum RAM/EEPROM/Flash lesen/schreiben
+110208 Idle-Modus beim Warten auf Timer-Tick, Achsdaten im EEPROM
+110322 Software-Endschalter auch für Tische mit Hardware-Endschaltern,
Definition von INF und NAN für int32
+110324 MaxAccel festlegbar in 7 logarithmischen Stufen (1..64)
+110328 Freiform-Bahnsteuerung für 1-6 Motoren
*110329 Motoren-Tausch mit SM2
+110401 Einheitliche Endschalter-Abfrage ohne Prozedurzeiger
*140117 Anpassung an SM2, 2 weitere Endschalter auf Board vorbereitet
-140224 EEPROM-Data in /eine/ Struktur (nicht jeder avr-gcc behält die Reihenfolge).
Keine Doppelnullterminierung der Beschreibungsstrings. nm-High-Nibble = 4.
-150122 InMoveAny() korrigiert, SetMotorData (RAM) Länge korrigiert
-1502xx Beschleunigungsbegrenzung beim Bahnfahren (nach unten) korrigiert
*/
#define F_CPU 15400000 // Hz (OSCCAL=0xFF, gemessen @ 23 °C)
#define BAUDRATE 38400 // stets 8N1, gemessene Abweichung bei 9600: -4‰
// Höhere Baudraten als 40000 (4 kByte/s) kann die Firmware nicht verarbeiten.
// Reicht für Statusreport über alle Motoren + Load (160 Byte): 24/s
#include <string.h> // memset
#include <stdlib.h> // abs()
#include <avr/io.h>
#include <avr/sleep.h>
#include <avr/pgmspace.h>
#include <avr/eeprom.h>
#include <avr/interrupt.h>
/************
* Hardware *
************/
/*
Verwendung der Ports:
12 PB0 [S0N] Endschalter
13 PB1 ↑RCK Parallelübernahme vom Schieberegister
14 PB2 [S1N] Endschalter
15 PB3 MOSI Schiebe-Daten
16 PB4 MISO Schiebe-Daten retour (Ketten-Ende, offen)
17 PB5 ↑SCK Schiebe-Takt
7 PB6 [S3N] Endschalter
8 PB7 [S3F] Endschalter
23 PC0 LED gelb
24 PC1 LED grün
25 PC2 [S5N] Endschalter
26 PC3 Taste S801 rechts (Kennfarbe ROT)
27 PC4 Taste S802 (oder Schaltkontakt Inkrementalgeber)
28 PC5 Taste S803 (Kennfarbe GRÜN)
30 PD0 RxD RS232-Empfangsdaten
31 PD1 TxD RS232-Sendedaten
32 PD2 Taste S804 links (Kennfarbe BLAU)
1 PD3 Inkrementalgeber
2 PD4 Inkrementalgeber
9 PD5 [S4N] Endschalter
10 PD6 [S5F] Endschalter
11 PD7 [S4F] Endschalter
*/
#ifndef F_CPU
# define F_CPU 8000000
#endif
#ifndef BAUDRATE
# define BAUDRATE 9600
#endif
#define F_MOT 8000 // 8 kHz Schrittfrequenz
#define T0START (uchar)(256.5-F_CPU/8/F_MOT) // Startwert für Timer0
#define elemof(x) (sizeof(x)/sizeof(*(x)))
#define nobreak
#define NOINIT __attribute__((section(".noinit")))
typedef unsigned char uchar;
typedef unsigned long ulong;
typedef unsigned short ushort;
static void Bootloader() __attribute__((naked,noreturn));
/******************************************
* EEPROM-Speicherinhalt (Achsdaten usw.) *
******************************************/
struct EeCtrl{ // Offset = 0: Kopfdaten
ushort fv; // Berechnungsschritte pro Sekunde für v (8 kHz)
char ea; // Exponent zur Basis 2 für a (64 kHz²)
char xs; // Irrelevante Bits für Positionsangabe (hier: Halbschritt)
uchar le; // dies entspricht Load = 0 %
uchar lf; // dies entspricht Load = 100 %
uchar bd; // Baudrate gemäß Baudrate.htm
uchar nm; // Achsen (Low-Nibble) à 48 Byte, Teiler für Bahnsteuerung (High-Nibble)
};
// Diese Struktur ist weitestgehend gleich der "struct Motor"-Struktur weiter unten.
// Um 4 Byte versetzt sowie ohne die PC-Angaben. Gesamtlänge: 48 Bytes
struct EeMotor{ // alle Positions- und Wegangaben in Mikroschritt
long Ist; // 04 00 Position zurzeit (wird häufiger geschrieben)
uchar flags; // 08 04 Nichtflüchtige Bits
// 0 = LEFT Phasenumkehr der Motorbestromung
// 1 = REF Position referenziert
// 2 = RIGHT Default-Referenzschaltersuche zur Motorferne
// 3 = MANU Niemals Referenzfahrt, sondern Null-Setzung
// 4 = HOLD Dauer-Bestromung des Motors
// 5 = ROTA Umlaufender Antrieb ohne Endlagen
// 6 = PILG ¿Pilgern, sonst Spielausgleich
// 7 = AZ Zielfahrt nach Null nach Referenzfahrt
char MaxAccel; // 09 05 Maximale Beschleunigung (vzl.)
int MaxSpeed; // 0A 06 Maximal erlaubte Verfahrgeschwindigkeit (vzl.)
long RefPos; // 0C 08 Position des Referenzschalters ‡
long Anfang; // 10 0C Software-Endschalter links †
long Ende; // 14 10 Software-Endschalter rechts †
int Pilger; // 18 14 Pilgerschritte oder Spiel, vzb., max. ±127 Halbschritte
char Jerk; // 1A 16 (Maximaler) Ruck, 0 = ohne Ruckbegrenzung
uchar endsw; // 1B 17 Endschalter-Zuweisung, Bits:
// 0 = Endschalter motornah vorhanden
// 1 = Endschalter motorfern vorhanden
// 2 = Referenzschalter vorhanden (gleiche Leitung wie „motornah“)
// 3 = Endschalter-Leitungen motornah/motorfern vertauscht
// 4 = Endschalter mit invertiertem Signal (Schließer)
uchar fl2; // 1C 18 weitere Flags (Verbote)
// 4 = DEMO ¿ungenutzt
// 5 = F2AR ¿Automatisch Referenzfahrt beim Einschalten
// 6 = F2RR ¿Keine Bewegung ohne Referennz
// 7 = F2RV ¿Referenzverlust beim Einschalten
uchar fl3; // 1D 19 unbestimmt
uchar Current; // 1E 1A Strom durch Motorwicklung, in 20 mA (SM3+)
uchar Curve; // 1F 1B Kurvenform der Sinusinterpolation (SM3+)
float PerUnit; // -- 1C Mikroschritt pro Einheit - nur für PC-Software
char Unit[8]; // -- 20 Einheitenbezeichner in UTF-8 - nur für PC-Software
// (nullterminiert oder nicht terminiert)
long long
FreeForApp; // -- 28 Handle o.ä. für Anwendungsprogramm
}; // Diese Struktur ist stückweise binärkompatibel mit „class Motor“:
// <RefPos> ist <Soll>; <ProEinheit> und <Einheit> kommen nicht vor
// † darf Inf sein, wenn Hardware-Endschalter vorhanden, sonst nicht.
// NaN ist nicht erlaubt.
// ‡ darf sich auch außerhalb von Anfang und Ende befinden.
// NaN oder Inf ist nicht erlaubt.
#define LNAN 0x80000000 // Not-a-number-Kode, keine oder ungültige Zahl, NaN
#define LINF 0x7FFFFFFF // Überlauf-Kode, +∞
EEMEM const struct EeData{
EeCtrl ec;
EeMotor em[6];
char EeNames[512-sizeof(EeCtrl)-6*sizeof(EeMotor)]; // ½ Kilobyte für alles reservieren
}ee={{ // Offset = 0: Kopfdaten
/*ushort fv*/ F_MOT, // Berechnungsschritte pro Sekunde für v (8 kHz)
/*char ea*/ -9, // Exponent zur Basis 2 für a (64 kHz²)
/*char xs*/ 8, // Irrelevante Bits für Positionsangabe (hier: Halbschritt)
/*uchar le*/ T0START, // dies entspricht Load = 0 %
/*uchar lf*/ 255, // dies entspricht Load = 100 %
/*uchar bd*/ 0x19, // Baudrate gemäß Baudrate.htm
/*uchar nm*/ 0x46}, // Lo-Nibble: 6 Achsen à 48 Byte,
// Hi-Nibble: Teilerfaktor für Bahnmodus, 2^4 = 16 → 500 Hz
// Offset = 8: echte Achs- und Motordaten (LONIBBLE(nm) Einträge à 48 Byte)
{// fl accel speed REF MIN MAX Pilger Jerk endsw flagsX ProUnit Unit
{0, 0x80,64, 160, -51*200L<<8, -180*200L<<8, 180*200L<<8, 0, 0, 0x04, 0,0,60,0, 200L<<8, "°"},
{0, 0x80,64, 144, 8*200L<<8, -180*200L<<8, 180*200L<<8, 0, 0, 0x04, 0,0,60,0, 200L<<8, "°"},
{0, 0xA8,32, 64, 0, 0*200L<<8, 360*200L<<8, 0, 0, 0x00, 0,0,60,0, 48L<<8, "U"},
{0, 0x01,64, 128, 0, -LINF, LINF, 0, 0, 0x13, 0,0,60,0, 1600L<<8, "mm"},
{0, 0x80,64, 64, long(-11.5*1680)<<8, -LINF, LINF, 0, 0, 0x03, 0,0,60,0, 1680L<<8, "mm"},
{0, 0x01,64, 128, 0, -LINF, LINF, 0, 0, 0x13, 0,0,60,0, 1600L<<8, "mm"},
},
// Die STANDA-Motoren sind allesamt linksläufig angeschlossen
// Offset = 8 + LONIBBLE(nm)*48:
// String-Deskriptoren (Achs-Beschreibung: Klartext-Namen in UTF-8)
"A: OWIS-Drehtisch\0"
"B: STANDA-Drehtisch 051279\0"
"S: Spindelmotor\0"
"X: STANDA-Lineartisch 045507\0"
"Y: CZJ-Lineartisch\0"
"Z: STANDA-Lineartisch 048635" // Abschluss mit Doppel-Null ist optional
};
/******************************************************************
* Grundlegender EEPROM-Zugriff (hier wichtig: nicht-blockierend) *
******************************************************************/
// Einzelbyte lesen (Bereitschaftstest muss voraus gehen!)
static void eeprom_read(const void*s) {
EEAR=(unsigned)s;
EECR|=1<<EERE; // lesen
}
// EEPROM-Speicherblock lesen, nicht blockieren
static void eeprom_read(uchar*d, const uchar*s, uchar len) {
if (eeprom_is_ready()) do{ // nicht blockieren, ggf. Ziel unverändert lassen
eeprom_read(s);
s++;
*d++=EEDR;
}while(--len);
}
static void eeprom_update_byte(uchar*a, uchar b) {
// kurz und schmerzlos mittels Direktzugriff
// Interrupts müssen gesperrt sein, oder das Schreiben kann schief gehen.
EEAR=(unsigned)a;
EECR|=1<<EERE; // lesen
if ((uchar)(EEDR-b)) { // wenn ungleich
EEDR=b;
EECR|=1<<EEMWE;
EECR|=1<<EEWE;
}
}
/****************************************************************
* Klasse Port-Expander (74HC595, da hängen die Motoren dran) *
* "Motor Nr. 6" sind 4 Schaltausgänge für Greifer *
* Weitere 16 Greifer per Kaskade (August 2010) *
****************************************************************/
/* Bitzuordnung (6 Bits pro Motor):
* Bit 0 = Phase A Bit 3 = Phase B
* Bit 1 = I1 (A) Bit 4 = I1 (B)
* Bit 2 = I0 (A) Bit 5 = I0 (B)
* Einstellbare Strangströme: 001 -100% // für Ganzschritt
101 -60% // für Halbschritt
011 -19% // für Haltefunktion
11x 0 // Freilauf (oder Ganzschritt)
010 +19%
100 +60%
000 +100%
*/
class PortExpander { // bits[6] bits[5] bits[4] bits[3] bits[2] bits[1] bits[0]
public:
uchar bits[7]; // 88888888 77777777 66665555 55444444 33333322 22221111 11000000
bool dirty; // !=0 wenn verändert
uchar length; // Länge in Bytes (mit Kaskade max. 7 #100812)
void Init(); // svw. Konstruktor (aber der bläht auf)
void ShiftOut(); // aktualisiert die Schieberegisterkette
void SetBits(uchar Motor, uchar b); // setzt 6 Steuerbits (3 pro Strang)
static uchar DetectLength();
};
static NOINIT PortExpander pe;
void PortExpander::Init() {
memset(this,0xFF,sizeof(*this));
bits[4]=0x0F; // Schaltausgänge ausnahmsweise LOW
bits[6]=bits[5]=0;
length=DetectLength();
}
void PortExpander::ShiftOut() {
uchar i=length;
const uchar *z=bits+i;
// Per SPI seriell schieben (40 Bits, MS-Bit und MS-Byte zuerst)
do{
SPDR=*--z;
while (!(SPSR&0x80)); // warten bis Interrupt
}while(--i);
// Parallele (gleichzeitige) Übernahme nach außen
PORTB|= _BV(1); // RCK↑
dirty=false;
PORTB&=~_BV(1); // RCK↓
}
void PortExpander::SetBits(uchar Motor, uchar b) {
b &= 0x3F;
uchar *z = bits;
if (Motor>=4) {
z += 3;
Motor -= 4;
}
switch (Motor) {
case 0: z[0] = z[0]&0xC0 | b; break;
case 1: z[0] = z[0]&0x3F | b<<6;
z[1] = z[1]&0xF0 | b>>2; break;
case 2: z[2] = z[2]&0xFC | b>>4;
z[1] = z[1]&0x0F | b<<4; break;
case 3: z[2] = z[2]&0x03 | b<<2; break;
}
dirty=true;
}
uchar PortExpander::DetectLength() {
uchar i=7; // maximal verarbeitbare Länge
do{
SPDR=0xFF; // Kette mit Einsen füllen
while (!(SPSR&0x80)); // warten bis Interrupt
}while(--i);
i=7;
do{
SPDR=0; // Kette mit Nullen füllen
while (!(SPSR&0x80)); // i==0 wenn MISO und MOSI verbunden, für jedes Byte 1 mehr
if (!SPDR) return 5;
}while(--i); // Nur Längen 5 und 7 können behandelt werden!
return 7;
}
/***************
* Endschalter *
***************/
enum {
ESA=1, // 0 Endschalter motornah vorhanden
ESE=2, // 1 Endschalter motorfern vorhanden
ESR=4, // 2 Referenzschalter vorhanden (gleiche Leitung wie „motornah“)
ESS=8, // 3 Endschalter-Leitungen motornah/motorfern vertauscht
ESI=16, // 4 Endschalter mit invertiertem Signal (Schließer)
};
// Alle Endschalter abfragen und in einem Byte aufsammeln
#define S0N 0x01
#define S1N 0x02
#define S3N 0x08
#define S3F 0x10
#define S4N 0x20
#define S4F 0x80
#define S5N 0x04
#define S5F 0x40
static uchar GetSwitches() {
uchar b=PINB;
return b&0x01 | b>>1&0x02 | b>>3&0x18 | PINC&0x04 | PIND&0xE0;
}
// Hier erfolgt die Zuordnung der 8 verfügbaren Endschalter zu den 6 Achsen:
PROGMEM uchar SwitchTab[12]={S0N,S1N,0,S3N,S4N,S5N,0,0,0,S3F,S4F,S5F};
// Achsenzuordnung einer 5-Achs-Maschine: A,B,S,X,Y,Z
// (A,B = Drehachse, S = Spindel, X,Y,Z = lineare Achsen)
// Endschalter abfragen
// i = Motorindex 0..5
// es = Endschalter-Bits (ausgewertet wird nur ESS und ESI)
// Bei ESS=0 wird der motornahe, bei ESS=1 der motorferne Schalter abgefragt
static uchar GetSwitch(uchar i, uchar es) {
if (es&ESS) i+=6;
uchar sw=GetSwitches();
if (es&ESI) sw=~sw;
return sw&pgm_read_byte(SwitchTab+i);
}
/**********************************
* Klasse Motor (für 6 Instanzen) *
**********************************/
// Phasentabelle für Halbschrittbetrieb, reduzierte und volle Bestromung
PROGMEM uchar PhaseTab[16]={
0b011110, /* -19%, 0 */ 0b001110, /* -100%, 0 */
0b011010, /* -19%, +19% */ 0b101100, /* - 60%, + 60% */
0b110010, /* 0 , +19% */ 0b110000, /* 0 , +100% */
0b010010, /* +19%, +19% */ 0b100100, /* + 60%, + 60% */
0b010111, /* +19%, 0 */ 0b000111, /* +100%, 0 */
0b010011, /* +19%, -19% */ 0b100101, /* + 60%, - 60% */
0b111011, /* 0 , -19% */ 0b111001, /* 0 , -100% */
0b011011, /* -19%, -19% */ 0b101101}; /* - 60%, - 60% */
#define MAXSPEED 256 // Absolute Maximalgeschwindigkeit für Verfahrbewegungen
// Rechentechnisch maximal 512 (= 1 Ganzschritt pro Zeittakt)
// Funktionierend maximal 256 (= 1 Halbschritt pro Zeittakt)
// 1 Zeittakt = 125 µs
#define MAXACCEL 64 // Rechengröße für 1 Geschwindigkeitsschritt pro ms
#define HOLDREFSPEED 64 // Darüber gibts beim abrupten Abbrechen einen REF-Verlust
#define SLOWSPEED 8 // Für Referenzschaltersuche
#define DEMOSPEED 64 // Initiale Demonstrations-Geschwindigkeit
/* Vollschritt, Halbschritt, Mikroschritt, Nanoschritt:
* Das Programm rechnet in Mikroschritt = 1/256 Halbschritt.
* Die Hardware kann nur Halbschritte steuern (das LSByte ist nur Rechengröße)
* Zwei Halbschritte sind ein Ganzschritt (ohne besondere Beachtung)
* Für feine Nanoschritt mangelt es dem AVR an Rechenleistung. -> MSP430
* Da die letzten 3 Bits pro Runde immer Null sind, könnte man auch meinen:
* 8 Nanoschritt -> 1 Mikroschritt } 1 Mikroschritt-Byte
* 32 Mikroschritt -> 1 Halbschritt } ohne weitere Unterteilung
* 2 Halbschritt -> 1 Vollschritt
* 4 Vollschtitt -> 1 Phasenumlauf
* 200 Vollschritt -> 1 Umdrehung am (50-poligen) Motor
* ≈ 42000 Umdrehungen -> maximaler Stellbereich der Achse
*/
class Motor { // Strukturgröße = 32 Bytes
void MakeBits() const;
int GetIndex() const;
public:
enum { // Nur flüchtige Bits: BRAKE,FULL,PHA3, HUNT,REFF,SLOW
HUNT=1, // Ist an Soll angleichen („Jagd“-Modus), Zielfahrt
REFF=2, // Referenzfahrt (bis zum Referenz-Schalter)
BRAKE=4, // Bremsung aktiv (automatisch bei speed=0 gelöscht)
SLOW=8, // Langsame Schaltersuche (ggf. dritter Pass)
FULL=16, // voller Strom
}; // die übrigen 3 Bits = Halbschritt-Phase
uchar phase; // 0 0..15; gerade Zahlen: geringer, ungerade: hoher Strom
char accel; // 1 0, +MaxAccel oder -MaxAccel, in Mikroschritt/(125µs)²*2^ea;
int speed; // 2 ±MAXSPEED, mehr nicht! In Mikroschritt/125µs
long Ist; // 4 In Mikroschritt
enum { // Nur feste Bits: LEFT,RIGHT,MANU,HOLD,ROTA,AZ
LEFT=1, // Linkslauf (Motor dreht andersherum)
REF=2, // Referenzierte Position (Software-Endschalter wirksam)
RIGHT=4, // Schaltersuche nach rechts (zum Ende hin)
MANU=8, // niemals Referenzfahrt auf Anschlag (Konf.-Bit)
HOLD=16, // bestromt (kein Freilauf, aber reduzierter Strom)
ROTA=32, // umlaufender Antrieb ohne Endlagen (bspw. Spindel)
DEMO=64, // Demo-Modus (fährt zwischen den Endschaltern) — ungenutzt
AZ=128, // Auto-Zero = Nach Referenzfahrt zur Null eilen
}; // „Halbfeste“ Bits hierin sind REF und DEMO.
// Mögliche weitere Bits und Bytes:
// - Referenzierung beim Einschalten verlieren
// - Referenzpunkt beim Lösen des Schalters
// - Pilgerschritt (± Schritte)
uchar flags; // 8 können per serielle Schnittstelle gesetzt werden
char MaxAccel; // 9 vzl. Maximalbeschleunigung, max. +MAXACCEL
int MaxSpeed; // 10 vzl. Maximalgeschwindigkeit für HUNT und REFF
long Soll; // 12 Sollposition, zu der hingefahren werden soll bei HUNT
#define RefDir ((char*)&Soll)[3] // +1 oder -1, Referenzfahrtrichtung
long Anfang; // 16 2 Software-Endschalter-Positionen (±∞ erlaubt!), …
long Ende; // 20 … diese werden nur ausgewertet, wenn REF gesetzt ist
int Pilger; // 24
uchar Jerk; // 26
uchar endsw; // 27
uchar fl2; // 28
uchar fl3; // 29
uchar pdc; // 30 PowerDownCounter (zz. ohne EEPROM-Startwert!)
char OneAccel; // 31 Einzel-Beschleunigungs-Befehl (wird in CalcSpeed null gesetzt)
// auch: Akkumulator für CalcSpeed-Zyklen
// Im Bahnmodus ceil(MaxAccel>>5) als a_max pro Sample
// meistens kommt 1 heraus wegen MaxAccel fast immer ≤ MAXACCEL
void Step(char by); // Endschalter-gesicherte Version von MoveBy
void Accel(char by);
void Brake(); // abbremsen lassen
void Null(); // Position nullsetzen, Referenz setzen
void DoRef(); // Referenzfahrt auslösen — oder sofort referenzieren (MANU gesetzt)
void Unref() {flags&=~REF;}
void Init(); // setzt Ist, flags, MaxAccel, MaxSpeed, ?, Anfang, Ende
uchar IsRef() const {return flags&REF;}
void CalcSpeed(); // Aufruf alle 8 * 125 µs = 1 ms (Lastverteilung)
void PeriodicAction(); // Aufruf alle 125 µs (8 kHz)
static void PeriodicAll();
static void BrakeAll();
static void NullAll();
static void DoRefAll();
static void UnrefAll();
static void InitAll();
static int InMoveAny(); // liefert !=0 wenn irgendeine Achse in Bewegung
int GetMaxSpeed() const; // liefert MaxSpeed (oder SLOWSPEED oder DemoSpeed)
void SetData(const uchar*,uchar,uchar);
void Command(const uchar*,uchar);
void CheckLimit(uchar ec) const;// prüft Position auf Limits sowie Endschalter (nur für Bahnsteuerung)
private:
uchar AufRef() const; // Referenzschalter betätigt?
uchar AmAnfang() const;// Endschalter links betätigt?
uchar AmEnde() const; // Endschalter rechts betätigt?
void MoveBy(char d);
static bool TooFast(long r, int v, char a);
void SetRef();
void NormalizeIst();
void ReadEeprom(void*, uchar, uchar) const; // von EeMotor[] ab Offset lesen
void LimitMaxAccel();
void LimitMaxSpeed();
};
Motor mot[6];
register uchar DemoFlags asm("r6");
// Bit 0 = ein/aus Demo/Schmiermodus
// Bit 1 = Richtung
// Bit 2 = Umschalter Motoren 0..2 / oder 3..5
// = 8: auf serieller Schnittstelle "U" ausgeben zur Frequenzmessung
// Bit 4 = Antworte mit '&' sobald alle Motoren stehen
// Bit 5 = Serielle Eingabedaten nach RecvBuf, sowie Anforderung Bahn-Modus
// Bit 6 = a-Modus
// Bit 7 = Bahn-Modus
static NOINIT int DemoSpeed;
int Motor::GetIndex() const {
return this-mot;
}
void Motor::PeriodicAll() {
Motor *m=mot;
do m->PeriodicAction();
while (++m<mot+6);
}
void Motor::BrakeAll() {
Motor *m=mot;
do m->Brake();
while (++m<mot+6);
}
// Referenzfahrten auslösen
void Motor::DoRefAll() {
Motor*m=mot;
do m->DoRef();
while (++m<mot+6);
}
// Setzt alle Motorpositionen zu Null und referenziert
// (Motoren können weiter laufen!)
void Motor::NullAll() {
Motor *m=mot;
do m->Null();
while (++m<mot+6);
}
void Motor::UnrefAll() {
Motor *m=mot;
do m->Unref();
while (++m<mot+6);
}
void Motor::InitAll() {
Motor*m=mot;
do m->Init();
while (++m<mot+6);
}
int Motor::InMoveAny() {
int r;
Motor*m=mot;
do r=m->speed;
while (!r && ++m<mot+6);
return r;
}
void Motor::MakeBits() const {
uchar b=0x3F; // kein Strom
if ((phase|flags)&HOLD) {
b=pgm_read_byte(PhaseTab+(phase>>4));
}
pe.SetBits(GetIndex(),b);
}
// Phasen steuern: Um 1, maximal 2 Halbschritte bewegen
// Ohne Aktualisierung der Ist-Position (das macht die Funktion Step())
void Motor::MoveBy(char d) {
pdc=255;
phase|=FULL;
if (flags&LEFT) d=-d;
phase+=d<<5;
MakeBits();
}
// r = Restweg in Fahrtrichtung
// v = Geschwindigkeit
// a = Beschleunigung, 64 = Maximum
bool Motor::TooFast(long r, int v, char a) {
v<<=1;
long z=(long)v*(v<0?v-2:v+2);
for (;a<64;a<<=1) z<<=1;
if (r<z) return true;
return false;
}
// Aufruf alle 1 ms (1 kHz): Geschwindigkeitsveränderung
void Motor::CalcSpeed() {
uchar ph=phase; // Register-Variable
char a=OneAccel;
uchar r=0;
// Bei Ziel- und Referenzfahrt Beschleunigungsvorgänge dezimieren
if (ph&(HUNT|REFF)) {
OneAccel=a+=MaxAccel;
if ((ph&HUNT || !(ph&REFF) || !(r=AufRef()))// nicht bei Schalterkontakt
&& !(ph&BRAKE) && a<MAXACCEL) return; // nicht bei Bremsung
a=0;
}
int v=speed; // Register-Variable
uchar fl=flags;
if (!(ph&BRAKE) && ph&REFF && !(ph&HUNT)) { // Referenzfahrt mit Schaltkontakt
a=RefDir;
if (r==0xFF) {
ph|=HUNT; // umschalten auf Referenzfahrt ohne Schaltkontakt
// 1-2 Phasen einer Referenzfahrt ohne Schalter:
// 1. Eilgang (oder Schleichgang) solange kein BRAKE-Signal gesetzt,
// Abbremsen auf v=0 und Setzen des Referenzpunktes
// 2. optional Zielfahrt zur Nullposition
if (a>=0) {
Soll=Ende; // darf hier nicht NaN/+∞ sein!
Ist=Anfang;
}else{
Soll=Anfang; // darf hier nicht NaN/–∞ sein!
Ist=Ende;
}
fl&=~REF; // das Setzen von Ist bedeutet das Aufheben der Referenzierung
}else{
// 3-4 Phasen einer Referenzfahrt mit Schalter:
// 1. Eilgang solange Endschalter nicht betätigt
// 2. Richtungsumkehr; Eilgang solange Endschalter betätigt
// 3. Normalrichtung; Schleichgang solange Endschalter nicht betätigt
// 4. optional Zielfahrt zur Nullposition
if (r) { // Referenzschalter betätigt?
a^=0xFE; // In entgegengesetzte Richtung beschleunigen
if (ph&SLOW) { // Schalter während Schleichfahrt betätigt?
ph&=~(REFF|SLOW); // Referenzfahrt erfolgreich beenden
v=0; // schlagartig Null
a=0;
SetRef();
fl|=REF; // Software-Endschalter gelten jetzt
if (fl&AZ) ph|=HUNT; // zur Null eilen
}
}else if (!(a+v)) ph|=SLOW; // jetzt Schleichfahrt zum Schalter hin
}
}else if (v) {
if (v<0) {
if (ph&BRAKE||AmAnfang()) {a=+1; ph|=BRAKE; goto brk;} // bremsen
if (ph&HUNT) { // Zielfahrt oder Referenzfahrt ohne Schaltkontakt
if (fl&ROTA && Soll==-LINF) {
a=-1;
NormalizeIst();
}else{
long r=Ist-Soll; // Restweg in Fahrtrichtung
if (TooFast(r,v,MaxAccel) || TooFast(r,--v,MaxAccel)) a=+1; // bremsen || nicht beschleunigen
}
}
}else{
if (ph&BRAKE||AmEnde()) {a=-1; ph|=BRAKE; goto brk;} // bremsen
if (ph&HUNT) { // Zielfahrt oder Referenzfahrt ohne Schaltkontakt
if (fl&ROTA && Soll==+LINF) {
a=+1;
NormalizeIst();
}else{
long r=Soll-Ist; // Restweg in Fahrtrichtung
if (TooFast(r,v,MaxAccel) || TooFast(r,++v,MaxAccel)) a=-1; // bremsen || nicht beschleunigen
}
}
}
}else{ // v==0
if (ph&BRAKE) ph&=~(HUNT|REFF|SLOW); // Referenz- oder Zielfahrt terminieren
if (ph&HUNT) {
if (Soll<Ist) {
if (AmAnfang()) goto nohunt;
a=-1; // Die Jagd beginnt (beschleunigen)
}else if (!(SREG&0x02)) { // Soll>Ist
if (AmEnde()) goto nohunt;
a=1;
}else nohunt: ph&=~HUNT; // Ziel erreicht, Jagd beenden
}
ph&=~(BRAKE|SLOW);
if (!a && pdc && !--pdc) {
ph&=~FULL;
phase=ph;
MakeBits();
*(uchar*)&Ist=0;
}
}
if (DemoFlags&1) {
if (DemoFlags&4 && this<mot+3 && IsRef()
|| !(DemoFlags&4) && this>=mot+3) {
if (v>0 || AmAnfang()) {a=+1; goto exi;} // zum Ende rasen
if (v<0 || AmEnde()) {a=-1; goto exi;} // zum Anfang rasen
v=DemoFlags&2?-1:1; // losrasen
}
}
exi:
{ // Geschwindigkeit um -1/0/1 verändern
int max=GetMaxSpeed();
if (v>max) a=-1;
else if (v==max && a>0) a=0;
max=-max;
if (v<max) a=+1;
else if (v==max && a<0) a=0;
}
brk:
v+=a;
accel=(char)(v-speed)*MaxAccel; // nur zum Loggen (für die Windows-Software)
speed=v;
if (!v && ph&REFF && ph&HUNT) {
ph&=~(REFF|HUNT|SLOW); // Ziel erreicht
SetRef(); // Position (null)setzen
fl|=REF; // Software-Endschalter gelten jetzt (auch bei Zwangsbremsung)
if (!(ph&BRAKE) && fl&AZ) ph|=HUNT; // zur Null eilen
}
phase=ph;
if (ph&(HUNT|REFF)) OneAccel-=MAXACCEL;
else OneAccel=0;
flags=fl;
}
// Aufruf alle 125 µs (8 kHz): Lageveränderung
void Motor::PeriodicAction() {
if (speed) {
union{
long l;
char c[4];
}i;
i.l=Ist;
char d=i.c[1];
i.l+=speed;
d-=i.c[1]; // Differenz, kann nur -1, 0 oder +1 sein
Ist=i.l;
if (d) MoveBy(-d);
}
}
uchar Motor::AufRef() const {
uchar es=endsw;
uchar i=GetIndex();
// Variante 1: Es existiert ein Referenzkontakt
if (es&ESR) return GetSwitch(i,es);
if (RefDir>=0) {
// Variante 2: Es existiert ein Endkontakt rechts
if (es&ESE) return GetSwitch(i,es^ESS);
// Variante 3: Es existiert ein Endkontakt links
}else{
if (es&ESA) return GetSwitch(i,es);
}
return 0xFF; // Kode für „kein (zutreffender) Schaltkontakt“
}
// Stellt fest, ob die gegebene Zahl eine „richtige“ Zahl ist (kein NaN, kein ∞)
bool isnum(const long&n) {
long nn=n;
return (nn>-LINF && nn<LINF);
}
uchar Motor::AmAnfang() const{ // Endschalter-Behandlung
uchar es=endsw;
if (es&ESA) return GetSwitch(GetIndex(),es);
if (flags&ROTA) return false;
return IsRef() && Ist<Anfang;
}
uchar Motor::AmEnde() const{ // Endschalter-Behandlung
uchar es=endsw;
if (es&ESE) return GetSwitch(GetIndex(),es^ESS);
if (flags&ROTA) return false;
return IsRef() && Ist>Ende;
}
void Motor::Step(char by) {
if (phase&(HUNT|REFF)) { // während der Fahrt Höchstgeschwindigkeit wählen
int v=MaxSpeed+by;
if (!v) return;
MaxSpeed=v;
if (by>0) LimitMaxSpeed();
return;
}
if (by<0) {
if (AmAnfang()) return;
}else{
if (AmEnde()) return;
}
Ist+=(int)by<<8; // sonst Einzelschritt machen
MoveBy(by);
}
int Motor::GetMaxSpeed() const{
int max=MaxSpeed;
if (phase&SLOW) max=SLOWSPEED; // Geschwindigkeit für Referenzfahrt
if (DemoFlags&1 && max>DemoSpeed) max=DemoSpeed;
return max;
}
void Motor::Accel(char by) {
if (phase&(HUNT|REFF)) { // Während der Fahrt Beschleunigung wählen
char a=MaxAccel;
if (by>=0) {
a<<=1;
MaxAccel=a;
LimitMaxAccel();
}else{
a>>=1;
if (!a) return;
MaxAccel=a;
}
return;
}
OneAccel=by; // sonst Geschwindigkeit stellen
}
void Motor::Brake() {
if (phase&BRAKE) {
if (speed>HOLDREFSPEED) Unref();
speed=0;
*(uchar*)&Ist=0; // Mikroschritt gleich Null!
}
phase|=BRAKE;
OneAccel=0;
}
// Aus <ee.em> ab <offset> <len> Bytes lesen
void Motor::ReadEeprom(void*d, uchar offset, uchar len) const {
eeprom_read((uchar*)d,(const uchar*)&ee.em[GetIndex()]+offset,len);
}
#define ReadEeprom(d,o,l) ReadEeprom(d,(uchar*)&ee.em[0].o-(uchar*)&ee.em[0],l)
// setzt Istwert auf Referenz-Position und berechnet die Abweichung nach Soll
// Bei Auto-Zero-Flag wird Soll auf Null gesetzt, für die anschließende Zielfahrt
// Die Software-Endschalter werden auf die EEPROM-Werte zurückgesetzt.
// Das REF-Bit wird hier NICHT gesetzt!!
void Motor::SetRef() {
long s=Ist;
ReadEeprom(&Ist,RefPos,4);
ReadEeprom(&Anfang,Anfang,8);
Soll=flags&AZ?0:Ist-s;
}
// Setzt Istwert zu Null und setzt manuelle Referenz
// Bei bereits vorhandener Referenzierung
// werden die Software-Endschalter entsprechend verschoben,
// d.h. der mechanische Stellbereich bleibt erhalten.
void Motor::Null() {
if (flags&REF) {
*(uchar*)&Ist=0; // Miroschritt gleich Null!
if (isnum(Anfang)) Anfang-=Ist;// Software-Endschalter entsprechend verschieben
if (isnum(Ende)) Ende-=Ist; // (nicht im EEPROM)
}
flags|=REF; // Software-Endschalter gelten jetzt
Ist=0;
}
// Standard-Referenzfahrt auslösen (Richtung je nach EEPROM-Flags)
void Motor::DoRef() {
if (flags&MANU) SetRef(); // Parallelkinematik stehen lassen
else{
RefDir=flags&RIGHT?+1:-1;
phase|=REFF; // Referenzfahrt auslösen
}
}
void Motor::LimitMaxAccel() {
uchar a=MAXACCEL,e=MaxAccel;
uchar k=e;
ReadEeprom(&k,MaxAccel,1);
if (!e || k && e>k) e=k; // nach oben begrenzen (runterziehen)
if (e) for(;a>e;a>>=1); // Nur „diskrete“ Werte 1,2,4,8,16,32,64 zulassen!
MaxAccel=a;
}
void Motor::LimitMaxSpeed() {
int max=MAXSPEED;
int v=max;
ReadEeprom(&v,MaxSpeed,2);
if (v && max>v) max=v;
if (!MaxSpeed || (unsigned)MaxSpeed>(unsigned)max) MaxSpeed=max;
}
void Motor::Init() {
ReadEeprom(&Ist,Ist,4+1+1+2+4+4+4+2+1+1+1+1);
LimitMaxAccel();
LimitMaxSpeed();
}
// Motor-Struktur setzen/ändern
// Die drei Funktionszeiger (am Ende von class Motor) werden nie überschrieben!
void Motor::SetData(const uchar*buf, uchar offset, uchar len) {
if (offset+len>32) return; // zu lang!
memcpy((uchar*)this+offset,buf,len);
len+=offset; // ab hier len == Ende+1
if (offset<10 && len>=10) { // Beschleunigung enthalten?
LimitMaxAccel();
}
if (offset<12 && len>10) { // Geschwindigkeit enthalten?
LimitMaxSpeed();
} // wenn zu groß oder negativ, auf EEPROM-festgelegtes Limit begrenzen
*(uchar*)&Soll=0; // Mikroschritt gleich Null!
if (len<=16) return;
if (isnum(Anfang)) *(uchar*)&Anfang=0; // (sonst versagt der Zielfahrt-Algorithmus)
if (isnum(Ende)) *(uchar*)&Ende=0;
if (Ende<Anfang) { // Anfang und Ende tauschen, wenn Ende<Anfang
long t=Ende; Ende=Anfang; Anfang=t;
}
}
// Diese Funktion setzt MaxAccel, MaxSpeed, Soll, Anfang, Ende
// und löst Referenzfahrt oder Zielfahrt aus.
void Motor::Command(const uchar*buf, uchar len) {
if (len>32-8) return; // zu lang!
if (!len) return; // zu kurz!
long PrevSoll=Soll;
if (len>1) SetData(buf+1,9,len-1); // restliche Parameter kopieren
char command=*buf; // hier: Kommandobyte, keine Flags!
if (command&0xF0) return; // verbotene Bits gesetzt: raus!
if ((command&7)==(HUNT|4)) {
if (flags&ROTA) NormalizeIst();
long s=Soll;
if (!(phase&HUNT)) PrevSoll=Ist;
s+=PrevSoll; // relative Sollposition (Fahrweite) in absolute umrechnen
if (SREG&0x08) { // vzb. Überlauf aufgetreten?
if (SREG&0x04) s=+LINF; // auf ∞ „begrenzen“
else s=-LINF;
}else *(uchar*)&s=0; // unsauber!?
Soll=s;
}
if (command&HUNT && !(flags&ROTA)) {
if (Soll<Anfang) Soll=Anfang;
if (Soll>Ende) Soll=Ende;
}
if (command&REFF) {
char d;
if (len<8) { // Referenzfahrt ohne explizite Richtungsangabe? (Kurzkommando)
d=((command^flags)<<5)-0x80;
// 1. tatsächliche Richtung aus Kommando und EEPROM zusammensetzen
// 2. zum Vorzeichen machen
// 3. Vorzeichen umkehren (negativ ist die Normalrichtung!)
}else d=RefDir; // explizite Richtungsangabe eines Langkommandos
RefDir=d>=0?+1:-1; // Vorzeichen in Richtungsangabe (±1) umwandeln
}
command&=~BRAKE;
phase|=command; // Bits HUNT, REFF, SLOW einblenden (los geht's!)
}
static void HandleEos(uchar);
// Limits prüfen (für koordinierte Freiform-Bahnsteuerung)
void Motor::CheckLimit(uchar ec) const{
int v=speed;
if (!v) return;
if (v>0) {
if (AmEnde()) HandleEos(ec);
}else{
if (AmAnfang()) HandleEos(ec);
}
}
// Ist-Wert in Bereich Min..Max ziehen
void Motor::NormalizeIst() {
long range=Ende-Anfang;
if (range<0x1000) return; // negativ oder zu klein: nichts tun!
register unsigned long i=Ist-Anfang;
do i-=range; while (!(SREG&4)); // subtrahieren bis negativ
do i+=range; while (!(SREG&1)); // addieren bis Überlauf (oder positiv)
Ist=i+Anfang; // Modulo-Division vermeiden
}
/****************************************************
* Gemeinsame Routinen für Lokal- und Fernbedienung *
****************************************************/
#if F_CPU != 8000000
static NOINIT uchar SaveOsccal; // Originaler OSCCAL-Inhalt
#endif
static void Bootloader() {
#if F_CPU != 8000000
OSCCAL=SaveOsccal;
#endif
goto *(void*)(0x1C00/2); // bei 7 KByte (WORD-Adresse)
}
// Aktuelle Achse (die mit dem Inkrementalgeber gesteuert wird)
register uchar Achse asm("r3");
// 0 = X (Schiebetisch)
// 1 = A (Drehtisch)
// 2 = Y (Eigenbau-Schiebetisch)
// 3 = X (alle 3 Motoren der Parallelkinematik)
// 4 = Y (Neigung seitlich)
// 5 = Z (Nicken vorn/hinten)
// 6 = Einzelmotor links (nur auswählbar wenn nicht referenziert)
// 7 = Einzelmotor rechts
// 8 = Einzelmotor hinten
void HandleWheelStep(uchar speed1step0,char step) {
if (DemoFlags&1) {
int v=DemoSpeed;
v+=step;
if (v && v<=MAXSPEED) DemoSpeed=v;
}else{ // Zeiger auf Memberfunktion! (toll!)
void (Motor::*fp)(char)=&Motor::Step;
if (speed1step0) fp=&Motor::Accel;
switch (Achse) {
case 0: (mot[3].*fp)(step); break; // X Lineartisch
case 1: (mot[4].*fp)(step); break; // A Drehtisch
case 2: (mot[5].*fp)(step); break; // Y kurzer Eigenbau-Lineartisch
case 3: (mot[2].*fp)(step); // Z Greifplattform hoch/runter
(mot[1].*fp)(step);
(mot[0].*fp)(step); break;
case 4: (mot[1].*fp)(-step); // C Greifplattform vorn/hinten nicken
(mot[0].*fp)(step); break;
case 5: (mot[2].*fp)(-step); // B Greifplattform seitlich neigen
(mot[1].*fp)(step);
(mot[0].*fp)(step); break;
case 6: (mot[0].*fp)(step); break; // Einzelmotoren
case 7: (mot[1].*fp)(step); break;
case 8: (mot[2].*fp)(step); break;
}
}
}
register uchar Greifer asm("r8"); // svw. aktuelle Schaltausgangs-Nummer 0..19
static uchar BelueftungCount; // Belüftungszeit für Vakuumgreifer
// dreht (op=3), setzt (op=2), löscht (op=1) oder fragt nur Zustand ab (op=0)
static uchar GreiferOp(uchar g, uchar op=0) {
uchar b; // Bits und Bytes gestürzt auf Expansionsplatine (hier nicht zurück)
g+=36; // über die 6x6 Motor-Bits hinweg
uchar *z=pe.bits+(g>>3); // Byte-Adresse
g=1<<(g&7); // Bitmaske
b=*z;
if (op==3) b^=g;
if (op==2) b|=g;
if (op==1) b&=~g;
if (op) {*z=b; pe.dirty=true;}
return b&g; // liefert jetzigen Greiferzustand
}
// Grüne LED zeigt Greifer-Zustand (vom aktuell ausgewählten Greifer)
static void ZeigeGreifer() {
if (GreiferOp(Greifer)) { // grüne LED: Zustand anzeigen
PORTC|=2;
}else{
PORTC&=~2;
}
}
static void HandleBelueftung() {
if (!BelueftungCount) return;
if (!(--BelueftungCount)) GreiferOp(2,1); // Belüftung AUS
}
static void HandleGreifer(uchar op=3) {
uchar state=GreiferOp(Greifer,op); // fassen oder lösen
if (op && Greifer==1 && !state) { // Greifer 1 (Vakuum) lösen?
GreiferOp(2,2); // Belüftung betätigen!
BelueftungCount=0xFF; // ¼ Sekunde belüften, dann abschalten
}
ZeigeGreifer();
}
// Alle Motoren anhalten und (ggf.) Demo-Mode beenden
void BrakeAll() {
if (DemoFlags&1) DemoFlags--;
Motor::BrakeAll();
}
// Zur Auswertung der CPU-Last
static struct{
uchar Min,Max;
ushort Avg;
}Load;
/*****************************************
* Serielle Schnittstelle: Fernsteuerung *
*****************************************/
static NOINIT char SendBuf[256]; // Ringpuffer
static uchar SendW,SendR; // Lese- und Schreibzeiger
// Byte aus Puffer zu (bereiter) serieller Schnittstelle schicken
static void SendBufferByte() {
uchar r=SendR;
if (r==SendW) return; // Ringpuffer leer
UDR=SendBuf[r]; // Zeichen ausgeben
SendR=r+1; // Lesezeiger anpassen
}
// Byte zu serieller Schnittstelle schicken (Polling)
static void HandleSerialOutput() {
if (!(UCSRA&0x20)) return; // Schnittstelle beschäftigt
if (DemoFlags==8) {UDR='U'; return;} // Frequenzmessung (Diagnose)
SendBufferByte();
}
// Zeichen in Ringpuffer schreiben — ohne Rücksicht auf Verluste
static void SendByte(uchar b) {
uchar w=SendW;
SendBuf[w]=b;
SendW=w+1;
}
static void SendBytes(const uchar *p, uchar l) {
do SendByte(*p++); while(--l);
}
static void SendBytes_P(const uchar *p, uchar l) {
do {SendByte(pgm_read_byte(p)); p++;} while(--l);
}
static void SendBytes_E(const uchar *p, uchar l) {
if (eeprom_is_ready()) do { // sonst nichts senden! Nicht blockieren!
eeprom_read(p);
SendByte(EEDR);
p++;
} while(--l);
}
// Zustands-Bytes senden (Kommando '@'):
// - Selektierte Achs-Nummer
// - Selektierte Greifer-Nummer
// - Greifer-Zustand
// - DemoFlags
static void SendReport() {
SendByte(Achse);
SendByte(Greifer);
SendByte(GreiferOp(Greifer));
SendByte(DemoFlags);
}
// Motor-Istwerte als 8-Byte-Struktur (little endian) schicken
// also <phase>, <accel>, <speed> und <ist> (Kommando 'A'..'F')
static void SendMotorReport(uchar idx) {
SendBytes((uchar*)&mot[idx],8);
}
// Belastungs-Info des Mikrocontrollers übertragen (Kommando '%')
static void SendLoad() {
SendBytes((uchar*)&Load,4);
Load.Min=0xFF; // Minimum und Maximum zurücksetzen
Load.Max=0;
}
static uchar RequestRemote(); // weiter unten
// Paket-Aufbau für Mehr-Byte-Kommandos
// [0] = Kommandobyte, stets >= 0x80
// [1] = Nutzdatenlänge, in Bytes, 0..254
// [2].. = Nutzdaten
static NOINIT uchar RecvPacket[8+256];
// Mitnutzung von RecvPacket für v-Liste vom Host
// bei koordinierter Bewegung, ab Index 8
// Gültig solange DemoFlags&0x20.
// Dann RecvPacket[1] == Anzahl beteiligter Kanäle (1..6)
// RecvPacket[2:7] == Liste der Kanäle
#define RecvBuf (RecvPacket+8)
static uchar RecvW, RecvR; // Indizes für RecvBuf (Umlauf-Puffer)
// Schreibindex für aufgeschobenes EEPROM-Schreiben
// beginnt mit 3 (weil 3-Byte-Adresse vor Schreibbytes in Nutzdaten)
// und endet mit RecvPacket[1]
static uchar EeWriteIdx;
static void HandleRecvPacket() {
uchar cmd=RecvPacket[0];
uchar len=RecvPacket[1];
Motor*pMot=&mot[cmd&0x07];
if (cmd>=0x80 && cmd<=0x85 && len<=sizeof(Motor)-8) { // fahren
pMot->Command(RecvPacket+2,len);
}else if (cmd>=0x88 && cmd<=0x8D && len<=sizeof(Motor)-8) { // Daten (Limits) setzen
pMot->SetData(RecvPacket+2,8,len);
}else if (uchar(cmd-0x8E) < 2) { // Umschalten zur koordinierten Bewegung?
if (uchar(len-1)<6) {
DemoFlags|=0x20; // Serielle Daten umlenken, Bahn-Modus anfordern
RecvW=RecvR=0; // mit leerem Puffer vorn beginnen
}
}else if (cmd>=0x90 && cmd<=0x95 && len==2) { // Motor-Struktur abfragen
cmd=RecvPacket[2]; // Offset
len=RecvPacket[3]; // Länge
if (cmd+len>sizeof(Motor)) return;
SendBytes((uchar*)pMot+cmd,len);
}else if (cmd>=0x98 && cmd<=0x9D && len>1) { // Motor-Struktur setzen (mit Vorsicht!)
pMot->SetData(RecvPacket+3,RecvPacket[2],len-1);
}else if (cmd==0xA0 && len==4) { // Speicher auslesen (adr,adrh,len)
uchar*addr=*(uchar**)(RecvPacket+2); // Quelladresse
char adrh=RecvPacket[4]; // High-Teil (80h = RAM, 81h = EEPROM)
len=RecvPacket[5];
if (len) {
if (adrh>=0) SendBytes_P(addr,len); // Flash lesen
else if (adrh==-128) SendBytes(addr,len); // RAM lesen
else SendBytes_E(addr,len); // EEPROM lesen
}
}else if (cmd==0xA1 && len>3) { // Speicher schreiben (adr, adrh, ...)
uchar*addr=*(uchar**)(RecvPacket+2); // Zieladresse
char adrh=RecvPacket[4];
if (adrh==-128) memcpy(addr,RecvPacket+5,len-3); // RAM schreiben
if (adrh==-127) EeWriteIdx=3; // EEPROM schreiben lassen
// (noch) kein Flash schreiben!
}
}
register uchar RecvI asm("r5"); // Schreibindex für RecvPacket
static long nData; // Datensatz-Zähler
// Byte von serieller Schnittstelle entgegennehmen und bearbeiten
static void HandleSerialInput() {
if (!(UCSRA&0x80)) return; // keine Daten
uchar inchar=UDR; // Byte einlesen
DemoFlags&=~0x10; // Bewegungsende-Suchen-Modus aus
if (DemoFlags&0x20) { // Bahn-Modus (für die Eingabedaten)?
if (UCSRA&0x10) { // BREAK detektiert?
HandleEos(0xA1); // Gewaltsam abbrechen
return;
}
RecvBuf[RecvW++]=inchar; // Byte abspeichern
if (RecvW==RecvR) {
HandleEos(0xA2); // Pufferüberlauf
return;
}
if (inchar==0x80) DemoFlags&=~0x20; // Normalmodus für kommende Bytes, Anforderung löschen
}else if (RecvI) { // Beim Paket-Empfang?
RecvPacket[RecvI]=inchar; // Byte speichern
RecvI++; // jetzt mindestens =2
if ((uchar)(RecvI-2)==RecvPacket[1]) {
HandleRecvPacket(); // Paket im Ganzen behandeln
RecvI=0;
}
}else if(inchar>='A' && inchar<='F') SendMotorReport(inchar-'A');
else if (inchar=='#') SendByte(inchar); // Echo
else if (inchar=='$') SendBytes((uchar*)&nData,4);
else if (inchar=='%') SendLoad();
else if (inchar=='&') DemoFlags|=0x10;
else if (inchar=='?') {SendByte('S'); SendByte('M'); SendByte('1');}
else if (inchar>='0' && inchar<='8') Achse=inchar-'0';
else if (inchar>='G' && inchar<='Z') { // Greifer 0..19 (oder 0..3)
if (pe.length==7 || inchar<='J') {
Greifer=inchar-'G';
ZeigeGreifer();
}
}
else if (inchar=='+') HandleWheelStep(0,1);
else if (inchar=='-') HandleWheelStep(0,-1);
else if (inchar=='@') SendReport();
else if (inchar=='a') HandleWheelStep(inchar,1);
else if (inchar=='b') HandleWheelStep(inchar,-1);
else if (inchar=='c') BrakeAll();
else if (inchar>=20 && inchar<=25) mot[inchar-20].Brake();
else if (inchar=='d') DemoFlags=~DemoFlags&2|1; // Demo Serienkinematik
else if (inchar=='e') DemoFlags=~DemoFlags&2|5; // Demo Parallelkinematik
else if (inchar=='g') HandleGreifer();
else if (inchar=='h') HandleGreifer(1);
else if (inchar=='f') Motor::NullAll();
else if (inchar>='n' && inchar<='s') mot[inchar-'n'].Null();
else if (inchar=='t') Motor::UnrefAll();
else if (inchar>='u' && inchar<='z') mot[inchar-'u'].Unref();
else if (inchar=='~') SendByte(RequestRemote());
else if (inchar>=0x80) {
RecvPacket[0]=inchar;
EeWriteIdx=0; // EEPROM schreiben terminieren (sonst Chaos)
if (!(DemoFlags&0x80)) RecvI=1; // Folgebytes NUR im Normalmodus entgegennehmen
}
}
// Sendet '&' beim Beenden von Zielfahrt oder EEPROM schreiben, wenn angefordert
static void HandleEndOfMove() {
if (DemoFlags&0x10 && !EeWriteIdx && !Motor::InMoveAny()) {
SendByte('&');
DemoFlags&=~0x10;
}
}
/*************************
* Koordinierte Bewegung *
*************************/
// Ende: Kode senden und Zurückschalten
// Fehlerkodes: Fx = Beschleunigung zu groß (x = betreffender Index)
// Ex = Geschwindigkeit zu groß
// Dx = Position außerhalb erlaubter Bereich
// Cx = Datenstrom-Abriss (Unterlauf) mit speed≠0
// Bx = End Of Stream erkannt, speed≠0, wird aus 00 oder 80 generiert
// A3 = CNAN (0x80) an nicht-erster Position
// A2 = Puffer-Überlauf
// A1 = Abbruch durch BREAK
// A0 = Taste HALT (S802) betätigt
// 81 = Datenstrom-TimeOut (Unterlauf, speed=0) (beinahe OK)
// 80 = End Of Stream erkannt, speed=0 (OK)
static void HandleEos(uchar code) {
if (code==0x80) {
uchar i=0;
uchar le=RecvPacket[1];
do{
Motor&m=mot[RecvPacket[2+i]];// Ziel-Objekt adressieren
if (m.speed) {
m.Brake();
if (code==0x80) code=0xB0+i; // ersten Problemfall melden
else code|=0x0F; // weitere Problemfälle andeuten
}
i++;
}while(--le);
}
if (uchar(RecvW-RecvR)>=128) SendByte(0x11); // mit XON evtl. blockierende Gegenstelle enteisen
SendByte(code);
DemoFlags&=~0xF0; // Zweite Hauptschleife verlassen
}
static void WaitTick2();
// Unterlauf des Empfangspuffers:
// Gnadenfrist 64 ms bei v=0, sonst Stream-Ende
static void HandleUnderrun() {
uchar i=0;
uchar le=RecvPacket[1];
do{
Motor&m=mot[RecvPacket[2+i]]; // Ziel-Objekt adressieren
if (m.speed) {
HandleEos(0xC0+i); // Fehlerkode für v!=0
return;
}
i++;
}while(--le);
// Warte max. 64 ms auf das Eintrudeln von "n" Bytes im Empfangspuffer
do{
WaitTick2();
if (RecvW-RecvR>=RecvPacket[1]) return;
}while(--le);
HandleEos(0x81); // "Halbes" OK für v==0
}
static void SetNextSpeed() {
uchar le=RecvPacket[1]; // Anzahl der erwarteten Datenbytes
uchar lr=RecvR; // lokale Kopie
uchar lv=RecvW-lr; // Anzahl der vorhandenen Datenbytes
if (lv && RecvBuf[lr]==0x80) {
HandleEos(0x80); // korrektes Ende
return;
}
if (lv<le) {
HandleUnderrun();
return; // Problem!!
}
SendByte(lv>=128?0x13:0x11); // ständig XOFF oder XON senden
uchar i=0;
do{
int x=(signed char)RecvBuf[lr]; // auslesen
if (x==-128) {
HandleEos(0xA3); // CNAN an nicht-erster Position!
return;
}
Motor&m=mot[RecvPacket[2+i]];// Ziel-Objekt adressieren
if (DemoFlags&0x40) { // a-Modus
if (x) {
int v=m.speed+x; // v prüfen und ändern
if (abs(x)>m.OneAccel) {HandleEos(0xF0+i); return;}
if (abs(v)>m.MaxSpeed) {HandleEos(0xE0+i); return;}
m.speed=v;
}
}else{ // v-Modus
int a=x-m.speed; // a prüfen
if (a) {
if (abs(a)>m.OneAccel) {HandleEos(0xF0+i); return;}
if (abs(x)>m.MaxSpeed) {HandleEos(0xE0+i); return;}
m.speed=x; // setzen
}
}
lr++;
i++;
}while(--le);
nData++;
RecvR=lr;
}
static void PrepareOneAccel() {
uchar le=RecvPacket+1; // Anzahl beteiligter Achsen
for (uchar i=0; i<le; i++) {
Motor&m=mot[RecvPacket[2+i]]; // Ziel-Objekt adressieren
uchar a=m.MaxAccel;
uchar j=0,k=5; // 5 = log2(MAXACCEL/Abtastrate[kHz])
do{
a>>=1;
j|=SREG; // C-Flag sammeln
}while (--k);
a+=j&1; // aufrunden wenn Bit rausgeschoben wurde
m.OneAccel=a;
}
}
void CheckLimit(uchar i) {
if (DemoFlags&0x80 && i<RecvPacket[1]) mot[RecvPacket[2+i]].CheckLimit(0xD0+i);
}
/*********************************************
* Blinkende LED, die die Achsnummer anzeigt *
*********************************************/
static ushort LedBlinkCount;
// Start gezähltes Blinken der grünen LED, <count> = sinnvoll 1..6
static void StartLedBlink(uchar count) {
LedBlinkCount=((ushort)count<<8)-0x80; // Pegelwechsel in den Bits 15:7
PORTC|=0x01; // LED ein
}
// Gelbe LED blinken lassen (periodischer Aufruf)
static void HandleLedBlink() {
if (!LedBlinkCount) return; // nichts tun wenn Null
if (!(--LedBlinkCount&0x7F)) // Zeit abwarten (in den Bits 6:0)
PORTC^=0x01; // LED umschalten (ATmega8 toggelt nicht auf PINC)
}
/*******************************
* Tasten und Inkrementalgeber *
*******************************/
register uchar KeyState asm("r2");
/* Bit 0 = S801 (rechts)
Bit 1 = S802
Bit 2 = S803
Bit 3 = S804 (links)
Bit 5:4 = Inkrementalgeber-Schaltzustand (binär)
Bit 7:6 = Inkrementalgeber-Ganzperiodenzähler (für '~'-Kommando)
*/
// 4 Tasten und 1 Inkrementalgeber in einem Byte einlesen
static uchar KeyRead() {
uchar k=(uchar)~PINC>>3&0x07 | (uchar)~PIND<<1&0x38;
if (k&0x20) k^=0x10; // Inkrementalgeber binärkodiert einlesen
uchar ksh=KeyState&0xF0; // altes High-Nibble
ksh+=(char)((k&0xF0)-ksh<<2)>>2; // neues High-Nibble
return (k&0x0F)+ksh;
}
// liefert geänderte Bits sowie Vor/Zurück-Info für Inkrementalgeber:
// Bit 4 = 1: Inkrementalgeber-Aktivität
// Bit 5 = Richtung der Inkrementalgeber-Aktivität, sonst undefiniert
static uchar KeyPress() {
uchar ks=KeyRead(); // Tasten einlesen
uchar kss=(ks^KeyState)&0x0F; // geänderte Bits (nur Tasten)
uchar diff=(ks&0xF0)-(KeyState&0xF0); // Differenz per Subtraktion
KeyState=ks; // neuer Tastenstatus
kss|=diff;
return kss; // Änderungen mitteilen
}
register uchar RemoteCount asm("r7");
static uchar RequestRemote() {
RemoteCount=100; // 100 ms
LedBlinkCount=0; // Blinken abbrechen
PORTC^=0x01; // LED umschalten
return KeyState;
}
static void CountdownRemote() {
if (RemoteCount && !--RemoteCount) {
PORTC&=~0x01; // LED aus
}
}
static void EndRemote() {
RemoteCount=0;
PORTC&=~0x01; // LED aus
}
/* Tastenbelegung: (↓ = Drücken, ↑ = Loslassen, ▼ = Halten, ▲ = gelöst)
S4 S3 S2 S1
↓ ▲ Auswählen Achse X-A-Y (Werkstück)
▲ ↓ Auswählen Achse Z-B-C-6-7-8 (Greiferplattform)
↓ Motoren Stop (bei Fernsteuerung oder Demo-Modus)
↑ Motoren Stop (auch Lokalbedienung)
▼ Geschwindigkeit statt Schritte mit Inkrementalgeber
▲ ↓ Greifer betätigen oder lösen
▼ ↓ Referenzfahrten starten (S2 festhalten bis Ende) ◊
↓ ▼ Start/Stopp Demo/Schmier-Modus (Fahren zwischen Endschaltern)
▼ ↓ Greifer (1 aus 4 oder 1 aus 20) auswählen
▼ ▼ ▼ ↓ Bootloader aktivieren
▼ beim Einschalten: Bootloader
◊ setzt beide Endschalter anhand bekannter Weglängen;
betrifft nur Achsen ohne Hardware-Endschalter
*/
register char LastStep asm("r4"); // für Hysterese-Effekt beim Handrad (Entprellen)
static void HandleKeyInput() {
uchar kss=KeyPress(); // kss = geänderte Tasten
if (RemoteCount) return; // keine Lokalbedienung (nicht mal Bootloader)
if (!(kss&0x1F)) return; // Abkürzung
if (kss&KeyState&8) { // linke Taste S804 niedergedrückt?
if (KeyState&4) { // Start/Stopp Demo/Schmier-Modus
DemoFlags=DemoFlags&0xF0|DemoFlags+1&0x0F; // Low-Nibble erhöhen
if (!(DemoFlags&1)) Motor::BrakeAll();
}else{ // Auswählen Achse X-A-Y (Werkstück)
Achse++;
if (Achse>2) Achse=0;
StartLedBlink(Achse+1);
}
}
if (kss&KeyState&4) { // nächste Taste S803 niedergedrückt?
if (KeyState&8) { // Greifer (1 aus 4 oder 1 aus 20) auswählen
if (Greifer==1) Greifer++; // Greifer 2 gibt's nicht! 090803
Greifer++; // durchschalten
if (pe.length==5 && Greifer==4 || Greifer==20) Greifer=0;
ZeigeGreifer();
StartLedBlink(Greifer+1); // Auswahl anzeigen (1..4 x blinken)
}else{ // Auswählen Achse Z-B-C-6-7-8 (Greifer)
Achse++;
if ((uchar)(Achse-3)>(mot[0].IsRef()?2:5)) Achse=3;
StartLedBlink(Achse-2);
}
}
if (kss&2) BrakeAll(); // Taste S802 gedrückt oder losgelassen?
if (kss&KeyState&1) {
if (!(~KeyState&0x0F)) Bootloader(); // Bootloader anspringen
else if (KeyState&2) Motor::DoRefAll(); // Referenzfahrten starten
else HandleGreifer(); // Greifer betätigen
}
if (kss&0x10) { // Inkrementalgeber?
char step=1;
if (kss&0x20) step=-step;
if (step==LastStep) HandleWheelStep(KeyState&2,step);
else LastStep=step; // ersten Schritt ignorieren
}
}
static void HandleKeyInput2() {
uchar kss=KeyPress(); // kss = geänderte Tasten
if (kss) SendByte(kss&0x30|KeyState&0x0F|0x40);
if (KeyState&0x02) HandleEos(0xA0);
}
#define TIMEOUT TIFR&0x01
// zum Aufwecken aus Schlafmodus
EMPTY_INTERRUPT(TIMER0_OVF_vect);
// wartet auf Timer-„Interrupt“, alle 125 µs
// Danach Port-Expander aktualisieren und Motorpositionen errechnen
static void WaitTick() {
uchar t=TCNT0;
if (TIMEOUT) t=0xFF;
if (Load.Min>t) Load.Min=t;
if (Load.Max<t) Load.Max=t;
Load.Avg=(((long)(Load.Avg+t)<<8)-Load.Avg)>>8; // ohne Runden
// PORTC&=~1;
if (!TIMEOUT) {
sei();
sleep_cpu();
cli();
TCNT0=T0START; // Zeit verkürzen auf 125 µs (8 kHz)
}
TIFR=0x01; // Interrupt-Flag löschen
if (pe.dirty) pe.ShiftOut();
// PORTC|=1; // Rechenleistung abschätzen
Motor::PeriodicAll();
}
static void WaitTick2() {
WaitTick();
HandleSerialInput();
WaitTick();
HandleSerialOutput();
}
/*************************
* EEPROM-Update-Routine *
*************************/
// Speichert Ist-Position und phase-Bits
// ODER vollzieht aufgeschobenen EEPROM-Schreibbefehl
static void HandleEepromUpdate() {
if (!eeprom_is_ready()) return;
uchar*a; // EEPROM-Adresse
if (EeWriteIdx) { // Aufgeschobener EEPROM-Schreibbefehl?
a=*(uchar**)&RecvPacket[2];
eeprom_update_byte(a,RecvPacket[2+EeWriteIdx]);
a++;
*(uchar**)&RecvPacket[2]=a; // zurückschreiben
EeWriteIdx++;
if (EeWriteIdx==RecvPacket[1]) EeWriteIdx=0; // Ende
return;
}
uchar*m=(uchar*)mot; // = 0x0060
a=(uchar*)ee.em; // = 0x0008
do{
if (*m&Motor::FULL) { // Während Bestromung nicht speichern!
a+=sizeof(EeMotor);
m+=sizeof(Motor);
continue;
}
m+=4; // Ist
for (uchar i=0; i<5; i++) {
eeprom_update_byte(a,*m);
if (!eeprom_is_ready()) return;
a++; // nächstes Byte
m++;
}
a+=sizeof(EeMotor)-5; // restliche Bytes überspringen
m+=sizeof(Motor)-5-4; // Veränderte Limits /nicht/ aktualisieren!
}while(m<(uchar*)&mot[6]);
}
/*************************************
* Initialisierung und Hauptschleife *
*************************************/
int main(void) {
#if F_CPU != 8000000
SaveOsccal=OSCCAL;
# if F_CPU == 12000000
OSCCAL=0xDE;
# else
OSCCAL=0xFF; // Vollgas!
# endif
#endif
// Timer 0 aktivieren: Zeitgeber
TCCR0=0b00000010; // Vorteiler 8: 7,5 kHz = 133 µs Maximum
TIMSK=0x01; // Timer0-Überlaufinterrupt frei
MCUCR=0x80; // Sleep (= Idle) aktivieren
Load.Min=0xFF;
// Ports initialisieren (macht eigentlich schon der Bootloader!)
PORTB=0b11110101;
PORTC=0b00111100;
PORTD=0b11111100;
DDRB =0b00101010; // Ausgänge außer MISO
DDRC =0b00000011; // LEDs als Ausgang
DDRD =0b00000010; // TxD = Ausgang
// SPI initialisieren
SPCR =0b01010000; // MSB zuerst, SPI-Master, Mode 0
SPSR =0b00000001; // mit 7,7 MHz schieben
// 5 FF-Bytes in die Schieberegister ausgeben
pe.Init();
pe.ShiftOut();
Motor::InitAll();
KeyState=Achse=DemoFlags=LastStep=RemoteCount=RecvI=0;
KeyState=KeyRead();
DemoSpeed=DEMOSPEED;
// Serielle Schnittstelle initialisieren
UBRRL = (uchar)(F_CPU/16/BAUDRATE-0.5);
UCSRA = 0x00; // kein Turbo-Modus
UCSRB = 0x18; // 8-n-1
normal:
// Schema der Hauptschleife, zur Lastverteilung
// Es wird 8x WaitTick() aufgerufen, welches neue Motorpositionen anhand
// der aktuellen Geschwindigkeit errechnet und ausgibt (125 µs = 8 kHz je Motor).
// In den Zeiten dazwischen wird für jeden Motor eine Beschleunigung
// oder Verzögerung berechnet und Endschalter abgefragt (1 ms = 1 kHz je Motor).
// In den zwei verbleibenden Zeitschlitzen wird die Lokalbedienung u.ä.
// abgefragt (1 ms = 1 kHz).
// Die serielle Schnittstelle wird dazwischen bedient (250 µs = 4 kHz),
// da sind 38,4 kBaud (max. 3840 Bytes pro Sekunde) adäquat.
do{
WaitTick(); // insgesamt 8 Aufrufe von WaitTick()
mot[0].CalcSpeed();
HandleSerialOutput();
WaitTick();
mot[1].CalcSpeed();
HandleSerialInput();
WaitTick();
mot[2].CalcSpeed();
HandleSerialOutput();
WaitTick();
mot[3].CalcSpeed();
HandleSerialInput();
WaitTick();
mot[4].CalcSpeed();
HandleSerialOutput();
WaitTick();
mot[5].CalcSpeed();
HandleSerialInput();
WaitTick();
HandleKeyInput();
HandleEndOfMove();
HandleSerialOutput();
WaitTick();
HandleLedBlink();
HandleBelueftung();
CountdownRemote();
HandleEepromUpdate();
HandleSerialInput();
}while (!(DemoFlags&0x20));
DemoFlags|=RecvPacket[0]-0x8C<<6; // setzt Bit 7 — sowie Bit 6 im a-Modus
PrepareOneAccel(); // MaxAccel der beteiligten Achsen anpassen
RequestRemote(); // keine Lokalbedienung
nData=0;
// Koordinierte Bewegung (zweite Hauptschleife)
// ebenfalls mit Lastverteilung — für den Endschalter-Test
while (DemoFlags&0x80) {
WaitTick2(); // insgesamt 16 Aufrufe von WaitTick()
CheckLimit(0);
WaitTick2();
CheckLimit(1);
WaitTick2();
CheckLimit(2);
WaitTick2();
CheckLimit(3);
WaitTick2();
CheckLimit(4);
WaitTick2();
CheckLimit(5);
WaitTick2();
HandleKeyInput2();
// ┌──┬──┬──┬──┬──┬──┬──┬──┐
// mit Kode-Bits │ 0│ 1│I-│ I│T3│T2│T1│T0│
// └──┴──┴──┴──┴──┴──┴──┴──┘
HandleBelueftung();
WaitTick2();
if (DemoFlags&0x80) SetNextSpeed(); // mit 500 Hz
}
BrakeAll();
EndRemote();
goto normal;
}
// Beachte: Maximale Flash-Größe: 7168 Bytes! (87,5%)
| Detected encoding: UTF-8 | 0
|