Contents

Einführung

Autor: Thomas Block
Datum: Juli 1997

Der PC hat in der heutigen Zeit eine Leistungfähigkeit erreicht, die es ermöglicht, ihn auch für schnelle, komplexe Reglungsaufgaben einzusetzen. Der PC ist preisgünstig und wird durch eine imense Zahl von Tools und Programmen unterstüzt, so daß er eine ideale Plattform zur Entwicklung eines digitalen Lagereglers darstellt. Bei der Suche nach unterstützender Literatur wurde ich leider nur begrenzt fündig, so daß ich mich entschloß dieses Buch zu schreiben. Es erhebt nicht den Anspruch die Grundlagen der Reglunstechnik zu behandeln, es soll vielmehr dem Leser ermöglichen, selbständig einen vollständigen digitalen PID-T1-Lageregler auf einfache Weise zu implementieren. Durch einige Änderungen in der Software können natürlich auch andere Größen als die Lage geregelt werden.

Technische Daten:

Regelalgorithmus: PID-T1
Anzahl Achsen: 1 bis 8
Zykluszeit: 2 ms bei 486DX2-66 MHz
Beschleunigung: gleichförmig oder sinusförmig
Vorsteuerung: Geschwindigkeitsvorsteuerung
Bewegung: alle Achsen unabhängig oder elektronisch gekoppelt
Lageerfassung: Inkrementaldekoder
Ausgang: +/- 10 V analog für Soll- und Istwert
sonstiges: digitale Tachosimulation

Kleine Einführung in die Analogregeltechnik

Die Steuerung

Die Steuerung möchte ich an Hand eines einfachen Beispiels näherbringen Ich möchte mit einem Auto von meiner Wohnung zum nächsten Briefkasten fahren, wenn das ökologisch auch nicht besonders sinnvoll sein mag. Ich kenne diese Strecke außerordentlich gut, ich könnte sie sozusagen im Schlaf fahren. Ich setze mich also ins Auto schließe meine Augen und fahre los. Das geht doch nicht, meinen Sie? So aber funktioniert eine Steuerung. D.h. ich kenne den Verlauf der Strecke und fahre genau nach diesem Vorbild los. Wenn auf der Strecke nichts unvorhergesehendes passiert, könnte das ganze sogar klappen. Doch wehe irgend etwas ist anders, als ich mir das vorher ausgemahlt habe.

Im technischen läuft das ganz ähnlich, ein Rechner kennt einen bestimmten Verlauf (Sollwert) und sendet die berechneten Werte (Stellgröße) an eine unterlagerte Einheit, die dann der Vorgabe folgt. Treten Störungen auf, so führt das zu Abweichungen, die nicht korregiert werden.

Die Reglung

Damit das Problem der Abweichung minimiert oder sogar entfällt, setzt man eine Reglung ein. Beim Autofahren öffne ich meine Augen und schalfe natürlich nicht. Trifft nun ein unvorhergesehendes Ereignis ein, so reagiere ich darauf, ich regle sozusagen mein Auto, bis es wieder auf dem richtigen Kurs ist. Technisch gelangt man von der Steuerung zur Reglung, wenn der Sollwert mit dem Istwert, z.B. dem Weg, verrechnet wird und aus dieser Rechnung eine Stellgröße ausgegeben wird. Es gibt eine Menge unterschiedlicher Regler und es kommen immer neuere hinzu, in der Praxis trifft man auf einige wenige Typen , die hier im näheren besprochen werden.

Der P-Regler

Der P-Regler ist ein Regler, bei dem die Differenz aus Soll- und Istwert verstärkt wird und dies auf den Ausgang gegeben wird. Wenn die Variable $xa$ die Ausgangsgröße, $xe$ die Eingangsgröße und $k_p$ die Verstärkung des Reglers ist, gilt:
$\displaystyle xa$ $\textstyle =$ $\displaystyle k_p \cdot xe$ (1.1)
$\displaystyle xe$ $\textstyle =$ $\displaystyle soll - ist$ (1.2)

Je größer die Verstärkung $k_p$ ist, desto kleiner bleibt die Abweichung vom Sollwert.Jedoch kann die Verstärkung nicht beliebig groß werden, da die Ausgangsgröße immer begrenzt sein wird. Ebenso kann eine zu große Verstärkung zu einem unerwünschten Verhalten führen. Der P-Regler ist ein schneller Regler, der immer eine Regelabweichung besitzt.

Der PD-Regler

Der PD-Regler ist eine Paralleschaltung von P- und D-Regler. Der D-Anteil differenziert dabei den Eingangswert $xe$. Für diesen Reglertyp gilt somit:
$\displaystyle xa$ $\textstyle =$ $\displaystyle k_p \cdot (xe + T_d \cdot \frac {dxe}{dt})$ (1.3)

Der PD-Regler ist schneller als der P-Regler und besitzt eine bleibende Regelabweichung.

Ein reines D-Glied ist physikalisch nicht realisierbar, es kann im Computer nur näherungsweise berechnet werden. Meist wird ein abgeschwächtes Glied, ein sogenanntes D-T1-Glied benutzt.

$\displaystyle xa + T_1 \dot xa$ $\textstyle =$ $\displaystyle k_p \cdot (xe + (T_d + T_1) \dot xe)$ (1.4)

Der PI-Regler

Der PI-Regler ist eine Paralleschaltung von P- und I-Regler. Der I-Anteil integriert den Eingangswert $xe$. Die mathematische Gleichung lautet:
$\displaystyle xa$ $\textstyle =$ $\displaystyle k_p \cdot (xe + \frac{1}{T_i} \int xe dt)$ (1.5)

Der PI-Regler ist langsamer als der PD-Regler. Der Vorteil des I-Reglers liegt darin, daß die Regelabweichung zu Null wird. Er kann in einem System höherer Ordnung aber zu Instabilitäten führen.

Der PID-Regler

Der PID-Regler ist eine Paralleschaltung von P-, I- und D-Regler. Die mathematische Gleichung lautet:
$\displaystyle xa$ $\textstyle =$ $\displaystyle k_p \cdot (xe + \frac{1}{T_i} \int xe dt + T_d \cdot \frac {dxe}{dt})$ (1.6)

Der PID-Regler vereinigt die Schnelligkeit des PD- und die Genauigkeit des PI-Reglers. Diese Vorteile haben dazu geführt, daß er sich in der Praxis stark durchgesetzt hat.

Der PID-T1-Regler

Der PID-Regler ist eine Paralleschaltung von P-, I- und DT1-Regler. Die mathematische Gleichung lautet:
$\displaystyle xa$ $\textstyle =$ $\displaystyle xe \cdot k_p \cdot (1 + \frac{1}{T_i \cdot s} + \frac {T_d \cdot s} {1 + T_1\cdot s} )$ (1.7)
$\displaystyle xa + T_1 \dot xa$ $\textstyle =$ $\displaystyle k_p \cdot (xe \cdot (1+\frac{T_1}{T_i}) + \frac{1}{T_i} \int xe dt + (T_d + T_1) \dot xe)$ (1.8)

Es gibt auch Regler, die den DT1-Anteil nicht von der Regelabweichung, sondern von dem Istwert berechnen und diesen, dann vom Sollwert abziehen. Es hat sich gezeigt, daß diese Art von D-Anteil gutmütiger auf sprunghafte Sollwertänderungen wirkt.

Verkettete Regelung

Oftmals besteht ein Regelkreis nicht nur aus einem Regler, sondern aus mehreren unterlagerten Regelkreisen. Dies führt zu einer sehr schnellen und stabilen Regelung.

Als Beispiel läßt sich eine Regelung nennen, die einen Motor regelt, der z.B. eine Achse einer Maschine antreibt. Der gesamte Antrieb kann aus einem digitalen Lageregler, einem analogen Geschwindigkeits- und Stromregler, Motor und Mechanik bestehen. Der digitale Kreis liefert dann den aus der Lagereglung errechneten Sollwert für den Geschwindigkeitskreis.

Wenn der digitale Rechner es von der Rechenkapazität zulässt, können natürlich auch die analogen Kreise ersetzt werden, so daß vor dem Motor nur noch ein Leistungsstellglied geschaltet wird. In näherer Zukunft wird sich die voll digitale Regelung durchsetzen.

Unterlagerte Geschwindigkeitsregelung

Als unterlagerte Geschwindigkeitsreglung wird in den meisten Fällen ein PI- oder P-Regler eingesetzt. Der Regler erhält den Soll- unf Istwert der Geschwindigkeit und gibt als Ausgangswert eine der Beschleunigung entsprechnede Größe aus.

Stromregelung

Auch die Stromreglung eines Motors wird als P oder PI-Regler ausgelegt. Die Kraft bzw. das Moment eines Motors ist proportional zum Strom. D.h. die Geschwindigkeit eines Motors wird solange größer, bis die Haftreibung und Geschwindigkeitsreibung so groß sind, wie die durch den Strom erzeugt Kraft.

Digitalregelung

Umsetzung vom kontinuierlichen ins diskrete

Der Computer kann die Berechnung der Ausgangsgröße nicht zu jeden beliebigen Zeitpunkt vornehmen, da er nicht immer genug Rechenkapazität zu Verfügung hat. Deshalb berechnet man die Regelgröße nur zu bestimmten und der Einfachheit wegen zeitgleichen Abständen. Für den Rechner ist die Zeit also keine kontinuierliche Größe, sondern sie wird nur auf bestimmte Punkte, die alle den gleichen Abstand voneinander haben, reduziert. Der Abstand der Punkte wird als Abtastzeit T bezeichnet.

Um die oben aufgeführten Gleichungen (1.2 bis 1.8) im Computer umsetzten zu können, werden folgende Definitionen eingeführt, die eine Überführung vom analogen zum diskreten erlauben.

Definition 1: Eine Funktion $f_n$ sei die kurze Schreibweise für $f(t=n \cdot T)$ im Zeitbereich. Für $n<0$ sei der Wert der Funktion Null.
Definition 2: Der Operator $\Delta f_{n}$ sei definiert zu: $\Delta f_{n} = f_{n} - f_{n-1}$ und $\Delta^n f_{n} =\Delta^{n-1} f_{n} - \Delta^{n-1} f_{n-1}$.

$f_n$ stellt z.B. einen Meßwert oder Ausgangswert zum Zeitpunkt $n \cdot t$ dar. Der $\Delta$-Operator kann Berechnungen von Integralen und Ableitungen vereinfachen.

Numerische Berechnung eines Integrales

Eine Möglichkeit das Integral anzunähern ist es, die Kurve in viele kleine Rechtecke mit der Abtastzeit T zu unterteilen und die Fläche aufzusummieren. Man erhält so:
$\displaystyle xa (t=n \cdot T) = \frac{1}{T_i} \int_0^{n \cdot T} xe \cdot d\tau$ $\textstyle {T \atop \models}$ $\displaystyle xa_{n} = \frac {T}{T_i} \sum_{l=0}^{n-1} xe_{l}$ (2.1)

Im Abtastbereich kann die Summe sehr einfach rekursiv berechnet werden. Bildet man die Differenz auf zwei aufeinanderfolgenden Werten, so folgt: $\Delta xa_{n} = xa_{n} - xa_{n-1} = \frac {T}{T_i} xe_{l-1}$ dies umgeformt ergibt:
\begin{displaymath}
xa_{n} = \frac {T}{T_i} xe_{l-1} + xa_{n-1}
\end{displaymath} (2.2)

