/* 6-fach-Schrittmotorsteuerung
* tabsize = 8, encoding = utf-8 (ohne Windows-Header BOM)
* Änderungen (+dazu, *geändert, -Bugfix):
140127 erstellt aus sm2
*/
#define F_CPU 16000000
#define BAUDRATE 38400 // stets 8N1
#include <string.h> // memset
#include <stdlib.h> // abs()
#include <msp430x261x.h>
#define SREG read_sr()
static uint16_t read_sr() {
uint16_t x;
asm volatile("mov r2,%0":"=r"(x):);
return x;
}
/************
* Hardware *
************/
/* Verwendung der Ports:
12 P1.0 K10 Strom-Bit
13 P1.1 P10 BL:TxD Phase
14 P1.2 J10 Strom-Bit
15 P1.3 E10 Endschalter
16 P1.4 K11 Strom-Bit
17 P1.5 P11 Phase
18 P1.6 J11 Strom-Bit
19 P1.7 E11 Endschalter
20 P2.0 K0 Strom-Bit
21 P2.1 P0 Phase
22 P2.2 J0 BL:RxD Strom-Bit
23 P2.3 E0 Endschalter
24 P2.4 K1 Strom-Bit
25 P2.5 P1 Phase
26 P2.6 J1 Strom-Bit
27 P2.7 E1 Endschalter
28 P3.0 M0 Analogmultiplexer
29 P3.1 M1 Analogmultiplexer
30 P3.2 M2 Analogmultiplexer
31 P3.3 G3 Greifer-Port, RS485-Sendefreigabe
32 P3.4 TxD TxD0 RS232-Sendedaten
33 P3.5 RxD RxD0 RS232-Empfangsdaten
34 P3.6 G2 TxD1 Greifer-Port, RS485-Kopplung
35 P3.7 G1 RxD1 Greifer-Port, RS485-Kopplung
36 P4.0 K2 Strom-Bit
37 P4.1 P2 Phase
38 P4.2 J2 Strom-Bit
39 P4.3 E2 Endschalter
40 P4.4 K3 Strom-Bit
41 P4.5 P3 Phase
42 P4.6 J3 Strom-Bit
43 P4.7 E3 Endschalter
44 P5.0 K4 Strom-Bit
45 P5.1 P4 Phase
46 P5.2 J4 Strom-Bit
47 P5.3 E4 Endschalter
48 P5.4 K5 Strom-Bit
49 P5.5 P5 Phase
50 P5.6 J5 Strom-Bit
51 P5.7 E5 Endschalter
75 P6.0 B Bedienfeld: frei
76 P6.1 A Bedienfeld: L = 4 Tasten abfragen
77 P6.2 E Bedienfeld: L = Inkrementalgeber abfragen
2 P6.3 D4 Bedienfeld: Geber, blaue Taste
3 P6.4 D5 Bedienfeld: Geber, grüne Taste
4 P6.5 D6 Bedienfeld: gelbe LED, graue Taste
5 P6.6 D7 Bedienfeld: grüne LED, rote Taste
6 P6.7 DAC1 DAC1 Stromvorgabe -/-/I2/I1/I0/I9/I11/I10
28 P7.0 K6 Strom-Bit
29 P7.1 P6 Phase
30 P7.2 J6 Strom-Bit
31 P7.3 E6 Endschalter
32 P7.4 K7 Strom-Bit
33 P7.5 P7 Phase
34 P7.6 J7 Strom-Bit
35 P7.7 E7 Endschalter
62 P8.0 K8 Strom-Bit
63 P8.1 P8 Phase
64 P8.2 K8 Strom-Bit
65 P8.3 E8 Endschalter
66 P8.4 K9 Strom-Bit
67 P8.5 P9 Phase
68 P8.6 K9 Strom-Bit
69 P8.7 E9 Endschalter
70 TDO/TDI X6 JTAG
71 TDI/TCLK X8 JTAG
72 TMS X9 JTAG
73 TCK X10 BL:RTS JTAG
74 !RST/NMI X11 BL:DTR JTAG; Bootloader
7 UREF+ Kondensator
8 XIN Quarz (nicht bestückt)
9 XOUT Quarz (nicht bestückt)
10 UREF+/ DAC0 DAC0 Stromvorgabe -/-/I8/I3/I6/I4/I7/I5
11,53,78,79 GND
1,52,80 3P3
*/
#define F_MOT 8000 // 8 kHz Schrittfrequenz
#define elemof(x) (sizeof(x)/sizeof(*(x)))
#define nobreak
typedef unsigned char uchar;
typedef unsigned long ulong;
typedef unsigned short ushort;
/*****************************************************
* ehemaliger EEPROM-Speicherinhalt (Achsdaten usw.) *
*****************************************************/
struct EeCtrl{
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, +∞
/* ATmega-EEPROM-Daten sind nun im Flash-Speicher */
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*/ 0, // dies entspricht Load = 0 %
/*uchar lf*/ 255, // dies entspricht Load = 100 %
/*uchar bd*/ 0x19, // Baudrate gemäß Baudrate.htm
/*uchar nm*/ 0x46}, // 6 Achsen (Low-Nibble) à 48 Byte, Teilerfaktor 2^4 = 16 → 500 Hz
// Offset = 8: echte Achs- und Motordaten (LOWNIBBLE(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 Flash-Zugriff (hier wichtig: nicht-blockierend) *
*****************************************************************/
/***************
* Endschalter *
***************/
volatile uchar *const PortInTab[6]={&P2IN,&P4IN,&P5IN,&P7IN,&P8IN,&P1IN};
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)
};
// 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) {
uchar sw=*PortInTab[i];
if (es&ESI) sw=~sw;
return sw&(es&ESS?0x80:0x08);
}
/**********************************
* Klasse Motor (für 6 Instanzen) *
**********************************/
volatile uchar *const PortOutTab[6]={&P2OUT,&P4OUT,&P5OUT,&P7OUT,&P8OUT,&P1OUT};
// Phasentabelle für Halbschrittbetrieb, reduzierte und volle Bestromung
// Bit 0 = Strom-Bit I1 "K", Bit 1 = Phase "P", Bit 2 = Strom-Bit I0 "J"
const uchar PhaseTab[16]={
0x35, /* -19%, 0 */ 0x25, /* -100%, 0 */
0x31, /* -19%, +19% */ 0x64, /* - 60%, + 60% */
0x51, /* 0 , +19% */ 0x50, /* 0 , +100% */
0x11, /* +19%, +19% */ 0x44, /* + 60%, + 60% */
0x17, /* +19%, 0 */ 0x07, /* +100%, 0 */
0x13, /* +19%, -19% */ 0x46, /* + 60%, - 60% */
0x73, /* 0 , -19% */ 0x72, /* 0 , -100% */
0x33, /* -19%, -19% */ 0x66}; /* - 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)
* 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
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];
uchar DemoFlags;
// 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=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 (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; // Miroschritt gleich Null!
}
phase|=BRAKE;
}
// Aus <ee.em> ab <offset> <len> Bytes lesen
void Motor::ReadEeprom(void*d, uchar offset, uchar len) const {
memcpy(d,&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
int sr=SREG; // Statusregister nach der vzb. Addition auswerten
if (sr&1<<8) { // vzb. Überlauf aufgetreten?
if (sr&1<<2) 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 *
****************************************************/
static void Bootloader() {
goto *(void*)(0x1C00/2); // bei 7 KByte (WORD-Adresse)
}
// Aktuelle Achse (die mit dem Inkrementalgeber gesteuert wird)
uchar Achse;
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; // Z Lineartisch
case 3: (mot[0].*fp)(step); break; // Einzelmotoren unbelegt
case 4: (mot[1].*fp)(step); break;
case 5: (mot[2].*fp)(step); break;
}
}
}
// 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
UCA0TXBUF=SendBuf[r]; // Zeichen ausgeben
SendR=r+1; // Lesezeiger anpassen
}
// Byte zu serieller Schnittstelle schicken (Polling)
static void HandleSerialOutput() {
if (!(IFG2&0x02)) return; // Schnittstelle beschäftigt
if (DemoFlags==8) {UCA0TXBUF='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);
}
// Zustands-Bytes senden (Kommando '@'):
// - Selektierte Achs-Nummer
// - Null (war: Greifer-Nummer)
// - Null (war: Greifer-Zustand)
// - DemoFlags
static void SendReport() {
SendByte(Achse);
SendByte(0);
SendByte(0);
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 (unsigned(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(addr,len); // Flash lesen
else if (adrh==-128) SendBytes(addr+0x1100,len); // RAM lesen
else SendBytes(addr+(int)&ee,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
}
}
uchar RecvI; // Schreibindex für RecvPacket
static long nData; // Datensatz-Zähler
// Byte von serieller Schnittstelle entgegennehmen und bearbeiten
static void HandleSerialInput() {
if (!(IFG2&0x01)) return; // keine Daten
uchar inchar=UCA0RXBUF; // Byte einlesen
DemoFlags&=~0x10; // Bewegungsende-Suchen-Modus aus
if (DemoFlags&0x20) { // Bahn-Modus (für die Eingabedaten)?
if (UCA0STAT&0x08) { // 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('2');}
else if (inchar>='0' && inchar<='7') {
Achse=inchar-'0';
if (Achse<6) P6OUT&=~0x20; else P6OUT|=0x20; // LED ebenfalls schalten
}
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=='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.MaxAccel) {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.MaxAccel) {HandleEos(0xF0+i); return;}
if (abs(x)>m.MaxSpeed) {HandleEos(0xE0+i); return;}
m.speed=x; // setzen
}
}
lr++;
i++;
}while(--le);
nData++;
RecvR=lr;
}
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
P6OUT|=0x40; // 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)
P6OUT^=0x40; // LED umschalten
}
/*******************************
* HD44780-kompatibles Display *
*******************************/
// TODO
/*******************************
* Tasten und Inkrementalgeber *
*******************************/
static uchar KeyState;
/* 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=~P6IN<<1&0x30; // Inkrementalgeber einlesen
P6DIR&=~0x60;
P6OUT&=~0x08; // A umschalten
k|=~P6IN>>3&0x0F; // Tasten einlesen (Low-Nibble)
P6OUT|=0x08; // A zurückschalten
P6DIR|=0x60; // LEDs aktivieren
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
}
static uchar RemoteCount;
static uchar RequestRemote() {
RemoteCount=100; // 100 ms
LedBlinkCount=0; // Blinken abbrechen
P6OUT^=0x40; // LED umschalten
return KeyState;
}
static void CountdownRemote() {
if (RemoteCount && !--RemoteCount) {
P6OUT&=~0x40; // LED aus
}
}
static void EndRemote() {
RemoteCount=0;
P6OUT&=~0x40; // LED aus
}
/* Tastenbelegung: (↓ = Drücken, ↑ = Loslassen, ▼ = Halten, ▲ = gelöst)
S4 S3 S2 S1
↓ ▲ Auswählen Achse X-Y-Z (Lineare Achsen)
▲ ↓ Auswählen Achse A-B-S (Drehachsen)
↓ Motoren Stop (bei Fernsteuerung oder Demo-Modus)
↑ Motoren Stop (auch Lokalbedienung; auch Fokusmotor)
▼ Geschwindigkeit statt Schritte mit Inkrementalgeber
▲ ↓ Fokusmotor (1 aus 2) auswählen (grüne LED ständig EIN)
▼ ↓ Referenzfahrten starten (S2 festhalten bis Ende) ◊
↓ ▼ Start/Stopp Demo/Schmier-Modus (Fahren zwischen Endschaltern)
▼ ▼ ▼ ↓ Bootloader aktivieren
▼ beim Einschalten: Bootloader
◊ setzt beide Endschalter anhand bekannter Weglängen;
betrifft nur Achsen ohne Hardware-Endschalter
*/
static char LastStep; // 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;
P6OUT&=~0x20;
StartLedBlink(Achse+1);
}
}
if (kss&KeyState&4) { // nächste Taste S803 niedergedrückt?
if (KeyState&8) { // zz. freie Shift-Funktion
}else{ // Auswählen Achse 3-4-5 (frei)
Achse++;
if ((uchar)(Achse-3)>2) Achse=3;
P6OUT&=~0x20;
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{
Achse++;
if ((uchar)(Achse-6)>1) Achse=6; // Fokusmotor auswählen
P6OUT|=0x20;
StartLedBlink(Achse-5);
}
}
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);
}
#if 0
// 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();
}
#else
static void WaitTick() {}
#endif
static void WaitTick2() {
WaitTick();
HandleSerialInput();
WaitTick();
HandleSerialOutput();
}
/************************
* Flash-Update-Routine *
************************/
// Speichert Ist-Position und phase-Bits
// ODER vollzieht aufgeschobenen EEPROM-Schreibbefehl
static void HandleEepromUpdate() {
}
/*************************************
* Initialisierung und Hauptschleife *
*************************************/
int main(void) {
volatile uchar *const PortRenTab[]={&P2REN,&P4REN,&P5REN,&P7REN,&P8REN,&P1REN};
volatile uchar *const PortDirTab[]={&P2DIR,&P4DIR,&P5DIR,&P7DIR,&P8DIR,&P1DIR};
Load.Min=0xFF;
// Ports initialisieren
for (int i=0; i<6; i++) {
*PortOutTab[i]=0xFF; // Strom weg, Pullups
*PortRenTab[i]=0x88;
*PortDirTab[i]=0x77;
}
P3OUT=0xF8;
P3REN=0xA0;
P3DIR=0x5F;
P3SEL=0xF0; // Serielle Schnittstellen drauf legen
P6OUT=0x7F;
P6REN=0x1F;
P6DIR=0x66;
P6SEL=0x80; // DAC1 aktivieren
Motor::InitAll();
KeyState=KeyRead();
DemoSpeed=DEMOSPEED;
// Serielle Schnittstelle initialisieren
UCA0BR0 = (uchar)(F_CPU/16/BAUDRATE-0.5);
UCA0CTL1 = 0x40; // mit Break
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();
CountdownRemote();
HandleEepromUpdate();
HandleSerialInput();
}while (!(DemoFlags&0x20));
DemoFlags|=RecvPacket[0]-0x8C<<6; // setzt Bit 7 — sowie Bit 6 im a-Modus
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│
// └──┴──┴──┴──┴──┴──┴──┴──┘
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
|