Chris H
Libs for using Nucleo STM32F411 periphery
Embed:
(wiki syntax)
Show/hide line numbers
PIDControl.cpp
00001 #include "mbed.h" 00002 #include "PIDControl.h" 00003 00004 //==================================================================================================================================== 00005 // Konstruktor und Regelprozess 00006 //==================================================================================================================================== 00007 00008 PIDControl::PIDControl (bool P_usr, float KP_usr, bool I_usr, float KI_usr, bool D_usr, float KD_usr){ 00009 00010 awu_gain = 0; // Anti-Windup deaktivieren 00011 u_max = 32760; // Maximale Stellgröße limitiert auf max. Integer Wert 00012 u_min = -32760; // Minimale Stellgröße limitiert auf min. Integer Wert 00013 lockctrl = false; // Regler frei parametrierbar 00014 00015 e_last = 0; // Letzter Regelfehler ist 0 00016 e_sum = 0; // Integralteil ist 0 00017 u = 0; // Führungsgröße ist 0 00018 awu_value = 0; // Atin-Windup größe ist 0 00019 status = 0; // Statusregister bisher nicht gefüllt 00020 extfct = false; // Keine Externe Funktion festgelegt 00021 00022 P = P_usr; // P-Anteil on/off 00023 KP = KP_usr; // P-Anteil Verstärkung 00024 I = I_usr; // I-Anteil on/off 00025 KI = KI_usr; // I-Anteil Verstärkung 00026 D = D_usr; // D-Anteil on/off 00027 KD = KD_usr; // D-Anteil Verstärkung 00028 00029 } 00030 00031 00032 int16_t PIDControl::ctrltask (int16_t wsoll, int16_t yist, uint16_t time){ 00033 00034 setStatus(0); // Regeltask aktiv - Flag aktivieren 00035 00036 int16_t e = wsoll - yist; // Regelfehler bestimmen 00037 e_sum = e_sum + e; // Integralanteil bestimmen 00038 00039 if(e > 0){ // Regelfehler positiv 00040 setStatus(4); // Regelfehler positiv - Flag aktivieren 00041 resetStatus(3); // Regelfehler negativ - Flag deaktivieren 00042 resetStatus(2); // Regelfehler null - Flag deaktivieren 00043 } 00044 00045 if(e < 0){ // Regelfehler negativ 00046 resetStatus(4); // Regelfehler positiv - Flag deaktivieren 00047 setStatus(3); // Regelfehler negativ - Flag aktivieren 00048 resetStatus(2); // Regelfehler null - Flag deaktivieren 00049 } 00050 00051 if(e == 0){ // Regelfehler null 00052 resetStatus(4); // Regelfehler positiv - Flag deaktivieren 00053 resetStatus(3); // Regelfehler negativ - Flag deaktivieren 00054 setStatus(2); // Regelfehler null - Flag aktivieren 00055 } 00056 00057 int16_t divisor = log2(time); // Bestimmung der Zeller um die das geschoben werden soll 00058 00059 if(P){ // P-Anteil bestimmen 00060 P_var = (KP * e); // u = Kp * e 00061 } 00062 else{ 00063 P_var = 0; // kein P-Anteil vorhanden 00064 } 00065 00066 if(D){ // D-Anteil bestimmen 00067 D_var = KD * time * (e - e_last); // u = Kd * Ta * (e - e_last) + P-Anteil 00068 D_var = D_var << 10; // Zeitanpassung, da eingabe in µs 00069 } 00070 else{ 00071 D_var = 0; // kein D-Anteil vorhanden 00072 } 00073 00074 if(I){ // I-Anteil bestimmen 00075 I_var = (KI * (e_sum - awu_value)); // u = Ki / Ta * (e_sum - awu_value) + P-Anteil + D-Anteil 00076 I_var = I_var << divisor; // Division durch die Zeit 00077 } 00078 else{ 00079 I_var = 0; // kein I-Anteil vorhanden 00080 } 00081 00082 resetStatus(1); // I-Anteil an Grenze geraten 00083 00084 if(integrallimiter){ // Limiteriung des I-Anteils erforderlich? 00085 if(e_sum >= i_max){ // Limitierung der Integralsumme zur oberen Schwelle 00086 setStatus(1); // Fehler I-Anteil über Grenzwerte 00087 e_sum = i_max; // Limitierung der Summe 00088 awu_value = (e_sum - i_max) * awu_gain; // Anti-Windup Berechnung 00089 } 00090 00091 if(e_sum <= i_min){ // Limitierung der Integralsumme zur unteren Schwelle 00092 setStatus(1); // Fehler I-Anteil über Grenzwerte 00093 e_sum = i_min; // Limitierung der Summe 00094 awu_value = (e_sum - i_min) * awu_gain; // Anti-Windup Berechnung 00095 } 00096 } 00097 00098 u = P_var + D_var + I_var; // Reglerausgang definieren 00099 00100 resetStatus(6); // Regelgrenze max überschritten zurücksetzen 00101 resetStatus(7); // Regelgrenze min überschritten zurücksetzen 00102 00103 if(u >= u_max){ // Limitierung des Regelausgangs zur oberen Schwelle 00104 setStatus(7); // Fehler Regelgrenze max überschritten 00105 u = u_max; // Ausgabe des Stellgrößen Limits 00106 } 00107 00108 if(u <= u_min){ // Limitierung des Regelausgangs zur unteren Schwelle 00109 setStatus(6); // Fehler Regelgrenze min überschritten 00110 u = u_min; // Ausgabe des Stellgrößen Limits 00111 } 00112 00113 e = e_last; // Regelfehler als letzten Regelfehler übernehmen 00114 resetStatus(0); // Regeltask aktiv - Flag deaktivieren 00115 00116 return u; // Ausgabe der Stellgröße 00117 00118 } 00119 00120 //==================================================================================================================================== 00121 // Setter und Getter Funktionen 00122 //==================================================================================================================================== 00123 00124 bool PIDControl::setKP (bool P_usr, float KP_usr){ // Setzen des P-Anteils 00125 00126 if(!lockctrl){ 00127 P = P_usr; // P-Anteil on/off 00128 KP = KP_usr; // P-Anteil Verstärkung 00129 00130 return true; // Setzen der Werte erfolgreich 00131 } 00132 else{ 00133 return false; // Setzen der Werte nicht erfolgreich 00134 } 00135 } 00136 00137 00138 bool PIDControl::setKI (bool I_usr, float KI_usr){ // Setzen des I-Anteils 00139 00140 if(!lockctrl){ 00141 I = I_usr; // I-Anteil on/off 00142 KI = KI_usr; // I-Anteil Verstärkung 00143 00144 return true; // Setzen der Werte erfolgreich 00145 } 00146 else{ 00147 return false; // Setzen der Werte nicht erfolgreich 00148 } 00149 } 00150 00151 00152 bool PIDControl::setKD (bool D_usr, float KD_usr){ // Setzen des D-Anteils 00153 00154 if(!lockctrl){ 00155 D = D_usr; // D-Anteil on/off 00156 KD = KD_usr; // D-Anteil Verstärkung 00157 00158 return true; // Setzen der Werte erfolgreich 00159 } 00160 else{ 00161 return false; // Setzen der Werte nicht erfolgreich 00162 } 00163 } 00164 00165 00166 bool PIDControl::setAWU (float awu_gain_usr){ // Setzen des Anti-Windups 00167 00168 if(!lockctrl){ 00169 awu_gain = awu_gain_usr; // Anti-Windup Verstärkung 00170 00171 return true; // Setzen des Wertes erfolgreich 00172 } 00173 else{ 00174 return false; // Setzen des Wertes nicht erfolgreich 00175 } 00176 } 00177 00178 void PIDControl::setIlimits (bool integrallimiter_usr, int16_t i_max_usr, int16_t i_min_usr){ // Setzen des Integrallimits 00179 00180 if(!lockctrl){ 00181 integrallimiter = integrallimiter_usr; // Integrallimits aktivieren 00182 i_max = i_max_usr; // maximaler I-Anteil 00183 i_min = i_min_usr; // minimaler I-Anteil 00184 } 00185 } 00186 00187 void PIDControl::setUlimits (int16_t u_max_usr, int16_t u_min_usr){ // Setzen des Ausgangsgrößenlimits 00188 00189 if(!lockctrl){ 00190 u_max = u_max_usr; // maximale Stellgröße 00191 u_min = u_min_usr; // minimale Stellgröße 00192 } 00193 } 00194 00195 00196 00197 void PIDControl::lock (){ 00198 lockctrl = true; // Einstellung der Regelparamieter sperren 00199 } 00200 00201 00202 void PIDControl::unlock (){ 00203 lockctrl = false; // Einstellung der Regelparamter freigeben 00204 } 00205 00206 00207 uint8_t PIDControl::getStatus (){ 00208 return status ; // Rückgabe des aktuellen Statusvektors 00209 } 00210 00211 00212 void PIDControl::setERFFCT (void (*EXTERN_CTRL_HANDLER)(void)){ // Adresse zur externen Funktion übergeben und Freigabe setzen 00213 extfct = true; // Externe Funktion vorhanden. Freigabe setzen. 00214 CTRL_HANDLER = EXTERN_CTRL_HANDLER; // Funktionspointer der Funktion übernehmen 00215 } 00216 00217 //==================================================================================================================================== 00218 // Interne Funktionen 00219 //==================================================================================================================================== 00220 00221 00222 int16_t PIDControl::log2(int16_t a){ // Bestimmt die nächstkleinere Zweierpotenz der Zahl a 00223 00224 if(a == time_last){ 00225 return time_last_result; // Aktueller Wert entspricht letztem Wert 00226 } 00227 00228 a = time_last; // Speichert aktuellen Wert 00229 00230 static const int DeBruijnFolge[32] = { 0, 9, 1, 10, 13, 21, 2, 29, 11, 14, 16, 18, 22, 25, 3, 30, 8, 12, 20, 28, 15, 17, 24, 7, 19, 27, 23, 6, 26, 5, 4, 31}; 00231 00232 a |= a >> 1; // Runden auf die kleinste Stelle 00233 a |= a >> 2; 00234 a |= a >> 4; 00235 a |= a >> 8; 00236 a |= a >> 16; 00237 00238 time_last_result = DeBruijnFolge[(uint32_t)(a * 0x07C4ACDDU) >> 27]; // Ausgabe der passenden Zweierpotenz 00239 00240 return time_last_result; 00241 } 00242 00243 00244 void PIDControl::setStatus(int bit){ 00245 00246 status |= (1 << bit); // Setzt das vorher ausgewählte Bit 00247 00248 if((extfct) && (bit >= 5 )){ // Wenn externe Funktionen aktiviert sind und ein schwerwiegender Fehler abgelegt wird 00249 (*CTRL_HANDLER)(); // Aufruf der Externen Funktion 00250 } 00251 } 00252 00253 00254 void PIDControl::resetStatus(int bit){ 00255 00256 int temp = 0xFF; // Maske erstellen 00257 temp ^= (1 << bit); // Maske anpassen 00258 status &= temp; // Resetzen des entsprechenden Bits 00259 00260 }
Generated on Wed Jul 13 2022 17:06:12 by
1.7.2