Eine genauere Anäherung des Integrals erhält man durch die Trapezformel. Zwei benachtbarte Punkte werden durch eine Gerade verbunden. Daraus folgt:

$\displaystyle xa (t=n \cdot T) = \frac{1}{T_i} \int_0^{n \cdot T} xe \cdot d\tau$ $\textstyle {T \atop \models}$ $\displaystyle xa_{n} = \frac {T}{2 T_i} \sum_{l=0}^{n} xe_{l} + xe_{l-1}$ (2.3)

Die rekursive Form im Abtastbereich lautet dann: $\Delta xa_{n} = xa_{n} - xa_{n-1} = \frac {T}{2 T_i} (xe_{l}+xe_{l-1})$ und $\Delta^2 xa_{n} = \frac {T}{2 T_i} (xe_{l}-xe_{l-2})$


\begin{displaymath}
xa_{n} = \frac {T}{2 T_i} (xe_{l}+xe_{l-1}) + xa_{n-1}
\end{displaymath} (2.4)

Es handelt sich in beiden Fällen um einen Filter 1. Ordnung.

Numerische Berechnung eines Doppelintegrales

Durch Anwendung der Trapezformel erhält man:


\begin{displaymath}
xa (t) = \int_0^{n \cdot T} \int xe dt^2 {T \atop \models}
\end{displaymath} (2.5)


\begin{displaymath}
xa_{n} = \frac {T}{2} \sum_{l=0}^{n}
(T \sum_{i=0}^{l} \f...
..._{i-1}}{2} + T \sum_{i=0}^{l-1} \frac {xa_{i} + xa_{i-1}}{2})
\end{displaymath} (2.6)

Die rekursive Form im Abtastbereich lautet dann:


\begin{displaymath}
\Delta xa_{n} = \frac {T}{2}
(T \sum_{i=0}^{n} \frac {xa_...
..._{i-1}}{2} + T \sum_{i=0}^{n-1} \frac {xa_{i} + xa_{i-1}}{2})
\end{displaymath} (2.7)


$\displaystyle \Delta^2 xa_{n} = \frac {T}{2 T_i} (xe_{l}-xe_{l-2})$     (2.8)
$\displaystyle xa_n = \frac {T}{2 T_i} (xe_{l}-xe_{l-2}) + 2 xa_{n-1} - xa_{n-2}$     (2.9)

Numerische Berechnung der Ableitung

Die Ableitung kann angenähert werden als Differenzenquotient, d.h. der Steigung der Geraden durch die Punkte Xe(nT) und xe((n+1)T), also durch:
$\displaystyle xa (t=n \cdot T) = T_d \cdot \frac{d xe}{dt}$ $\textstyle {T \atop \models}$ $\displaystyle xa_n = \frac {T_d}{T} \Delta xe_{n}$ (2.10)
$\displaystyle xa_n$ $\textstyle =$ $\displaystyle \frac {T_d}{T} \cdot (xe_n - xe_{n-1})$ (2.11)

Numerische Regelgleichungen

Nimmt man Beziehung (1.8) und wendet man darauf die Abtastung und den Operator $\Delta$ an, so folgt:


$\displaystyle \Delta xa_n + \frac{T_1}{T} \Delta ^2 xa_n =
k_p \cdot (\Delta ...
...})+
\frac{T}{2 T_i} (Xe_n + Xe_{n-1}) +
\frac{T_d + T_1}{T} \Delta ^2 Xe_n)$     (2.12)

Dies umgeformt und sortiert ergibt:
$\displaystyle k_{a0} xa_n$ $\textstyle =$ $\displaystyle k_{e0} Xe_n + k_{e1} Xe_{n-1} + k_{e2} Xe_{n-2} +$  
    $\displaystyle k_{a1} Xa_{n-1} + k_{a2} Xa_{n-2}$ (2.13)

mit den Faktoren:
\begin{displaymath}
\begin{array}[t]{lll}
k_{a0} &=& 1 + \frac{T_1}{T} \\
k_...
...T_1/T \nonumber \\
k_{a2} &=& -T_1/T \nonumber
\end{array}
\end{displaymath}  

Die obige Gleichung berücksichtigt noch nicht, daß der analoge Ausgang des Reglers immer begrenzt ist. Ein D/A-Wandler ist durch seine maximale Ausgangsspannung begrenzt. Überschreitet die Größe $xa_n$ einen Schwellwert $xa_{max}$, so wird der Reglerausgang auf $xa_{max}$ begrenzt.

In der folgenden Tabelle sind verschiedene Reglertypen und die dazugehörigen Faktoren aufgeführt.

  P I D-T1 PI PD-T1 PID-T1
             
$k_{e0}$ $k_p$ $\frac{T}{2T_i}$ $\frac{2 T_d}{T+2T_1}$ $k_p (1 + \frac{T}{2T_i})$ $k_p (1 + \frac{T_d+T_1}{T})$ $k_p (1 + \frac{T+2T_1}{2T_i} + \frac{T_d+T_1}{T})$
$k_{e1}$ $0$ $\frac{T}{2T_i}$ $-k_{e0}$ $k_p ( \frac{T}{2T_i} - 1)$ $-k_p \frac{T_d+T_1}{T}$ $k_p ( \frac{T-2T_1}{2T_i} - 1 - 2 \frac{T_d+T_1}{T})$
$k_{e2}$ $0$ $0$ $0$ $0$ $0$ $k_p \frac{T_d + T_1 }{T}$
$k_{a0}$ $1$ $1$ $1$ $1$ $1 + \frac{T_1}{T}$ $1 + \frac{T_1}{T}$
$k_{a1}$ $0$ $1$ $\frac{2T_1-T}{T+2T_1}$ $1$ $\frac{T_1}{T}$ $1+2T_1/T$
$k_{a2}$ $0$ $0$ $0$ $0$ $0$ $-T_1/T$

  PID-T1 (Hütte)  
     
$k_{e0}$ $k_p (1 + \frac{T+T_1}{2T_i} + \frac{T_d+T_1}{T})$ $k_p (0.5 + \frac{2 T_1 + T}{4T_i} + \frac{T_d + T_1}{T})$
$k_{e1}$ $k_p ( \frac{T}{2T_i} - 1 - 2 \frac{T_d+T_1}{T})$ $k_p (\frac{T}{2T_i} - 2 \frac{T_d+T_1}{T})$
$k_{e2}$ $k_p (\frac{T_d + T_1 }{T} - \frac{T_1}{2T_i})$ $k_p (\frac{T_d+T_1}{T} + \frac{-2 T_1 + T}{4T_i} - 0.5)$
$k_{a0}$ $1 + \frac{T_1}{T}$ $(\frac{T_1}{T} + 0.5)$
$k_{a1}$ $1-k_{a2}$ $2 \frac{T_1}{T}$
$k_{a2}$ $-T_1/T$ $0.5 - \frac{T_1}{T}$

Man kann einen PIDT1-Regler auch aus der Summe eines DT1- plus I- und P-Reglers aufbauen. Dies hat den Vorteil, daß man für Sonderfälle mehrer Eingriffsmöglichkeiten hat. So kann man anstelle der Regelabweichung nur deren Richtung integrieren. Oder der Integrator wird nur eingeschaltet, wenn keine Bewegung stattfindet usw. Der Nachteil der Parallelschaltung von P, I und DT1-Glied ist ein größerer Speicherplatzbedarf und eine etwas langsamere Beechnung. Tiefergehende Informationen zur Reglungstechnik sind zu finden in /2/ und /3/.

Filter

Eine Gleichung der Form 2.13 beschreibt einen Filter. Die allgemeine Form lautet:
\begin{displaymath}
ka_0 \cdot xa_n + ka_1 \cdot xa_{n-1} \cdots ka_n \cdot xa_...
...
ke_0 \cdot xe_n + ke_1 \cdot xe_{n-1} \cdots ke_n \cdot xe_0
\end{displaymath} (2.14)

Sie nennt man auch Differenzengleichung n. Ordnung, da der Ausgangswert aus einer Folge von n Abtastzeitpunkten von Ein- und Ausgangswerten berechnet wird.

Oftmals wird dieser Zusammenhang auch als IRF-Filter n ter Ordnung bezeichnet, da der Ausgangswert sich auch bei einem konstanten Einganswert $\ne 0$ bis im unendlichen ändert.

Werden für den aktuellen Ausgangswert nur Eingangswerte benötigt, also $ka_i = 0$ und $ka_0 \ne 0$, dann spricht man von FIR-Filtern, da der Ausgangswert nach max. n Abtastzyklen konstant wird.

Die Geschwindigkeitsvorsteuerung

Besitzt der digitale Lageregelkreis einen unterlagerten Geschwindigkeitsregelkreis, so kann eine sogenannte Vorsteuerung die Regelgenauigkeit verbessern. Bei der Vorsteuerung wird dem Ausgangswert des digitalen Regelkreises der Sollwert für die Geschwindigkeit aufaddiert. Der Lageregelkreis muß dann sozusagen nur noch Störungen ausregeln, da die Vorsteuerung die Sollbewegung schon vorwegnimmt. Als Blockschaltbild wird dies folgendermaßen dargestellt:

Die Beschleunigungsvorsteuerung

Ebenso wie bei der Geschwindigkeitsvorrsteuerung kann in der Beschleunigungsphase eines Antriebes eine Beschleunigungsvorsteuerung sinnvoll sein. Diese bewirkt, daß die Regelabweichungen während einer Beschleunigung klein gehalten wird.

Umsetzung in C++-Klassen

Die oben dargestellten diskreten Gleichungen werden nun in Klassen gefasst. D.h. es folgen Klassen zum integrieren und ableiten, eine Klasse für den DT1-Algorithmus, einen Filter 1. Ordnung und einen allgemeinen Filter n ter Ordnung.

Obwohl man mit einem allgemeinen Filter alle anderen Klassen auch realisiern kann, wurden die zusätzlichen Klassen hinzugenommen, da sie weniger Zeit bei der Ausführung benötigen. Dies hat den Grund in der vollständigen Implementierung mit inline-Funktionen.

Die Definitionen und die Implementation der Hilfsklassen sieht folgendermaßen aus: //

#ifndef DSP_H
#define DSP_H 1


//template<class T> T Sgn (T direction, T val) { return direction < 0. ? -val : val; }

#define FILTER_T double

//---- quick function without overhead -----------------------------------------
class INTEGRATE_C //---- integrator as a sum of rectangles
{
//---- z-function: ke*t0 / (1 - z^-1)
public:
        INTEGRATE_C (void) { Reset (); }
  void  Reset       (void) { ke = xa0 = 0.; }
  void  Clear       (void) { xa0 = 0.; }

  void  Set         (FILTER_T ki, FILTER_T t0)  { ke   = ki*t0; }
  void  Run         (const FILTER_T& xe0)       { xa0 += ke*xe0; }

  const FILTER_T& CXa (void) const { return xa0; }
  const FILTER_T& CKe (void) const { return ke; }

protected:
  FILTER_T ke, xa0;
};

class INTEGRATE_T_C //---- integrator as a sum of trapezoids
{
//---- z-function: ke*t0* (1 + z^-1) / (1 - z^-1)
public:
				INTEGRATE_T_C (void) { Reset (); }
	void  Reset       	(void) { ke = xa0 = xe1 = 0.; }
	void  Clear       	(void) { xa0 = xe1 = 0.; }

	void  Set         	(FILTER_T ki, FILTER_T t0)  { ke   = ki*t0; }
	void  Run         	(const FILTER_T& xe0)       { xa0 += ke*(xe0 + xe1); xe1 = xe0;}

