Chris H / NUCLEO_F411_HW_LIB

Libs for using Nucleo STM32F411 periphery

Introduction

Descruption: This lib uses the hardware peripherie from STM32F411 under serveral conditions. So you can use an quadraturencoder with different timers.

Requirement: Only tested with the nucleo F411. Include the mbed lib! Interfacing details are explained in the documentary of each class.

Overview

  1. timer modules
    1. Quadratur Encoder (Version 1.2 - C. Hoyer 12.8.2015)
  2. SPI modules
    1. AD5664 (Version 1.1 - C. Hoyer 23.7.2015)
  3. software modules
    1. Ringbuffer (Version 0.9 - C. Hoyer 18.8.2015)
    2. PID-Regler (Version 1.0 - C. Hoyer 17.9.2015)

SWModule/PIDControl.cpp@0:1acdcc576936, 2016-11-28 (annotated)

Committer:
ChrisselH
Date:
Mon Nov 28 17:27:43 2016 +0000
Revision:
0:1acdcc576936
port from priv lib

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ChrisselH 0:1acdcc576936 1 #include "mbed.h"
ChrisselH 0:1acdcc576936 2 #include "PIDControl.h"
ChrisselH 0:1acdcc576936 3
ChrisselH 0:1acdcc576936 4 //====================================================================================================================================
ChrisselH 0:1acdcc576936 5 // Konstruktor und Regelprozess
ChrisselH 0:1acdcc576936 6 //====================================================================================================================================
ChrisselH 0:1acdcc576936 7
ChrisselH 0:1acdcc576936 8 PIDControl::PIDControl(bool P_usr, float KP_usr, bool I_usr, float KI_usr, bool D_usr, float KD_usr){
ChrisselH 0:1acdcc576936 9
ChrisselH 0:1acdcc576936 10 awu_gain = 0; // Anti-Windup deaktivieren
ChrisselH 0:1acdcc576936 11 u_max = 32760; // Maximale Stellgröße limitiert auf max. Integer Wert
ChrisselH 0:1acdcc576936 12 u_min = -32760; // Minimale Stellgröße limitiert auf min. Integer Wert
ChrisselH 0:1acdcc576936 13 lockctrl = false; // Regler frei parametrierbar
ChrisselH 0:1acdcc576936 14
ChrisselH 0:1acdcc576936 15 e_last = 0; // Letzter Regelfehler ist 0
ChrisselH 0:1acdcc576936 16 e_sum = 0; // Integralteil ist 0
ChrisselH 0:1acdcc576936 17 u = 0; // Führungsgröße ist 0
ChrisselH 0:1acdcc576936 18 awu_value = 0; // Atin-Windup größe ist 0
ChrisselH 0:1acdcc576936 19 status = 0; // Statusregister bisher nicht gefüllt
ChrisselH 0:1acdcc576936 20 extfct = false; // Keine Externe Funktion festgelegt
ChrisselH 0:1acdcc576936 21
ChrisselH 0:1acdcc576936 22 P = P_usr; // P-Anteil on/off
ChrisselH 0:1acdcc576936 23 KP = KP_usr; // P-Anteil Verstärkung
ChrisselH 0:1acdcc576936 24 I = I_usr; // I-Anteil on/off
ChrisselH 0:1acdcc576936 25 KI = KI_usr; // I-Anteil Verstärkung
ChrisselH 0:1acdcc576936 26 D = D_usr; // D-Anteil on/off
ChrisselH 0:1acdcc576936 27 KD = KD_usr; // D-Anteil Verstärkung
ChrisselH 0:1acdcc576936 28
ChrisselH 0:1acdcc576936 29 }
ChrisselH 0:1acdcc576936 30
ChrisselH 0:1acdcc576936 31
ChrisselH 0:1acdcc576936 32 int16_t PIDControl::ctrltask (int16_t wsoll, int16_t yist, uint16_t time){
ChrisselH 0:1acdcc576936 33
ChrisselH 0:1acdcc576936 34 setStatus(0); // Regeltask aktiv - Flag aktivieren
ChrisselH 0:1acdcc576936 35
ChrisselH 0:1acdcc576936 36 int16_t e = wsoll - yist; // Regelfehler bestimmen
ChrisselH 0:1acdcc576936 37 e_sum = e_sum + e; // Integralanteil bestimmen
ChrisselH 0:1acdcc576936 38
ChrisselH 0:1acdcc576936 39 if(e > 0){ // Regelfehler positiv
ChrisselH 0:1acdcc576936 40 setStatus(4); // Regelfehler positiv - Flag aktivieren
ChrisselH 0:1acdcc576936 41 resetStatus(3); // Regelfehler negativ - Flag deaktivieren
ChrisselH 0:1acdcc576936 42 resetStatus(2); // Regelfehler null - Flag deaktivieren
ChrisselH 0:1acdcc576936 43 }
ChrisselH 0:1acdcc576936 44
ChrisselH 0:1acdcc576936 45 if(e < 0){ // Regelfehler negativ
ChrisselH 0:1acdcc576936 46 resetStatus(4); // Regelfehler positiv - Flag deaktivieren
ChrisselH 0:1acdcc576936 47 setStatus(3); // Regelfehler negativ - Flag aktivieren
ChrisselH 0:1acdcc576936 48 resetStatus(2); // Regelfehler null - Flag deaktivieren
ChrisselH 0:1acdcc576936 49 }
ChrisselH 0:1acdcc576936 50
ChrisselH 0:1acdcc576936 51 if(e == 0){ // Regelfehler null
ChrisselH 0:1acdcc576936 52 resetStatus(4); // Regelfehler positiv - Flag deaktivieren
ChrisselH 0:1acdcc576936 53 resetStatus(3); // Regelfehler negativ - Flag deaktivieren
ChrisselH 0:1acdcc576936 54 setStatus(2); // Regelfehler null - Flag aktivieren
ChrisselH 0:1acdcc576936 55 }
ChrisselH 0:1acdcc576936 56
ChrisselH 0:1acdcc576936 57 int16_t divisor = log2(time); // Bestimmung der Zeller um die das geschoben werden soll
ChrisselH 0:1acdcc576936 58
ChrisselH 0:1acdcc576936 59 if(P){ // P-Anteil bestimmen
ChrisselH 0:1acdcc576936 60 P_var = (KP * e); // u = Kp * e
ChrisselH 0:1acdcc576936 61 }
ChrisselH 0:1acdcc576936 62 else{
ChrisselH 0:1acdcc576936 63 P_var = 0; // kein P-Anteil vorhanden
ChrisselH 0:1acdcc576936 64 }
ChrisselH 0:1acdcc576936 65
ChrisselH 0:1acdcc576936 66 if(D){ // D-Anteil bestimmen
ChrisselH 0:1acdcc576936 67 D_var = KD * time * (e - e_last); // u = Kd * Ta * (e - e_last) + P-Anteil
ChrisselH 0:1acdcc576936 68 D_var = D_var << 10; // Zeitanpassung, da eingabe in µs
ChrisselH 0:1acdcc576936 69 }
ChrisselH 0:1acdcc576936 70 else{
ChrisselH 0:1acdcc576936 71 D_var = 0; // kein D-Anteil vorhanden
ChrisselH 0:1acdcc576936 72 }
ChrisselH 0:1acdcc576936 73
ChrisselH 0:1acdcc576936 74 if(I){ // I-Anteil bestimmen
ChrisselH 0:1acdcc576936 75 I_var = (KI * (e_sum - awu_value)); // u = Ki / Ta * (e_sum - awu_value) + P-Anteil + D-Anteil
ChrisselH 0:1acdcc576936 76 I_var = I_var << divisor; // Division durch die Zeit
ChrisselH 0:1acdcc576936 77 }
ChrisselH 0:1acdcc576936 78 else{
ChrisselH 0:1acdcc576936 79 I_var = 0; // kein I-Anteil vorhanden
ChrisselH 0:1acdcc576936 80 }
ChrisselH 0:1acdcc576936 81
ChrisselH 0:1acdcc576936 82 resetStatus(1); // I-Anteil an Grenze geraten
ChrisselH 0:1acdcc576936 83
ChrisselH 0:1acdcc576936 84 if(integrallimiter){ // Limiteriung des I-Anteils erforderlich?
ChrisselH 0:1acdcc576936 85 if(e_sum >= i_max){ // Limitierung der Integralsumme zur oberen Schwelle
ChrisselH 0:1acdcc576936 86 setStatus(1); // Fehler I-Anteil über Grenzwerte
ChrisselH 0:1acdcc576936 87 e_sum = i_max; // Limitierung der Summe
ChrisselH 0:1acdcc576936 88 awu_value = (e_sum - i_max) * awu_gain; // Anti-Windup Berechnung
ChrisselH 0:1acdcc576936 89 }
ChrisselH 0:1acdcc576936 90
ChrisselH 0:1acdcc576936 91 if(e_sum <= i_min){ // Limitierung der Integralsumme zur unteren Schwelle
ChrisselH 0:1acdcc576936 92 setStatus(1); // Fehler I-Anteil über Grenzwerte
ChrisselH 0:1acdcc576936 93 e_sum = i_min; // Limitierung der Summe
ChrisselH 0:1acdcc576936 94 awu_value = (e_sum - i_min) * awu_gain; // Anti-Windup Berechnung
ChrisselH 0:1acdcc576936 95 }
ChrisselH 0:1acdcc576936 96 }
ChrisselH 0:1acdcc576936 97
ChrisselH 0:1acdcc576936 98 u = P_var + D_var + I_var; // Reglerausgang definieren
ChrisselH 0:1acdcc576936 99
ChrisselH 0:1acdcc576936 100 resetStatus(6); // Regelgrenze max überschritten zurücksetzen
ChrisselH 0:1acdcc576936 101 resetStatus(7); // Regelgrenze min überschritten zurücksetzen
ChrisselH 0:1acdcc576936 102
ChrisselH 0:1acdcc576936 103 if(u >= u_max){ // Limitierung des Regelausgangs zur oberen Schwelle
ChrisselH 0:1acdcc576936 104 setStatus(7); // Fehler Regelgrenze max überschritten
ChrisselH 0:1acdcc576936 105 u = u_max; // Ausgabe des Stellgrößen Limits
ChrisselH 0:1acdcc576936 106 }
ChrisselH 0:1acdcc576936 107
ChrisselH 0:1acdcc576936 108 if(u <= u_min){ // Limitierung des Regelausgangs zur unteren Schwelle
ChrisselH 0:1acdcc576936 109 setStatus(6); // Fehler Regelgrenze min überschritten
ChrisselH 0:1acdcc576936 110 u = u_min; // Ausgabe des Stellgrößen Limits
ChrisselH 0:1acdcc576936 111 }
ChrisselH 0:1acdcc576936 112
ChrisselH 0:1acdcc576936 113 e = e_last; // Regelfehler als letzten Regelfehler übernehmen
ChrisselH 0:1acdcc576936 114 resetStatus(0); // Regeltask aktiv - Flag deaktivieren
ChrisselH 0:1acdcc576936 115
ChrisselH 0:1acdcc576936 116 return u; // Ausgabe der Stellgröße
ChrisselH 0:1acdcc576936 117
ChrisselH 0:1acdcc576936 118 }
ChrisselH 0:1acdcc576936 119
ChrisselH 0:1acdcc576936 120 //====================================================================================================================================
ChrisselH 0:1acdcc576936 121 // Setter und Getter Funktionen
ChrisselH 0:1acdcc576936 122 //====================================================================================================================================
ChrisselH 0:1acdcc576936 123
ChrisselH 0:1acdcc576936 124 bool PIDControl::setKP(bool P_usr, float KP_usr){ // Setzen des P-Anteils
ChrisselH 0:1acdcc576936 125
ChrisselH 0:1acdcc576936 126 if(!lockctrl){
ChrisselH 0:1acdcc576936 127 P = P_usr; // P-Anteil on/off
ChrisselH 0:1acdcc576936 128 KP = KP_usr; // P-Anteil Verstärkung
ChrisselH 0:1acdcc576936 129
ChrisselH 0:1acdcc576936 130 return true; // Setzen der Werte erfolgreich
ChrisselH 0:1acdcc576936 131 }
ChrisselH 0:1acdcc576936 132 else{
ChrisselH 0:1acdcc576936 133 return false; // Setzen der Werte nicht erfolgreich
ChrisselH 0:1acdcc576936 134 }
ChrisselH 0:1acdcc576936 135 }
ChrisselH 0:1acdcc576936 136
ChrisselH 0:1acdcc576936 137
ChrisselH 0:1acdcc576936 138 bool PIDControl::setKI(bool I_usr, float KI_usr){ // Setzen des I-Anteils
ChrisselH 0:1acdcc576936 139
ChrisselH 0:1acdcc576936 140 if(!lockctrl){
ChrisselH 0:1acdcc576936 141 I = I_usr; // I-Anteil on/off
ChrisselH 0:1acdcc576936 142 KI = KI_usr; // I-Anteil Verstärkung
ChrisselH 0:1acdcc576936 143
ChrisselH 0:1acdcc576936 144 return true; // Setzen der Werte erfolgreich
ChrisselH 0:1acdcc576936 145 }
ChrisselH 0:1acdcc576936 146 else{
ChrisselH 0:1acdcc576936 147 return false; // Setzen der Werte nicht erfolgreich
ChrisselH 0:1acdcc576936 148 }
ChrisselH 0:1acdcc576936 149 }
ChrisselH 0:1acdcc576936 150
ChrisselH 0:1acdcc576936 151
ChrisselH 0:1acdcc576936 152 bool PIDControl::setKD(bool D_usr, float KD_usr){ // Setzen des D-Anteils
ChrisselH 0:1acdcc576936 153
ChrisselH 0:1acdcc576936 154 if(!lockctrl){
ChrisselH 0:1acdcc576936 155 D = D_usr; // D-Anteil on/off
ChrisselH 0:1acdcc576936 156 KD = KD_usr; // D-Anteil Verstärkung
ChrisselH 0:1acdcc576936 157
ChrisselH 0:1acdcc576936 158 return true; // Setzen der Werte erfolgreich
ChrisselH 0:1acdcc576936 159 }
ChrisselH 0:1acdcc576936 160 else{
ChrisselH 0:1acdcc576936 161 return false; // Setzen der Werte nicht erfolgreich
ChrisselH 0:1acdcc576936 162 }
ChrisselH 0:1acdcc576936 163 }
ChrisselH 0:1acdcc576936 164
ChrisselH 0:1acdcc576936 165
ChrisselH 0:1acdcc576936 166 bool PIDControl::setAWU(float awu_gain_usr){ // Setzen des Anti-Windups
ChrisselH 0:1acdcc576936 167
ChrisselH 0:1acdcc576936 168 if(!lockctrl){
ChrisselH 0:1acdcc576936 169 awu_gain = awu_gain_usr; // Anti-Windup Verstärkung
ChrisselH 0:1acdcc576936 170
ChrisselH 0:1acdcc576936 171 return true; // Setzen des Wertes erfolgreich
ChrisselH 0:1acdcc576936 172 }
ChrisselH 0:1acdcc576936 173 else{
ChrisselH 0:1acdcc576936 174 return false; // Setzen des Wertes nicht erfolgreich
ChrisselH 0:1acdcc576936 175 }
ChrisselH 0:1acdcc576936 176 }
ChrisselH 0:1acdcc576936 177
ChrisselH 0:1acdcc576936 178 void PIDControl::setIlimits(bool integrallimiter_usr, int16_t i_max_usr, int16_t i_min_usr){ // Setzen des Integrallimits
ChrisselH 0:1acdcc576936 179
ChrisselH 0:1acdcc576936 180 if(!lockctrl){
ChrisselH 0:1acdcc576936 181 integrallimiter = integrallimiter_usr; // Integrallimits aktivieren
ChrisselH 0:1acdcc576936 182 i_max = i_max_usr; // maximaler I-Anteil
ChrisselH 0:1acdcc576936 183 i_min = i_min_usr; // minimaler I-Anteil
ChrisselH 0:1acdcc576936 184 }
ChrisselH 0:1acdcc576936 185 }
ChrisselH 0:1acdcc576936 186
ChrisselH 0:1acdcc576936 187 void PIDControl::setUlimits(int16_t u_max_usr, int16_t u_min_usr){ // Setzen des Ausgangsgrößenlimits
ChrisselH 0:1acdcc576936 188
ChrisselH 0:1acdcc576936 189 if(!lockctrl){
ChrisselH 0:1acdcc576936 190 u_max = u_max_usr; // maximale Stellgröße
ChrisselH 0:1acdcc576936 191 u_min = u_min_usr; // minimale Stellgröße
ChrisselH 0:1acdcc576936 192 }
ChrisselH 0:1acdcc576936 193 }
ChrisselH 0:1acdcc576936 194
ChrisselH 0:1acdcc576936 195
ChrisselH 0:1acdcc576936 196
ChrisselH 0:1acdcc576936 197 void PIDControl::lock(){
ChrisselH 0:1acdcc576936 198 lockctrl = true; // Einstellung der Regelparamieter sperren
ChrisselH 0:1acdcc576936 199 }
ChrisselH 0:1acdcc576936 200
ChrisselH 0:1acdcc576936 201
ChrisselH 0:1acdcc576936 202 void PIDControl::unlock(){
ChrisselH 0:1acdcc576936 203 lockctrl = false; // Einstellung der Regelparamter freigeben
ChrisselH 0:1acdcc576936 204 }
ChrisselH 0:1acdcc576936 205
ChrisselH 0:1acdcc576936 206
ChrisselH 0:1acdcc576936 207 uint8_t PIDControl::getStatus(){
ChrisselH 0:1acdcc576936 208 return status; // Rückgabe des aktuellen Statusvektors
ChrisselH 0:1acdcc576936 209 }
ChrisselH 0:1acdcc576936 210
ChrisselH 0:1acdcc576936 211
ChrisselH 0:1acdcc576936 212 void PIDControl::setERFFCT(void (*EXTERN_CTRL_HANDLER)(void)){ // Adresse zur externen Funktion übergeben und Freigabe setzen
ChrisselH 0:1acdcc576936 213 extfct = true; // Externe Funktion vorhanden. Freigabe setzen.
ChrisselH 0:1acdcc576936 214 CTRL_HANDLER = EXTERN_CTRL_HANDLER; // Funktionspointer der Funktion übernehmen
ChrisselH 0:1acdcc576936 215 }
ChrisselH 0:1acdcc576936 216
ChrisselH 0:1acdcc576936 217 //====================================================================================================================================
ChrisselH 0:1acdcc576936 218 // Interne Funktionen
ChrisselH 0:1acdcc576936 219 //====================================================================================================================================
ChrisselH 0:1acdcc576936 220
ChrisselH 0:1acdcc576936 221
ChrisselH 0:1acdcc576936 222 int16_t PIDControl::log2(int16_t a){ // Bestimmt die nächstkleinere Zweierpotenz der Zahl a
ChrisselH 0:1acdcc576936 223
ChrisselH 0:1acdcc576936 224 if(a == time_last){
ChrisselH 0:1acdcc576936 225 return time_last_result; // Aktueller Wert entspricht letztem Wert
ChrisselH 0:1acdcc576936 226 }
ChrisselH 0:1acdcc576936 227
ChrisselH 0:1acdcc576936 228 a = time_last; // Speichert aktuellen Wert
ChrisselH 0:1acdcc576936 229
ChrisselH 0:1acdcc576936 230 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};
ChrisselH 0:1acdcc576936 231
ChrisselH 0:1acdcc576936 232 a |= a >> 1; // Runden auf die kleinste Stelle
ChrisselH 0:1acdcc576936 233 a |= a >> 2;
ChrisselH 0:1acdcc576936 234 a |= a >> 4;
ChrisselH 0:1acdcc576936 235 a |= a >> 8;
ChrisselH 0:1acdcc576936 236 a |= a >> 16;
ChrisselH 0:1acdcc576936 237
ChrisselH 0:1acdcc576936 238 time_last_result = DeBruijnFolge[(uint32_t)(a * 0x07C4ACDDU) >> 27]; // Ausgabe der passenden Zweierpotenz
ChrisselH 0:1acdcc576936 239
ChrisselH 0:1acdcc576936 240 return time_last_result;
ChrisselH 0:1acdcc576936 241 }
ChrisselH 0:1acdcc576936 242
ChrisselH 0:1acdcc576936 243
ChrisselH 0:1acdcc576936 244 void PIDControl::setStatus(int bit){
ChrisselH 0:1acdcc576936 245
ChrisselH 0:1acdcc576936 246 status |= (1 << bit); // Setzt das vorher ausgewählte Bit
ChrisselH 0:1acdcc576936 247
ChrisselH 0:1acdcc576936 248 if((extfct) && (bit >= 5 )){ // Wenn externe Funktionen aktiviert sind und ein schwerwiegender Fehler abgelegt wird
ChrisselH 0:1acdcc576936 249 (*CTRL_HANDLER)(); // Aufruf der Externen Funktion
ChrisselH 0:1acdcc576936 250 }
ChrisselH 0:1acdcc576936 251 }
ChrisselH 0:1acdcc576936 252
ChrisselH 0:1acdcc576936 253
ChrisselH 0:1acdcc576936 254 void PIDControl::resetStatus(int bit){
ChrisselH 0:1acdcc576936 255
ChrisselH 0:1acdcc576936 256 int temp = 0xFF; // Maske erstellen
ChrisselH 0:1acdcc576936 257 temp ^= (1 << bit); // Maske anpassen
ChrisselH 0:1acdcc576936 258 status &= temp; // Resetzen des entsprechenden Bits
ChrisselH 0:1acdcc576936 259
ChrisselH 0:1acdcc576936 260 }