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 |
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.
Je größer die Verstärkung 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.
(1.3) |
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.
(1.5) |
(1.6) |
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.
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.
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.
stellt z.B. einen Meßwert oder Ausgangswert zum Zeitpunkt dar. Der -Operator kann Berechnungen von Integralen und Ableitungen vereinfachen.
(2.1) |
(2.2) |
Eine genauere Anäherung des Integrals erhält man durch die Trapezformel. Zwei
benachtbarte Punkte werden durch eine Gerade verbunden. Daraus folgt:
(2.3) |
(2.4) |
Es handelt sich in beiden Fällen um einen Filter 1. Ordnung.
(2.5) |
(2.6) |
Die rekursive Form im Abtastbereich lautet dann:
(2.7) |
(2.8) | |||
(2.9) |
(2.10) | |||
(2.11) |
(2.12) |
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 einen Schwellwert , so wird der Reglerausgang auf begrenzt.
In der folgenden Tabelle sind verschiedene Reglertypen und die dazugehörigen Faktoren aufgeführt.
P | I | D-T1 | PI | PD-T1 | PID-T1 | |
PID-T1 (Hütte) | ||
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/.
(2.14) |
Oftmals wird dieser Zusammenhang auch als IRF-Filter n ter Ordnung bezeichnet, da der Ausgangswert sich auch bei einem konstanten Einganswert bis im unendlichen ändert.
Werden für den aktuellen Ausgangswert nur Eingangswerte benötigt, also und , dann spricht man von FIR-Filtern, da der Ausgangswert nach max. n Abtastzyklen konstant wird.
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:
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.
Mit den beiden folgenden Festlegungen
=1cm
=1pt
=1cm
=1pt
Mit den beiden folgenden Festlegungen
=1cm
=1pt
Die Bewegung besteht aus drei Teilen, bis zur Strecke wird beschleunigt, bis konstant und von bis verzögert gefahren.
=1cm
=1pt
Die Geschwindigkeit, die mit der vorgegebene Beschleunigung unter Einhaltung
des Weges s erreicht werden kann, berechnet sich zu:
Der Punkt ist dann der Umkehrpunkt von der Beschleunigung zur Verzögerung .
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.
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 . 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 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 } } }
Hieraus ergibt sich für den Weg durch Integration im Beschleunigungsintervall:
(3.3) | |||
Am Ende der Beschleunigung ergibt sich demnach:
Kann nicht erreicht werden, dann soll die Beschleunigung stetig durchfahren werden (entspricht dem Dreieckprofil). Dafür muß angepasst werden zu .
(3.4) | |||
(3.5) |
Die zyklisch durchlaufene Profilgenerierung lautet dann:
Folgende Funktionen sind public implementiert:
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 //
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 .
//
#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.
Der Filter kann ein gleitender Mittelwert oder ein nachgebildeter analoger Tiefpaß sein.
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