  const FILTER_T& CXa (void) const { return xa0; }
  const FILTER_T& CKe (void) const { return ke; }

protected:
  FILTER_T ke, xe1, xa0;
};


//-------------------------------------------------------------------------
class DERIVE_C : public INTEGRATE_T_C
{
//---- kd/t0 * (1 - z^-1)
public:
				DERIVE_C  (void) : INTEGRATE_T_C () { ; }

  void  Set       (FILTER_T kd, FILTER_T t0)  { ke  = kd/t0; }
  void  Run       (const FILTER_T& xe0)       { xa0 = ke * (xe0 - xe1); xe1 = xe0; }
};


//-------------------------------------------------------------------------
class DT1_C : public DERIVE_C
{
//---- kd/t0 * (1-z^-1)/(1+t1/t0 - t1/t0*z^-1) = kd * (1-z^-1)/(t0+t1-t1*z^-1)
public:
        DT1_C    (void) : DERIVE_C () , ka (0.) { ; }

  void  Reset     (void) { ka = 0.; DERIVE_C::Reset (); }

  void  Set       (FILTER_T kd, FILTER_T t1, FILTER_T t0)  { DERIVE_C::Set (kd, t0+t1); ka = t1/(t1+t0); }
  void  Run       (const FILTER_T& xe0)       { xa0 = ke * (xe0 - xe1) + ka*xa0; xe1 = xe0; }

private:
  FILTER_T ka;
};

//-------------------------------------------------------------------------
/*
    Implements all previous described classes, but with more overhead
*/
class FILTER_O1_C
{
//---- (ke0 + ke1* z^-1) / (ka0 + ka1*z^-1)
public:
  FILTER_O1_C (void) { Reset (); }

  void  Reset     (void) { ke0 = ke1 = ka1 = 0.; Clear   ();}
	void  Clear     (void) { xe1 = xa0 = 0.; }

  //---- Trapez
  void SetI   (FILTER_T ti, FILTER_T t0) { ka1 = 1.; ke1 = ke0 = ti ? 0.5*t0/ti : 0.; }
  //---- Rechteck
  void SetI1  (FILTER_T ti, FILTER_T t0) { ka1 = 1.; ke1 = 0.; ke0 = ti ? t0/ti : 0.; }
  void SetD   (FILTER_T td, FILTER_T t0) { ka1 = 0.; ke1 = ke0 = td/t0; }
  void SetDT1 (FILTER_T td, FILTER_T t1, FILTER_T t0) {ke0 = td/(t0+t1); ke1 = -ke0; ka1 = t1/(t0+t1);}
  void SetPI  (FILTER_T kp, FILTER_T ti, FILTER_T t0)
  {
    if (ti) {
      ke0 = kp*(0.5*t0/ti + 1.);
      ke1 = kp*(0.5*t0/ti - 1.);
      ka1 = 1.;
    }
    else {
      ke0 = ke1 = ka1 = 0.;
    }
  }
  void  SetZFct   (FILTER_T kz0, FILTER_T kz1, FILTER_T kn0, FILTER_T kn1)
  {
    if (kn0) {
      ke0 =  kz0 / kn0;
      ke1 =  kz1 / kn0;
      ka1 = -kn1 / kn0;
    }
    else
      ke0 = ke1 = ka1 = 0.;
  }

	void  Run   (FILTER_T xe0) {xa0 = ke0*xe0 + ke1*xe1 + ka1*xa0; xe1 = xe0;}

  const FILTER_T& CXa (void) const {return xa0;}

private:
  FILTER_T ke0, ke1, ka1, xe1, xa0;
};



#define WITH_FILTER 0
#if WITH_FILTER
//---- general filter class of n th order ---------------------------------
class FILTER_C
{
public:
  FILTER_C  (int order_) : fiosp (0), order (0) { Init (order_); }
  ~FILTER_C ()    { if (fiosp) delete [] fiosp; }

        void      Init      (int order);
        void      Reset     (void);
        void      Clear     (void);
        void      Set       (const FILTER_T *kiko);
        void      Run       (const FILTER_T& input);

  const FILTER_T& CXa       (void) const { return fiosp [0].xa; }

protected:
  void LimitTo (FILTER_T val) { Xe (0, CXe (0) + (SGN (CXa (0), val) - CXa (0))/CKe (0)); }

private:
  const FILTER_T& CXe (int el) const { return fiosp [el].xe; }
  const FILTER_T& CXa (int el) const { return fiosp [el].xa; }
  const FILTER_T& CKe (int el) const { return fiosp [el].ke; }
  const FILTER_T& CKa (int el) const { return fiosp [el].ka; }
    
  void Xe   (int el, FILTER_T val) { fiosp [el].xe = val; }
  void Xa   (int el, FILTER_T val) { fiosp [el].xa = val; }
  void Ke   (int el, FILTER_T val) { fiosp [el].ke = val; }
  void Ka   (int el, FILTER_T val) { fiosp [el].ka = val; }

  int order;
  struct FILTER_IO_S {
    FILTER_T ke, ka, xe, xa;
  } *fiosp;
};
#endif


#endif //---- DSP_H
//

Die Datei dsp.cpp implementiert nur noch die Methoden des allgemeinen Filters.

//

//---- file dsp.cpp

#include "types.h"
#include "dsp.h"

void FillMem (INT8 *dst, INT8 val, int len)
{
  while (len--)                       *dst++ = val;
}

#if WITH_FILTER
//---- FILTER_C -----------------------------------------------------------
void FILTER_C::Init (int order_)
{
  if (fiosp && order != ++order_) {   
    order = order_;
    delete [] fiosp;
    fiosp = new FILTER_IO_S [order];
    if (!fiosp) {
      return; //exit (-1);
    }
 }
  Reset ();
}

void FILTER_C::Clear (void)
{
  for (int i=0; i < order; i++) {
    Xe (i, 0.);
    Xa (i, 0.);
  }
}

void FILTER_C::Reset (void)
{
  for (int i=0; i < order; i++) {
    Ke (i, 0.);
    Ka (i, 0.);
    Xe (i, 0.);
    Xa (i, 0.);
  }
}

//---- Koeffizienten der 1/z-Übertragungsfunktion ke0 ... ken ka0 ... kan
void FILTER_C::Set (const FILTER_T *kiko)
{
  /*---- alle Faktoren mit 1/ka0 multiplizieren, d.h. aktuellen Ausgangswert
         auf eins normieren */
  FILTER_T  tmp = *(kiko+order) ? 1./ *(kiko+order) : 1.;
  int       i = order;

  while (--i >= 0) {
    Ke (i, *kiko * tmp);
    Ka (i, -*(kiko++ + order) * tmp);
  }
}

void FILTER_C::Run (const FILTER_T& input)
{
  FILTER_T    tmp = 0.;
  FILTER_IO_S *p  = &fiosp [order];

  while (--p > fiosp) {
    tmp += p->ke * (p->xe  = (p-1)->xe) + p->ka * (p->xa = (p-1)->xa);
  }
  p->xa = (p->xe = input)*p->ke + tmp;
}

#endif
//

Nun kommen wir zur eigentlichen Reglerklasse. Als erstes beschränken wir uns auf die Definition der Klasse. Welche Funktionen sind dabei sinnvoll für die Implementation? Folgende Eigenschaften soll der Regler besitzen:

In der Programmiersprache C++ werden wir nun eine Klasse bilden, die einen bzw. mehrere der oben beschriebene Regelkreise realisiert.

Folgende Funktionen sind public implementiert:

void Reset (void):
Die Initialisierungsfunktion muß vor dem erstmaligen Start des Reglers aufgerufen werden. Diese Funktion setzt den Regler in den Urzustand.
void Clear (void):
Diese Funktion setzt den inneren Zustand des Reglers zu Null. Dies wird immer dann ausgeführt wenn der Regler gestartet wird.
long SetPara (int type, int nel, FLOAT64 *dp)
Diese Methode parametriert die verschiedenen Regelkreise in Abhängigkeit des Parameters type. CTRL_SP_MISC, CTRL_SP_Z, CTRL_SP_SPID, CTRL_SP_VPID, CTRL_SP_SW, CTRL_SP_NOTCH, CTRL_SP_POS_WIN CTRL_SP_CHECK
void Start (void):
Funktion zum Aktivschalten des Reglers.
void Stop (void):
Funktion zum Ausschalten des Reglers.
BOOL IsActive (void):
Zeigt an, ob der Regler aktiv ist.
BOOL IsSActive (void):
Zeigt an, ob der Lageregler aktiv ist.
BOOL IsVActive (void):
Zeigt an, ob der Geschwindigkeitsregler aktiv ist.
BOOL IsAActive (void):
Zeigt an, ob der Beschleunigungsregler aktiv ist. Liefert immer FALSE zurück, da dieser Regler nicht implementiert ist.
const double& CXa (void):
Aktuellen Reglerausgangswert inkl. Vorsteuerung ausgeben.
const double& CSXe (void):
Aktuelle Regeldifferenz ausgeben des Lagereglers ausgeben.
const double& CVXe (void):
Aktuelle Regeldifferenz des Geschwindigkeitsreglers ausgeben.
void Run (const CTRL_T& soll, const CTRL_T& ist):
Führt die Berechnung des Regelalgorithmus aus. Diese Funktion muß in jedem Taktzyklus aufgerufen werden. Aus den Lagesollwerten werden die Geschwindigkeits- und Beschleunigungssollwerte, aus den Istweren die Geschwindigkeitsistwerte berechnet.
BOOL Is$\cdots$ (void)
Die Funktionen mit dem Präfix Is liefern den Status der Regelkreise zurück, d.h. ob sie aktiviert sind oder nicht.
BOOL Sw $\cdots$ (int flag)
Die Funktionen mit dem Präfix Sw schalten die verschiedenen Regelkreise ein (flag $==$ 1) oder aus (flag $== 0$).

Die Definition der Klasse sieht nun folgendermaßen aus: //

#ifndef PID_H
#define PID_H 1

#include "dsp.h"


#define CTRL_T double

//---- different parameters for configuration
enum CTRL_SPARA_E {
		CTRL_SP_MISC, CTRL_SP_Z, CTRL_SP_SPID, CTRL_SP_VPID,
    CTRL_SP_SW, CTRL_SP_NOTCH, CTRL_SP_POS_WIN = 10, CTRL_SP_CHECK
};

//---- different values for position status
enum { POS_ERROR=-1, POS_NOT_OK, POS_TRIGGERED, POS_OK };

#if 1

class BL_CTRL_C
{
public:
        BL_CTRL_C (void) { Reset (); }
	void  Reset     (void);
  void  Clear     (void);
  long  SetPara   (int type, int nel, FLOAT64 *dp);

	void  Start     (const CTRL_T& soll);
	void  Run       (const CTRL_T& soll, const CTRL_T& ist);
  void  Stop      (void);

  void  SwSLoop   (BOOL val)    { sw.s_loop = val ? ON : OFF; }
  void  SwVLoop   (BOOL val)    { sw.v_loop = val ? ON : OFF; }
  void  SwALoop   (BOOL val)    { ; /* reserved */ }

  BOOL  IsActive  (void) const  { return sw.active == ON; }
  BOOL  IsSLoop   (void) const  { return sw.s_loop == ON; }
  BOOL  IsVLoop   (void) const  { return sw.v_loop == ON; }
  BOOL  IsALoop   (void) const  { return ON == OFF; }
  int     PosOk   (void) const  { return pos_ok; }  //---- OFF, TRIGGERD, ON

