-
Notifications
You must be signed in to change notification settings - Fork 0
/
pid.c
49 lines (39 loc) · 1023 Bytes
/
pid.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
#include "stm32f4xx.h"
#include "pid.h"
void PID_Init(PID_PIDConfig *config)
{
config->errorSum = 0.0F;
config->errorOld = 0.0F;
return;
}
float PID_Control(float ist, float soll, PID_PIDConfig *config)
{
float error = soll - ist;
//Fehler aufsummieren
config->errorSum += error;
//if(config->errorSum >= 100)
// config->errorSum = 100;
//if(config->errorSum <= -100)
// config->errorSum = -100;
//Regleranteile berechnen
float K = config->K*error;
float I = config->I*config->errorSum*config->samplingTime;
float D = config->D*(error - config->errorOld)/config->samplingTime;
//Stellwert berechnen
float stellwert = K + I + D;
//Alten Fehler für den D-Anteil speichern
config->errorOld = error;
//Limitierung des Stellwertes
if(config->activateLimit > 0)
{
if(config->upperLimit < stellwert)
{
stellwert = config->upperLimit;
}
if(config->lowerLimit > stellwert)
{
stellwert = config->lowerLimit;
}
}
return stellwert;
}