  const CTRL_T& CXa   (void) const { return xa;}           //---- current output
  const CTRL_T& CSXe  (void) const { return sl.xe;}        //---- current S input
	const CTRL_T& CVXe  (void) const { return vl.xe;}        //---- current V input

  const CTRL_T& CRefV (void) const { return ref_v.CXa ();} //---- reference V
  const CTRL_T& CRefA (void) const { return ref_a.CXa ();} //---- reference A
  const CTRL_T& CCurV (void) const { return cur_v.CXa ();} //---- current   V
  const CTRL_T& CCurA (void) const { return cur_a.CXa ();} //---- current   V


private:
  BOOL  IsMoving (void) { return ref_v.CXa () != 0.; }
  void  Check   (void);     //---- check if drive is ok

  CTRL_T xa,                //---- current output
         xa_max;            //---- maximal output value

  CTRL_T v_max,
         pos_window,
         chk_s_max,
         chk_v_max;

  //---- different switches, active when ON
  struct {
    BOOL active,            //---- controller is active when ON
         s_loop,            //---- closed s loop
         v_loop,            //---- closed v loop
         notch,             //---- activate notch filter
         ic_online,         //---- continuous integrator
         ic_sgn,            //---- integrate Sgn (e)
         ic_clear,          //---- clear integrator (internal)
         check;             //---- check motion difference
  } sw;

  int         pos_ok;       //---- POS_ERROR, POS_OK, POS_TRIGGERED, POS_NOT_OK

  CTRL_T      kh [3];       //---- help coefficients feed forward for offset, v, a
  struct {                  //---- variables for closed S loop
    CTRL_T      xe;         //---- current input value
    CTRL_T      kp;         //---- proportional gain
    INTEGRATE_C ic;         //---- I part
    FILTER_O1_C dt1;        //---- DT1 part
	} sl;

  struct {                  //---- variables for closed V loop
    CTRL_T      xe;         //---- current input value
    FILTER_O1_C pi;        //---- PI part
  } vl;

#if WITH_FILTER              
  FILTER_C    notch (2);    //---- notch filter of second order
#endif  

  //---- current and reference variables used for feed forward
  DERIVE_C    ref_v,        //---- calc v reference from way
              ref_a,        //---- calc a reference from ref_v
              cur_a,        //---- calc current a   from current cur_v
              cur_v;        //---- calc current v   from current way
};
#endif

#endif
//

Ein möglicher Programmablauf könnte wie im folgenden aufgeführt aussehen:

Regler initialisieren
Regler parametrieren
Regler starten
...
solange aktiv
  warte bis Taktzyklus dran
  Istwerte einlesen
  Sollwerte berechnen
  Regelberechnung ausführen
  berechnete Werte der Regelung ausgeben.
  Test auf Abbruch
Regler stoppen

Mit der definierten Klasse folgt für diese Reglerprogramm:

#include "pid.h"

PID_C  pid [1];               //---- nur einen PID-Regler
double dp [5];                //---- Hilfsvariablen
int    el = 0;

pid [el].Reset ();            //---- Initialisierung (nur vor dem ersten Aufruf)

dp [0] = 10.;                 //---- Kp
dp [1] = 1./.026; 						//---- Ki=1/Ti
dp [2] = .010;                //---- Td
dp [3] = .004;                //---- T1
pid [el].SetPara (CTRL_SP_SPID, 4, dp);  //---- kp, ki, td, t1 setzen
dp [0] = 10;									//---- Xamax
dp [1] = 0.;									//---- S-Vorsteuerung
dp [2] = 0.;									//---- V-Vorsteuerung
dp [3] = 0.;									//---- A-Vorsteuerung
dp [4] = 0.;									//---- Vmax für Reglerüberwachung
pid [el].SetPara (CTRL_SP_MISC, 5, dp);

soll = berechneter_positionswert
pid [el].Start (soll);        //---- Regler einschalten
...
while (pid [el].IsActive ()) {
  WarteAufNeuenZyklus ();
  LeseIstWerte   (&ist);      //---- Istwerte einlesen, z.B. den Weg
  GeneriereSollWerte (&soll);
  pid [el].Run   (soll, ist); //---- Berechnung der Ausgangsgröße, wird zu
  SetzeAusgänge  ();          //---- jeder Abtastzeit T aufgerufen
  if (Abbruch ()) {
    pid [el].Stop ();
  }
}

Dieses Programm initialisiert den Regler, setzt die Regelparameter, startet den Regler und führt dann solange eine Regelschleife aus, bis der Regler in der durch die Abbruch-Funktion inaktiv geschaltet wird.

Der zugehörige Quelltext der Klasse für den PID-Regler lautet wie folgt:

//

//---- Datei pid.cpp
#include <math.h>
#include "types.h"
#include "ctrl.dcl"
#include "base.h"
#include "dsp.h"
#include "pid.h"
#include "com.h"

extern void BigError (long error);

#if 1
//---- Block controller ---------------------------------------------------
void BL_CTRL_C::Reset (void)
{
  Clear ();
  xa_max = 1.;
  pos_window = .01;
	kh [0] = kh [1] = kh [2] = 0.;
  pos_ok    = POS_OK;
  sw.check  = OFF;            //!!!! for test, normaly ON
  sw.active = sw.v_loop = sw.notch = sw.ic_online =
  sw.ic_sgn = sw.ic_clear = OFF;
  sw.s_loop = ON;
  cur_v.Clear ();
  cur_a.Clear ();

  cur_v.Set (1., T0);
  cur_a.Set (1., T0);
  ref_v.Set (1., T0);
  ref_a.Set (1., T0);
}

void BL_CTRL_C::Clear (void)
{
  sw.active = OFF;
	xa = 0.;

  ref_v.Clear ();
  ref_a.Clear ();
  sl.dt1.Clear ();
  sw.ic_clear = ON;
}

//---- MPARA_KH, MPARA_PID ,[MPARA_SW]
long BL_CTRL_C::SetPara (int type, int nel, CTRL_T *argp)
{
  switch (type)
  {
  case CTRL_SP_CHECK:
    chk_s_max = *argp++;
    chk_v_max = *argp++;
  case CTRL_SP_MISC:
		xa_max = *argp++;
    kh [0] = *argp++;
    kh [1] = *argp++;
    kh [2] = *argp++;
    v_max  = *argp++;
    break;
	case CTRL_SP_SPID:  //---- Kp, Ki, Kd, T1
    sl.kp = argp [0];
    sl.ic.Set (argp [1], T0);
    sl.dt1.SetDT1 (argp [2], argp [3], T0);
    break;
	case CTRL_SP_VPID:  //---- Kp, Ki, Kd, T1
    vl.pi.SetPI (argp [0], argp [1], T0);
    break;
  case CTRL_SP_SW:
    sw.ic_online  = *argp++ ? ON : OFF;
    sw.ic_sgn     = *argp++ ? ON : OFF;
    sw.notch      = *argp++ ? ON : OFF;;
    break;
  case CTRL_SP_POS_WIN:
    pos_window = Abs (*argp);
    break;
  case CTRL_SP_NOTCH:
#if WITH_FILTER
    notch.Set (argp);
#endif    
    break;
  }
  return E_OK;
}
 

void BL_CTRL_C::Start (const CTRL_T& soll)
{
  Clear ();
  ref_v.Run (soll);          //---- calculate v & a reference
	ref_a.Run (ref_v.CXa ());
  sw.active = ON;
}

void BL_CTRL_C::Stop (void)
{
  sw.active = OFF;
	xa = 0.;
}

/*-----------------------------------------------------------------------------
  description       : checks if controller difference is in allowed window
                      if motion difference is greater 1., use 16 times
                      higer ti for 0.5 seconds
  parameters        : ---
  return values     : ---
  errors            : ---
  side effects      : ---
-----------------------------------------------------------------------------*/
void BL_CTRL_C::Check (void)
{
  static long sw_flag = 0;

  if (IsSLoop ()) {
    if (sw.check &&
			 ( (Abs (cur_v.CXa ()) >= chk_v_max) || (ABS(CSXe ()) > chk_s_max) )) {
			pos_ok = POS_ERROR;
//			BigError (E_DRIVE_NOT_OK - el*0x100);
			Stop ();
			return;
    }

    //----- switch to greater ti
		if (ABS(CSXe ()) > 1.) {
      if (sw_flag == 0 && sl.ic.CKe ()) {
				sw_flag = .5/T0;
        sl.ic.Set (sl.ic.CKe ()/T0*16, T0);
			}
		}
		else if (sw_flag != 0) {
			if (--sw_flag == 0) {
        sl.ic.Set (sl.ic.CKe ()/T0/16, T0);
			}
		}

    if (IsMoving ()) {
      pos_ok = POS_NOT_OK;
    }
		else if (PosOk () < POS_OK && Abs (CSXe ()) < pos_window &&
          cur_v.CXa () == 0. && cur_a.CXa () == 0. ) {
      pos_ok++;
    }
  }
  else {
    if ( sw.check &&
       ( (Abs (cur_v.CXa ()) >= chk_v_max) ||
         (Abs (CXa ()) == xa_max &&
          Abs (cur_v.CXa ()) == 0. && cur_a.CXa () == 0.) ) ) {
			pos_ok = POS_ERROR;
//			BigError (E_DRIVE_NOT_OK - el*0x100);
			Stop ();
			return;
		}
		if (IsMoving ()) {
			pos_ok = POS_NOT_OK;
    }
    else if (PosOk () < POS_OK) {
      pos_ok++;
    }
	}
}

/*
    PIDT1-Regler mit
    Geschwindigkeits- und Beschleunigungsvorsteuerung,
    Offsetaufschaltung abhängig von Sgn (soll - ist) zusätzlich
    Filter 2. Ordnung

    Integrator immer aktiv oder nur bei v_soll == 0
               Integration über Regelabweichung oder Sgn (Regelabweichung)
    Filter abschaltbar
    DT1-Anteil im Istwertkreis (vor Vergleicher)

*/
void BL_CTRL_C::Run (const CTRL_T& soll, const CTRL_T& ist)
{
  cur_v.Run (ist);                    //---- cur v,a is always calculated
  cur_a.Run (cur_v.CXa ());
  if (!IsActive ())                   return;

  ref_v.Run (soll);                   //---- calculate v & a reference
	ref_a.Run (ref_v.CXa ());

  Check ();                           //---- check axis errors

  if (!IsSLoop ()) {
    xa = 0.;                          //---- open S loop
  }
  else {
    sl.xe = soll - ist;

    sl.dt1.Run (ist);
    xa = sl.xe        -    //---- P
         sl.dt1.CXa ();    //---- DT1

    //---- use integrator if ic_online is active or move is inactive
    if (!IsMoving () && sw.ic_online == OFF) {
      sw.ic_clear = ON;
    }
    else {
      if (sw.ic_clear == ON) {
        sl.ic.Clear ();
        sw.ic_clear = OFF;
      }
      //---- integrate motion difference xe or only direction of xe
			sl.ic.Run (sw.ic_sgn ? Sgn (CSXe ()) : CSXe ());
      xa += sl.ic.CXa ();
    }

    xa *= sl.kp;
  }

  xa += kh [1] * ref_v.CXa ();        //---- V feed forward

  if (IsVLoop ()) {
    vl.xe = xa - cur_v.CXa ();
		vl.pi.Run (CVXe ());
    xa = vl.pi.CXa ();
  }
  if (IsSLoop ())
		xa += kh [0] * Sgn (CSXe ());     //---- friction
  xa += kh [2] * ref_a.CXa ();        //---- A feed forward

#if WITH_FILTER
  if (sw.notch == ON) {
		notch.Run (xa);
		xa = notch.CXa ();
  }
#endif

  if (ABS (xa) > xa_max) {            //---- delimiter
		xa = SGN (xa, xa_max);
  }
}
//-------------------------------------------------------------------------
#endif

//---- Datei pid.cpp
//

Diese Klasse ist nicht geschwindigkeitsoptimiert, da dies die Lesbarkeit verschlechtert hätte. Die Optimierung wäre dabei in der Methode Run vorzunehmen, z.B. durch Verwendung von Funktionspointern anstelle der if-Abfragen für die verschiedenen Optionen. Die heutigen Rechner sind aber so leistungsfähig, daß man diese zusätzlichen Abfragen in Kauf nehmen kann.

Mit der implementierten Reglerklasse ist es möglich, Achsen zu regeln, bei der die Schnittstelle zum Antrieb entweder einen analogen Geschwindigkeitsregler oder einen Momentenregler anbietet. Dabei muß noch die Lageistwert-erfassung z.B. für Zählerkarten und die Sollwertausgabe z.B. für D/A-Wandler implementiert werden.

Von der Lage zur Bewegung

Ein Lageregler regelt die Position und führt im allgemeinen nicht zu einer definierten Bewegung. Um eine Bewegung zu erzeugen, die nach einem gewünschtem Geschwindigkeits und Beschleunigungsprofil verläuft, muß man dem Lageregler im Abtastzyklus-Takt richtig berechnete Wegstückchen vorgeben. Man benötigt einen Profilgenerator.

Die gleichförmig beschleunigte Bewegung aus dem Stand

Es soll eine Strecke $s_3$ schnellst möglich mit einer maximalen Beschleunigung $a_1$, einer maximal zulässigen Geschwindigkeit $v$ und einer maximalen Abbremsbeschleunigung $a_2$ verfahren werden. Die Bewegung soll vom Stillstand ausgeführt werden.

Mit den beiden folgenden Festlegungen

sind zwei Fälle zu unterscheiden:

  1. $ds_1 + ds_3 < s_3$ für $s_3 > 0$ bzw.
    $ds_1 + ds_3 > s_3$ für $s_3 < 0$, d.h. die maximale Geschwindigkeit wird erreicht (Trapezprofil).
    Im diesem Fall wird innerhalb des Weges $ds1$ bis auf $v$ beschleunigt, danach bis zum Weg $s_2 = s_3 - ds3$ mit konstanter Geschwindigkeit gefahren und danach bis $s_3$ gebremst.

    =1cm
    \begin{picture}(10, 5)
\put (2,0) {
\put (0,0) { \vector(0,1) {3} }
\put (0...
...
\put (8,-.2) { \line (0,1) {.4} }
\put (8,-.5) {$s_3$}
}
}
\end{picture}
    =1pt

  2. $ds_1 + ds_3 >= s_3$ für $s_3 > 0$ bzw.
    $ds_1 + ds_3 <= s_3$ für $s_3 < 0$, die maximale Geschwindigkeit wird hierbei nicht oder gerade erreicht (Dreieckprofil). Bis zum Weg $s_2$ wird beschleunigt, danach gebremst.

    =1cm
    \begin{picture}(6, 5)
\put (2,0) {
\put (0,0) { \vector(0,1) {4} }
\put (0,...
...
\put (4,-.2) { \line (0,1) {.4} }
\put (4,-.5) {$s_3$}
}
}
\end{picture}
    =1pt

    Dreieckprofil der Geschwindigkeit als Funktion der Zeit

Konstante Beschleunigung aus der Bewegung

Es sei $s_3$ der vorgegebene Weg, $v$ die maximale Geschwindigkeit, $a_1$ die Beschleunigung und $a_2$ die (Brems-) Verzögerung.
Die Variablen $s_0, v_0$ und $a_0$ seien die Werte bei Übergabe des neuen Moves.

Mit den beiden folgenden Festlegungen

können die zwei notwendigen Unterscheidungsfälle folgendermaßen dargestellt werden:

  1. $ds1 + ds3 < s_3$ für $s_3 > 0$ bzw.
    $ds1 + ds3 > s_3$ für $s_3 < 0$, die gewünschte Geschwindigkeit $v$ wird erreicht.

    =1cm
    \begin{picture}(10, 6)
\put (1,0) {
\put (0,0) { \vector(0,1) {6} }
\put (0...
...-.2) { \line (0,1) {.4} }
\put (8,-2) { \line (1,1) {2} }
}
}
\end{picture}
    =1pt

    Die Bewegung besteht aus drei Teilen, bis zur Strecke $s_1$ wird beschleunigt, bis $s_2 = s_3 - ds3$ konstant und von $s_2$ bis $s_3$ verzögert gefahren.

  2. $ds1 + ds3 >= s_3$ für $s_3 > 0$ bzw.
    $ds1 + ds3 <= s_3$ für $s_3 < 0$, die maximale Geschwindigkeit wird unter Vorgabe von $s_3, a_1$ und $a_2$ nicht erreicht (Dreieckprofil).

    =1cm
    \begin{picture}(6, 6)
\put (2,0) {
\put (0,0) { \vector(0,1) {6} }
\put (0,...
...t (3,-1) { \line (1,1) {1} }
\put (3,-1.5) { $v_{neu}$ }
}
}
\end{picture}
    =1pt

    Die Geschwindigkeit, die mit der vorgegebene Beschleunigung unter Einhaltung des Weges s erreicht werden kann, berechnet sich zu:

    \begin{displaymath}
v_{neu}^2 = 2 a_1 (ds1 + ds3 - s_3)
\end{displaymath} (3.1)

    Der Punkt $s_2 = s_3 + \frac{v_{neu}^2}{2 a_2}$ ist dann der Umkehrpunkt von der Beschleunigung $a_1$ zur Verzögerung $a_2$.

    Die Bewegungen mittels einer konstanten Beschleunigung sind zwar einfach zu programmieren, haben aber den Nachteil, daß das Umschalten der Beschleunigungen in der am Motor angkoppelten Mechanik zu einer erhöhten Belastung durch den sogenannten Ruck führt. Außerdem ist die Energiebilanz nicht optimal.

    Um diese Nachteile zu vermindern, können andere Beschleunigungsarten gewählt werden.

Implementierung

Man könnte nun die Bewegung analog den obigen Überlegungen nach folgendem Schema berechnen:

Diese Art der Implementierung ist durchaus möglich, sie benötigt aber relativ viel Zeit für die Berechnung z.B. des Weges sn0. Zur Geschwindigkeitssteigerung wird der Weg rekursiv berechnet. D.h. der Weg zum Abtastzyklus n ergibt sich aus dem Weg zum Abtastzyklus n-1 plus der Wegänderung innerhalb der Abtastzeit, also:

1 sn1 = sn0;                          // Weg des Zyklus' n-1
2 vn1' = vn0';                        // alte Geschwindigkeit
3 vn0' = vn1' + an0'*T0;              // neue Geschwindigkeit
4 sn0 = sn1 + 0.5*(vn0' + vn1')*T0;   // neuer Weg für Zyklus n

Zeile vier umgeformt ergibt $sn0 = sn0 + vn1'*T0 + 0.5*an0'*T0*T0$. Für eine genügend kleine Zykluszeit T0 ergibt sich damit näherungsweise der Weg nach Formel 3.2.

Um die Multiplikation mit T0 einzusparen, wird diese in der Beschleunigung an0' direkt mit einberechnet. Mit $an0 = an0' * T0 * T0$ erhält man:

1 sn1 = sn0;
2 vn1 = vn0;
3 vn0 = vn1 + an0;
4 sn0 = sn1 + 0.5*(vn0 + vn1);

Die Memberfunktion der noch zu implementierenden Klasse MOVE_C, die das Bewegungsprofil generiert und in jedem Taktzyklus aufgerufen wird, unterscheidet nun nur noch die verschiedenen Phasen, d.h. die Beschleunigungs-, die Gleich-, die Verzögerungsphase und das Ende der Bewegung. Disee Methode sieht folgendermaßen aus:

void MOVE_C::SMove (void)
{
  //----- Wenn Endposition erreicht
  if ((SGN (ds, (s3 - sn0)) <= 0) || (SGN (ds, vn0) < 0 && SGN (an0, vn0) >= 0) ) {
    sn0 = s3;
    vn0 = an0 = s3 = 0.;
    Off ();
  }
  else {
    sn1 = sn0;
    vn1 = vn0;
    vn0 = vn1 + an0;

    //----- Begrenzung auf +/- v_max
    if ( ABS (vn0) >= v_max && SGN (vn0, an0) > 0  ||
         (mv_flag != 2 && ABS (vn0) <= v_max && SGN (vn0, an0) < 0) ) {
      vn0 = SGN (ds, v_max);
      an0 = 0;
    }
    sn0 = sn1 + 0.5*(vn0 + vn1);

    //----- Beginn des Abbremsvorganges (s2 erreicht) im nächsten Takt
    if (mv_flag != 2 && SGN (ds, sn0 + vn0 + 0.5*an0 - s2) >= 0) {
      an0 = 0.5 * vn0*vn0 / (sn0 - s3);
      mv_flag = 2;                    //----- damit an0 nur einmal berechnet wird
    }
  }
}

Die sinusförmig beschleunigte Bewegung aus dem Stand

Soll ein Move stetig im Beschleunigungsprofil sein, so kommt als Beschleunigungsfunktion u.a. die Funktion $a (t) = \frac{1}{2} \cdot (1 -
\cos (kt), t \in [0 \ldots 2 \frac{\pi}{k}] $ in Betracht.

Hieraus ergibt sich für den Weg durch Integration im Beschleunigungsintervall:

$\displaystyle s$ $\textstyle =$ $\displaystyle \frac{1}{2} \int \!\! \int 1 - \cos (kt) dt^2$  
  $\textstyle =$ $\displaystyle \int t - \frac{2}{k} \sin (k t) dt$  
  $\textstyle =$ $\displaystyle \frac{1}{2} \left[\frac{t^2}{2} + \frac{\cos kt}{k^2} \right]_0^x$ (3.3)
$\displaystyle x,t$ $\textstyle \in$ $\displaystyle [0 \ldots \frac{2\pi}{k}]$  

Am Ende der Beschleunigung ergibt sich demnach:

$\displaystyle t$ $\textstyle =$ $\displaystyle 2 \frac{v_{max}}{a}$  
$\displaystyle v_{max}$ $\textstyle =$ $\displaystyle \frac{\pi}{k} \cdot a \leftrightarrow k = \pi \frac{a}{v_{max}}$  
$\displaystyle s$ $\textstyle =$ $\displaystyle \frac{\pi}{k}^2 \cdot a = \frac{v_{max}^2}{a}$  

Kann $v_{max}$ nicht erreicht werden, dann soll die Beschleunigung stetig durchfahren werden (entspricht dem Dreieckprofil). Dafür muß $k$ angepasst werden zu $k = \pi \cdot \sqrt {\frac{2a}{s}}$.

Programmablauf

Wird ein Move-Kommando übergeben, so wird k nach folgendem Schema berechnet:
$\displaystyle \frac{v_{max}^2}{a} > \frac {s}{2}$ $\textstyle \longrightarrow$ $\displaystyle k = \pi \cdot \sqrt {\frac{2a}{s}}
, \quad s_2 = s/2$ (3.4)
$\displaystyle sonst$ $\textstyle \longrightarrow$ $\displaystyle k = \pi \cdot \frac{a}{v_{max}}, \quad s_2 = s -
\frac{v_{max}^2}{a}$ (3.5)

Die zyklisch durchlaufene Profilgenerierung lautet dann:

-
vor $s_2$ und ($v_{max}$ erreicht oder $kiT_0 > 2\pi$) ?
$ \stackrel {nein}{\longrightarrow} a_n = a \cdot \frac {1 - \cos
kiT_0}{2} \quad ; i++$
-
$v_{max}$ erreicht und vor $s_2$ ?
$ \stackrel {ja}{\longrightarrow} a_n = 0 \quad ;i = 0$
-
$s_2$ erreicht ?
$ \stackrel {ja}{\longrightarrow} a_n = -a \cdot \frac {1 - \cos
kiT_0}{2} \quad ; i++$
-
$s$ erreicht oder $SGN (s,v) \leq 0$ ?
$ \stackrel {ja}{\longrightarrow} $ Ende

Folgende Funktionen sind public implementiert:

void Reset (void):
Funktion zum Zurücksetzten der internen Variablen.
void Set (FLOAT64 *dp)
dp [0] setzt die maximale zulässige Achsgeschwindigkeit.
void Start (FLOAT64 start_pos, FLOAT64 *dp):
Funktion zum Start der Profilgenerierung.
Übergabeparameter:
start_pos
ist die aktuelle Sollposition bei Aufruf des Move
dp [0]
Weg s
dp [1]
max. Geschwindigkeit v
dp [2]
Beschleunigung a
dp [3]
Verzögerung d
dp [4]
1. == rel, 0. == abs
dp [5]
1. == sinus, 0. == rechteck
void Stop (FLOAT64 *dp):
Funktion zum Stoppen des move mit der Abbremsrampe dp [0].
void Run (FLOAT64 *refp):
Funktion zum Berechnen des Sollwertes. Diese Funktion muß zyklisch vor der Lagereglung aufgerufen werden. Der Berechnete Sollwert wird nachh *refp geschrieben.
BOOL IsActive (void):
Zeigt an, ob der Move noch aktiv ist.
const double& CXa (void):
Aktueller Ausgangswert (Weg) der Profilgenerierung.

Einige Anmerkungen zur Memberfunktion Start: Beim Start des Move werden die User-Daten übernommen, die genaue Berechnung der Wege s1, s2, s3 und die erreichbere Geschwindigkeit werden erst in der privaten Funktion SFlyMove bzw. SinSFlyMove im Taktzyklus beim nächsten Aufruf von Run berechnet. Start setzt dafür eine Variable, um dies der Funktion Run mitzuteilen. Dies hat den Vorteil, daß eine Funktion sowohl bei der Bewegung aus dem Stillstand, als auch bei einer neuen Bewegung während einer noch aktiven, benutzt werden kann.

Die oben aufgeführten Zusammenhänge werden nun in eine Klasse implementiert. Zuerst die Klassendefinition???(deklaration): //

#ifndef MOVE_H
#define MOVE_H 1

#define FCT         0
#if FCT
#define TAMove      1
#define TVMove      MOVE_C::VMove
#define TSMove      MOVE_C::SMove
#define TSFlyMove   MOVE_C::SFlyMove
#define TSinSMove   MOVE_C::SinSMove
#define TSinSFlyMove   MOVE_C::SinSFlyMove
#define TPStop      MOVE_C::PStop
#else
#define TAMove      1
#define TVMove      2
#define TSMove      3
#define TSFlyMove   4
#define TSinSMove   5
#define TSinSFlyMove   6
#define TPStop      7
#endif


//----- benötigt types.h, pid.h, trig.h
class MOVE_C //: public BASE_C
{
public:
         MOVE_C (void) { active = mv_flag = 0; }
  void   Reset  (void);
  void   Set    (const FLOAT64 *dp) { v_peak = dp [0]*T0*T0; }
  int    Start  (FLOAT64 start_pos, const FLOAT64 *dp);  //---- s,v,a,d,{r|a},sin
  void   Run    (void);
  void   Run    (FLOAT64 *refp) { Run (); *refp = sn0; } //---- s reference
  void   Stop   (const FLOAT64 *dp);                     //---- *dp = decelleration

  BOOL   IsActive (void) const { return (BOOL) active == ON; }
	BOOL   Check  (void) const { return 1; } //!!!
  const FLOAT64& TMove (void) const { return t_move; }
  const FLOAT64& CXa   (void) const { return sn0; }

#if 0
  void   Set    (const FLOAT64& vpeak) { v_peak = vpeak*T0*T0; }
  void   Set    (int mode, FLOAT64 val);
  void   Set    (FLOAT64 s, FLOAT64 v, FLOAT64 a, FLOAT64 d);
#endif

private:
  void   On    (void) { active = ON;  }
  void   Off   (void) { active = OFF; }

#if 0
  double GetS   (void) { return ctrl.com.bb.para [1]; }
  double GetV   (void) { return ctrl.com.bb.para [2]*T0; }
  double GetA   (void) { return ctrl.com.bb.para [3]*T0*T0; }
  double GetD   (void) { return ctrl.com.bb.para [4]*T0*T0; }
  double EmA    (void) { return ctrl.com.bb.para [1]*T0*T0; }

  double GetMode    (void) const {return ctrl.com.bb.para [5]; }
  double GetSinus   (void) const {return ctrl.com.bb.para [6]; }
#endif

#if FCT
  void   SetFct     (void (MOVE_C::*fp)(void)) { fct_ptr = fp; }
#else
  void   SetFct     (int type) { fct_ptr = type; }
#endif
  void   PStop      (void);
  void   VMove      (void);
  void   SFlyMove   (void);
  void   SMove      (void);
  void   SinSFlyMove(void);
  void   SinSMove   (void);

#if FCT
  void      (MOVE_C::*fct_ptr) (void);
#else
  int       fct_ptr;
#endif
  int       active,     // ON if active
            absolute;   // != 0, if absolute way
                        // user values
  FLOAT64   s_user,     // way s
            v_user,     // velocity v
            a_user,     // acceleration
            d_user;     // deceleration
  FLOAT64   v_peak;
  FLOAT64   v_max,      // max. v in unit per t0
            a_max,      // max. a in unit per t0^2
            d_max;      // max. d in unit per t0^2
  FLOAT64   ds;         // reference way - current way
  FLOAT64   s1,         // rel. s during acceleration
            s2,         // s, when start breaking
            s3;         // end position
  FLOAT64   an0, an1,   // calculated acceleration as way in t0^2 !!!
            vn0, vn1;   // velocity as way in t0 !!!
  FLOAT64   sn0, sn1;   // calculated way
  FLOAT64   t_move;
  int       mv_flag;

  //----- only for sinus move
  FLOAT64   k,          // Winkelgeschwindigkeit for cos
            v_new,
            soff;       // Offset for sinus acceleration
  UINT32    tics,       // counter for sinus move
            sinus_mode; // flag for sinus on
  long      scou1,      // counter during acceleration
            scou2;
};

#if 0
void MOVE_C::Set (int mode, FLOAT64 val)
{
  switch (mode)
  {
  case MOVE_S:
    s_user = val;
    break;
  case MOVE_V:
    v_user = Abs (val)*T0;
    break;
  case MOVE_A:
    a_user = Abs (val)*T0*T0;
    break;
  case MOVE_D:
    d_user = Abs (val)*T0*T0;
    break;
  case MOVE_MAX_D:
    d_user_max = Abs (val)*T0*T0;
    break;
  case SINUS:
    sinus_mode = val ? ON : OFF;
  }
}

int MOVE_C::Set (FLOAT64& s, FLOAT64& v, FLOAT64& a, FLOAT64& d,
                 int mode, int profile)
{
  long error = E_OK;

  if (v_user == 0.)                   error = E_MOVE_V_NULL;
  if (a_user == 0.)                   error = E_MOVE_A_NULL;
  if (d_user == 0.)                   error = E_MOVE_D_NULL;
  if (error == E_OK) {
    s_user = s;
    v_user = Abs (v*T0);
    a_user = Abs (a*T0*T0);
    d_user = Abs (d*T0*T0);
  }
  absolute   = mode ? 1 : 0;
  sinus_mode = profile ? 1 : 0;
  return error;
}
#endif



#endif
//

Der Sourcecode der Klasse lautet: //

#include <math.h>
#include "types.h"
#include "ctrl.dcl"
#include "base.h"
#include "other.h"
#include "move.h"
#include "pid.h" //!!! für VOn

#if SIN_TAB
extern TRIG_C trig;
#endif
//extern long ref_offset [];

void MOVE_C::Reset (void)
{
	//el     = dp [0];
  s1 = s2 = s3 =  sn0 = sn1 =  vn0 = vn1 =  an0 = an1 = 0.;
  sinus_mode = CTRL_SINUS_OFF;
  Off ();
}

int MOVE_C::Start (FLOAT64 start_pos, const FLOAT64 *dp)       //---- s, v, a, d, rel|abs, sinus
{
  if (dp [4] >= 2.) {              //----- A- oder V-Move
    sn0 = start_pos;
    vn0 = vn1 = an0 = an1 = 0.;
    //SetV (0);
    if (dp [4] == 2)
      ;// SetFct (TAMove);
    else                              SetFct (TVMove);
  }
  else {
    s_user = dp [0];
    v_user = dp [1]*T0;
    a_user = dp [2]*T0*T0;
    d_user = dp [3]*T0*T0;
    absolute   = dp [4] ? 1 : 0;
    sinus_mode = dp [5] ? 1 : 0;

//    if (absolute)                     s_user -= ref_offset [el];

    if (v_user == 0.)                 return E_MOVE_V_NULL;
    if (a_user == 0.)                 return E_MOVE_A_NULL;
    if (d_user == 0.)                 return E_MOVE_D_NULL;

    t_move = 0.;
    if (!IsActive () ) {
      sn0 = start_pos;
      if (absolute)                   s3 = 0;
      else                            s3 = sn0;
    }
		if (ABS (s_user) == 0.) {              //neu 031197
      if (!absolute)                  return E_MOVE_DS_NULL;
      else                            return E_OK;
    }
    if (sinus_mode)                   SetFct (TSinSFlyMove);
    else                              SetFct (TSFlyMove);
  }
  On ();
  return E_OK;
}

/*-----------------------------------------------------------------------------
  Beschreibung      : Führt Move aus für Geschwindigkeits-Modus.
  Seiteneffekte     : Ändert s,v,a
-----------------------------------------------------------------------------*/
void MOVE_C::VMove (void)
{
#if 0
  //----------------------- für Geschwindigkeitsvorgabe
  v_user = GetV ();
  //----- auf zulässiges v begrenzen
  if (ABS (v_user) > v_peak)         v_user = SGN (v_user, v_peak);
  a_max = ABS (GetA ());
  an0   = SGN (v_user - vn0, a_max);

  v_max = ABS (v_user);

  sn1 = sn0;
  vn1 = vn0;
  vn0 = vn1 + an0;
  if (ABS (vn0) >= v_max && SGN (vn0, an0) >= 0.) {
    vn0 = v_user;
  }
  sn0 = sn1 + vn0;
#endif

#if 0
  //----------------------- für Geschwindigkeitsvorgabe
  v_new = GetV ();
#if JOY_V_MODE
  if (!(v_new || v0)) {
    pid [el].VOn ();
  }
  else {
    pid [el].VOff ();
  }
#endif
  a_max = ABS (GetA ());
  v_max = ABS (v_new);
  a0    = SGN (v_new, a_max);         //----- Vorzeichen a0 wie v_new

  //----- auf zulässiges v begrenzen
  if (v_max > VPeak ()) {
    v_max = VPeak ();
    v_new = SGN (v_new, v_max);
  }

  s1 = s0;
  v1 = v0;
  if (ABS (v0) > v_max) {             //----- abbremsen auf v_max
    if (ABS (v_new - v0) <= a_max) {
      a0 = v_new - v0;                //----- Überschwingen unterbinden
    }
    else
      a0 = -SGN (v0, a_max);
  }
  v0 = v1 + a0;                       //----- neues v berechnen

  //----- beschleunigen
  if ( ABS (v0) >= v_max && SGN (v0, a0) > 0.) {
    a0 = 0;
    v0 = v_new;
  }
  s0 = s1 + 0.5 * (v0 + v1);
#endif
}
/*-----------------------------------------------------------------------------
  Beschreibung      : Führt Move aus für Weg-Modus, Move kann online
                      verändert werden.
                      Die übergebene Geschwindigkeit darf nicht niedriger sein
                      als die im vorherigen Move, sonst wird der Antrieb
                      schlagartig auf die neue Geschwindigkeit gebracht!
  Seiteneffekte     : Ändert s,v,a
-----------------------------------------------------------------------------*/
void MOVE_C::SFlyMove (void)
{
  FLOAT64 tmp, v, a_1, a_2, ds1, ds2, ds3;

  s3   += s_user;
  v_max = v_user; //----- alles Beträge
  a_max = a_user;
  d_max = d_user;

  mv_flag = 0;

  ds  = s3 - sn0;                     // relativer Weg
  if (ds < 0.) {
    v = -v_max;
    a_2 = d_max;                      // Verzögerung
  }
  else {
    v   = v_max;
    a_2 = -d_max;                      // Verzögerung
  }

	 a_1 = SGN ((v - vn0), a_max);       // Beschleunigung

  ds1 =  .5*(v*v-vn0*vn0)/a_1;        // Weg während der Beschleunigung
  ds3 = -.5*v*v/a_2;                  // Weg während der Verzögerung
  ds2 = ds - ds1 - ds3;

  if (SGN (ds, ds2) < 0.) {     // Dreieckprofil
    // tmp  *= .5*a_1;                // richtig ?
    a_2 = - SGN (a_1, d_max);
    v_max = (2.*a_1*a_2*ds + a_2*vn0*vn0) / (a_2 - a_1);
    v_max = sqrt (ABS (v_max));
    v     = SGN (a_1, v_max);
    ds3   = -.5*v*v/a_2;
  }
  else {
    t_move = ds2 / v;
  }
  t_move += (v - vn0)/a_1 - v/a_2;

  if (ABS (ds) <= ABS (a_1)) {
    sn0 = s1 = s2 = s3;
  }
  else {
    s2 = s3 - ds3;
    an0 = a_1;
  }
  SetFct (TSMove);
  SMove ();
}

/*-----------------------------------------------------------------------------
  Beschreibung      : Führt Move aus für Weg-Modus
-----------------------------------------------------------------------------*/
void MOVE_C::SMove (void)
{
  //----- Wenn Endposition erreicht
  if ( (SGN (ds, (s3 - sn0)) <= 0.) ||
       (SGN (ds, vn0) < 0. && SGN (an0, vn0) >= 0.) ) {
    sn0 = s3;
    vn0 = an0 = s3 = 0.;
    Off ();
  }
  else {
    sn1 = sn0;
    vn1 = vn0;
    vn0 = vn1 + an0;

    //----- Begrenzung auf +/- v_max
    if ( ABS (vn0) >= v_max && SGN (vn0, an0) > 0. ||
         (mv_flag != 2 && ABS (vn0) <= v_max && SGN (vn0, an0) < 0) ) {
      vn0 = SGN (ds, v_max);
      an0 = 0.;
    }
    sn0 = sn1 + 0.5*(vn0 + vn1);

    //----- Beginn des Abbremsvorganges (s2 erreicht) im nächsten Takt
    if (mv_flag != 2 && SGN (ds, sn0 + vn0 + an0/2 - s2) >= 0) {
      an0 = 0.5 * vn0*vn0 / (sn0 - s3);
      mv_flag = 2;                    //----- damit an0 nur einmal berechnet wird
    }
  }
}

/*-----------------------------------------------------------------------------
  Beschreibung      : Führt Move aus für Sinus-Weg-Modus, Move kann online
                      verändert werden.
                      Die übergebene Geschwindigkeit darf nicht niedriger sein
                      als die im vorherigen Move, sonst wird der Antrieb
                      schlagartig auf die neue Geschwindigkeit gebracht!
-----------------------------------------------------------------------------*/
void MOVE_C::SinSFlyMove (void)
{
  double ds1, ds2, ds3, v, a, d;

  s3     += s_user;
  v_max   = v_user;
  a_max   = a_user;
  d_max   = d_user;

  mv_flag = 0;
  sinus_mode = CTRL_SINUS_ON;

  ds = s3 - sn0;
  v  = SGN (ds, v_max);
  a  = SGN (ds, a_max);

  vn1  = vn0;
  an0  = SGN ((v - vn0), a_max);
  soff = sn0;                          //----- Startposition
  tics = 0;

  ds1 = (v*v - vn0*vn0)/an0;
  ds3 =  - v*v/d;
  ds2 = ds1 + ds3 - ds;
  if ( SGN (ds, ds2) > 0) {            //----- Dreieckprofil
    d = - SGN (a, d_max);
    v = a*d*ds + d*vn0*vn0 / (d - a);
    v_max = v = sqrt (ABS (v));
    v = SGN (a, v);
    ds3 =  - v*v/d;
  }
  k   = PI * an0 / (v - vn0);
  s2  = s3 - ds3;
  an1 = 0.5*an0;           //----- damit Berechnung in SinSMove schneller

  scou1 = 2.0 * PI / ABS (k) + .5;
  t_move  = 2. * scou1 + (s3 / v - 2. * v / d);
  scou2 = 1;
  v_new = v;
  SetFct (TSinSMove);
  SinSMove ();
}

/*-----------------------------------------------------------------------------
  Beschreibung      : Führt Move aus für Sinus-Weg-Modus,
  Vorraussetzungen  : soff, scou1, k, an0
                      muß vor dem ersten Aufruf berechnet sein
-----------------------------------------------------------------------------*/
void MOVE_C::SinSMove (void)
{
  /*an0 = .5*SGN (ds, a_max)/k*(1-cos (k*t));
    vn0 = .5*SGN (ds, a_max)*(t-sin(k*t)/k);
    sn0 = .5*SGN (ds, a_max)*(t*t/2 + cos (k*t)/k/k); */
  FLOAT64 t;

  if (!scou1) {                       //---- neuer Abschnitt
    tics = 0;
    soff = sn0;
    if (++mv_flag == 1) {             //---- s1 bis s2, v = const. Bereich
      vn1 = v_new;
      if (SGN (ds, sn0 + vn1) >= SGN (ds, s2))    mv_flag++;
      else {
        scou1 = (long) (ABS (s2 - sn0) / v_max );
        if (!scou1)                               mv_flag++;
      }
    }
    if (mv_flag == 2) {               //---- Bereich s2 bis s3
      vn1 = v_new;
      an1 = vn1 * vn1 / (s3 - sn0);
      k  = PI * an1 / vn1;

      scou1 = 2.*PI/ABS(k) + .5;
      an1   *= -.5;
    }
    else if (mv_flag == 3) {          //---- Endposition s3 erreicht
      sn0 = s3;
      vn0 = vn1 = an0 = an1 = 0.;
      sinus_mode = CTRL_SINUS_OFF;
      Off ();
    }
  }
  if (scou1) {
    t = ++tics;
    sn0 = soff + vn1*t;
    if (mv_flag != 1)
#if SIN_TAB
			sn0 += an1*(.5*t*t + (trig.Cos (k*t)-1.)/k/k);
#else
			sn0 += an1*(.5*t*t + (cos (k*t)-1.)/k/k);
#endif
    scou1--;
  }
  an0 = -vn0;
  vn0 = sn0 - sn1;                    //---- Istgeschwindigkeit berechnen
  an0 += vn0;                         //---- Istbeschleunigung  berechnen
  sn1 = sn0;
}

void MOVE_C::Run (void)
{
  if (!IsActive ())                   return;

#if FCT
  (this->*fct_ptr) ();
#else
  switch (fct_ptr)
  {
  case TSMove       : SMove     (); break;
  case TSinSMove    : SinSMove  (); break;
  case TVMove       : VMove     (); break;
  case TPStop       : PStop     (); break;
  //case TAMove       : AMove     (); break;
  case TSFlyMove    : SFlyMove  (); break;
  case TSinSFlyMove : SinSFlyMove  (); break;
  }
#endif

//  Soll ().a0 = an0;
//  Soll ().v0 = vn0;
//  Soll ().s0 = sn0;
}

void MOVE_C::Stop (const FLOAT64 *dp)
{
  if (!IsActive ())                     return;
  d_user = ABS (*dp)*T0*T0; //d_user_max;
  SetFct (TPStop);
}

void MOVE_C::PStop (void)
{
  an0 = d_user;
  s2  = sn0;                          //----- ab jetzt bremsen
  an0 = - SGN (vn0, an0);
  s3  = sn0 - 0.5 * vn0 * vn0 / an0;

  sinus_mode = CTRL_SINUS_OFF;
  mv_flag    = 2;                     //----- damit a nicht neu berechnet wird
  SetFct (TSMove);
  SMove ();
}



#if 0
//---- Move nicht rekursiv, sondern in Abschnitten
void MOVE_C::SFlyMove (void)
{
  FLOAT64 tmp, v, a_1, a_2;

  s3   += s_user;
  v_max = v_user; //----- alles Beträge
  a_max = a_user;
  d_max = d_user;

  mv_flag = 0;

  ds = s3 - sn0;                      // relativer Weg
  v  = SGN (ds, v_max);               //
  a_1  = SGN ((v_new - vn0), a_max);  // Beschleunigung
  a_2  = - SGN (ds, d_max);           // Verzögerung

  tmp = (2*ds*a_1 + vn0*vn0) /
        (1-a_1/a_2);                  // aus ds = s(Beschl.) + s(Verz.)
  if (SGN (ds, v*v-tmp) > 0) {        // Dreieckprofil
    v_max = sqrt (ABS (tmp));
    v     = SGN (a_1, v_max);
  }

  s2 = s3 + 0.5 * v*v / a_2;
  if (ABS (ds) < ABS (a_1))           sn0 = s1 = s2 = s3; //neu

  an0 = a_1;
  vn1 = 0.;
  v_new = v;

  SetFct (TSMove);
  SMove ();
}

void MOVE_C::SMove (void)
{
  FLOAT64 t;

  if (!scou1) {
    tics = 0;
    if (++mv_flag == 1) {
      scou1 = (v_new - vn0) / an0 + 0.5;
      soff  = sn0;
      vn1   = vn0;
    }
    else if (mv_flag == 2) {             //----- s1 bis s2, v = const. Bereich
      scou1 = (long) (s2 - sn0) / v_new;
      if (scou1 > 0) {
        soff = sn0;
        vn1  = v_new;
        an0  = 0.;
      }
      else {
        mv_flag++;
      }
    }
    if (mv_flag == 3) {               //----- s2 bis s3
      scou1 = 2*(s3-sn0) / vn0;
      soff = s2;
      an0   = vn0 / scou1;
    }
    else if (mv_flag == 4) {          //----- Endposition s3 erreicht
      sn0 = s3;
      vn0 = vn1 = an0 = 0.;
      sinus_mode = CTRL_SINUS_OFF;
      Off ();
    }
    mv_flag++;
  }
  if (scou1) {
    t = ++tics;
    sn0 = soff + (vn1 + 0.5 * an0 * t) * t;
    scou1--;
  }
  vn0 = sn0 - sn1;
  sn1 = sn0;
#endif
//

Das elektronische Getriebe

Bevor es digitale Regler gab, wurden Achsen, die mit einem Hauptantrieb gekoppelt wurden, durch mechanische Getriebe angekoppelt. Wollte man den Kopplungsfaktor ändern, so mußte das Getriebe bzw. Zahnrad ausgetauscht werden. Solches Umrüsten bedeutet einen Stillstand der Machine. Zusätlich mußte für jeden Kopplungsfaktor ein passendes Getriebe vorhanden sein.

Durch die digitalen Regler entfallen diese Nachteile, denn das Getriebe wird durch Software simuliert. Das Getriebe ist mathematisch nichts anderes als die Multiplikation des Weges der Hauptachse mit einem Faktor. Die Hauptachse ist dabei die Achse, die den größten Weg zurücklegt. (Dies gilt nur bei gleicher Wegauflösung der Antriebe.) Der Weg der nachgeführten Achse ist dann $s_{nach} = s_{Haupt}*k_{nach}$.

//

#ifndef COUPLE_H
#define COUPLE_H 1

#define COUPLE_NEW 1

class COUPLE_C : public BASE_C
{
public:
const   int&      Main      (void) const  { return main; }
									COUPLE_C  (void) { main = 0; fac = 0; active = 0;}
				void      Init      (int drive);

#if COUPLE_NEW
        void      Start     (FLOAT64 fac, FLOAT64 s_ref,
                             FLOAT64 s_ref_main, int main);
        void      Run       (const FLOAT64& s_cur_or_ref_main);
#else
				void      Start     (void);
				void      Run       (void);
				void      SetFac    (double fact, int main);
#endif
        void      Stop      (void);

  const FLOAT64&  Factor    (void) const  { return fac; }
				BOOL      IsActive  (void) const  { return (BOOL) active == 1; }
  const FLOAT64&  CXa       (void) const  { return xa; }

private:
	int      active;
	int      main;        //----- führender Antrieb
	FLOAT64  start_cou;   //----- Startzählerstand
	FLOAT64  fac;         //----- Koppelfaktor
  FLOAT64  xa;

  SOLL_STATE_T& Soll (const int& ele) const { return prctrl.soll [ele]; }
  IST_STATE_T & Ist  (const int& ele) const { return prctrl.ist [ele]; }
  SOLL_STATE_T& Soll (void) const { return BASE_C::Soll (); }
  IST_STATE_T & Ist  (void) const { return BASE_C::Ist  (); }
	void          Off  (void)       { active = 0; }
	void          On   (void)       { active = 1; }

};


#endif
//

Der Sourcecode der Klasse lautet: //

#include <math.h>
#include "types.h"
#include "ctrl.dcl"
#include "base.h"
#include "couple.h"

void COUPLE_C::Init (int drive)
{
  el = drive;
}

#if COUPLE_NEW
void COUPLE_C::Start (FLOAT64 factor, FLOAT64 s_ref, FLOAT64 s_ref_main, int main_drive)
{
  if (factor) {
    fac  = factor;
    main = main_drive;
    xa = start_cou = s_ref - fac*s_ref_main;
    On  ();
  }
  else                                Off ();
}

void COUPLE_C::Run (const FLOAT64& s_main)
{
  if (!IsActive () || el == Main ())    return;

  xa = fac*s_main + start_cou ;
}


#else
void COUPLE_C::SetFac (double factor, int main_drive)
{
  if (factor) {
    fac  = factor;
    main = main_drive;
  }
}

void COUPLE_C::Start (void)
{
  if ( fac ) {
    start_cou = Soll ().s0 - fac*Soll (Main ()).s0;
    On  ();
  }
  else                                Off ();
}

void COUPLE_C::Run (void)
{
  if (!IsActive () || el == Main ())    return;

#if 1
  Soll ().s0 = fac*Soll (Main ()).s0 + start_cou ;
#else
  Soll ().s0 = fac*Ist (Main ()).s0 + start_cou ;
#endif
}

#endif

void COUPLE_C::Stop (void)
{
#if 0
  if (el != Main ()) {
    FLOAT64 s = fac*Soll (Main ()).sn0 + start_cou ;
    if (ABS (Soll ().sn0 - s) < .01)   Soll ().sn0 = s;
  }
  Soll ().vn0 = 0;
#endif
  Off ();
  fac  = 0;
  main = 0;
}


//

Das Beispielprogramm des Regler wird nun etwas variiert und mit einem Move erweitert.

#include "types.h"
#include "pid.h"
#include "move.h"
#include "ctrl.h"

  PID_C  pid    [1];
  MOVE_C move   [1];

void SetPidAndMove (void)
{
  double dp [6];
  int    el = 0;

  pid  [el].Reset ();
  move [el].Reset ();

  dp [0] = 10.;                 //---- Kp
  dp [1] = 1./.026;             //---- Ki=1/Ti
  dp [2] = .010;                //---- Td
  dp [3] = .004;                //---- T1
  pid [el].SetPara (CTRL_SP_SPID, 4, dp);

  dp [0] = 10;                  //---- Xamax
  dp [1] = 0.;                  //---- S-Vorsteuerung
  dp [2] = 0.;                  //---- V-Vorsteuerung
  dp [3] = 0.;                  //---- A-Vorsteuerung
  dp [4] = 0.;                  //---- Vmax für Reglerüberwachung
  pid [el].SetPara (CTRL_SP_MISC, 5, dp);

  soll = berechneter\_positionswert
  pid [el].Start (soll);

  move [el].Set (dp [0] = 100.);//---- zulässige Geschwindigkeit setzen

  //---- Move-Bewegung starten
  dp [0] = 100.;                //---- Weg
  dp [1] = 50.;                 //---- max. Geschwindigkeit
  dp [2] = 100.;                //---- Beschleunigung
  dp [3] = 300.;                //---- Verzögerung
  dp [4] = 0.;                  //---- relative Wegangabe
  dp [5] = 0.;                  //---- Rechteckbeschleunigung
  move [el].Start (soll, dp);
}

void TaskCtrl (void)
{
  int el;
  while (!Abbruch ()) {
    for (el=0; el<1; el++) {
      WarteAufNeuenZyklus ();
      LeseIstWerte  (&ist);
      move [el].Run (&soll);
      pid  [el].Run (soll, ist);
      SetzeAusgänge  ();
    }
  }
}

void main (void)
{
  SetPidAndMove ();
  TaskCtrl ();
}

In einem echtzeitfähigem Multitasking-Betriebssystem würde TaskCtrl als unabhängige Task mit hoher Priorität laufen. In einer weiteren Kommando-Task würden dann die Befehle angenommen und verarbeitet werden. Hier ein Beispiel für den Ablauf mit einem Echtzeitbetriebssystem bei dem vier Achsen gleichzeitig geregelt werden können.

#include "types.h"
#include "pid.h"
#include "move.h"
#include "ctrl.h"

#define MAX_DRIVES  4

PID_C    pid    [MAX_DRIVES];
COUPLE_C couple [MAX_DRIVES];
MOVE_C   move   [MAX_DRIVES];

FLOAT64 soll [MAX_DRIVES];
FLOAT64 ist  [MAX_DRIVES];

void TaskRx (void)
{
  while (1) {
    WarteAufKommando ();
    FühreKommandoAus ();
  }
}

void TaskCtrl (void)
{
  int el;

  WarteAufInitialisierung ();
  while (1) {
    WarteAufNeuenZyklus ();
    for (el=0; el<MAX_DRIVES; el++) LeseIstWerte  (&ist [el]);
    for (el=0; el<MAX_DRIVES; el++) {
      move [el].Run (&soll [el]);
    }
    for (el=0; el<MAX_DRIVES; el++)
      couple [i].Run (&ist [couple [i].Main ()]);
    for (el=0; el<MAX_DRIVES; el++) {
      pid  [el].Run (soll [el], ist [el]);
      SetzeAusgänge  ();
    }
  }
}

void main (void)
{
  InitialisiereTaskRx   ();
  InitialisiereTaskCtrl ();
  for (el=0; el<MAX_DRIVES; el++) {
    pid  [el].Reset ();
    move [el].Reset ();
  }
  StarteTaskRx ();
  StarteTaskCtrl ();
  while (1) {
    If (Abbruch ())   return;
  }
}

Die drei for-Schleifen in der Funktion TaskCtrl können nicht zu einer zusammengefasst werden, da die Koppelklasse die Sollwerte der führenden Achse benötigt, diese kann aber irgend eine Achse sein, auch die mit der höchsten Nummer. Daher müssen vorher die Sollwerte aller Achsen vom MOVE-Befehl zur Verfügung stehen.

Die Funktion FühreKommandoAus würde Benutzerdaten abfragen und die Regler und moves usw. initialisieren und starten.

Die Tachosimulation

Besitzt der digitale Lagereglerkreis einen unterlagerten analogen Drehzahlregelkreis, so benötigt der Drehzahlregler zum Sollwert auch einen Istwert. Der Istwert, d.h. die aktuelle Geschwindigkeit, kann dabei vom Lageregelkreis aus der Weginformation berechnet werden. Dieser berechnete Istwert wird meistens noch gefiltert und z.B. als Spannungswert ausgegeben.

Der Filter kann ein gleitender Mittelwert oder ein nachgebildeter analoger Tiefpaß sein.

Das Reglerprogramm

Befehlsschnittstelle vom Client

Befehlsschnittstelle zum Client

Reglerbefehle

Fehlermeldungen

Reglerschnittstellen zur Außenwelt

Schnittstelle zum Encoder

Schnittstelle zum DA-Wandler

E/A-Interface

Compiler

Sicherheitsvorkehrungen

Einstellen der Parameter

Abgleich unterlagerter Regler

Berechnung der digitalen Parameter aus den Antriebsdaten

Einige Beispielantriebe

Quellenverzeichnis

/1/ Dubbel, Taschenbuch für den Maschinenbau, 17. Auflage, Seite X18
/2/, Franzis', Reglungstechnik mit PC, Zirpel, ISBN 3-7726-6954-5
/3/

About this document ...

This document was generated using the LaTeX2HTML translator Version 2008 (1.71)

Copyright © 1993, 1994, 1995, 1996, Nikos Drakos, Computer Based Learning Unit, University of Leeds.
Copyright © 1997, 1998, 1999, Ross Moore, Mathematics Department, Macquarie University, Sydney.

The command line arguments were:
latex2html -split 0 -iso_language GE -address 'Autor: Thomas Block, HTML code generated by latex2html' rbuch.tex

The translation was initiated by Knoppix User on 2010-08-18 up

Autor: Thomas Block, HTML code generated by latex2html