-
Notifications
You must be signed in to change notification settings - Fork 1
/
pid.c
88 lines (68 loc) · 1.8 KB
/
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
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
#include "pid.h"
void PID_Init(ptrPIDdata pPd) {
pPd->setpoint = 0.0f;
pPd->Kp = 0.0f;
pPd->Ki = 0.0f;
pPd->Kd = 0.0f;
pPd->Perr = 0.0f;
pPd->Ierr = 0.0f;
pPd->Derr = 0.0f;
pPd->Perrmin = -1000.0f;
pPd->Perrmax = 1000.0f;
pPd->Ierrmin = -1000.0f;
pPd->Ierrmax = 1000.0f;
}
/*
* set pid terms
*/
void PID_SetPID(ptrPIDdata pPd, float setpoint, float pidP, float pidI, float pidD) {
pPd->setpoint = setpoint;
pPd->Kp = pidP;
pPd->Ki = pidI;
pPd->Kd = pidD;
}
/*
* set P term limits
*/
void PID_SetLimitsPerr(ptrPIDdata pPd, float Perr_min, float Perr_max) {
pPd->Perrmin = Perr_min;
pPd->Perrmax = Perr_max;
}
/*
* set I term limits
*/
void PID_SetLimitsIerr(ptrPIDdata pPd, float Ierr_min, float Ierr_max) {
pPd->Ierrmin = Ierr_min;
pPd->Ierrmax = Ierr_max;
}
/*
* reset I term limit
*/
void PID_ResetIerr(ptrPIDdata pPd) {
pPd->Ierr = 0;
}
/*
* pid control algorithm
*/
float PID_Update(ptrPIDdata pPd, float input) {
//if this function get called always at the same period, dt = 1 can be used
//otherwise dt should be calculated
static float inputprev = 0;
//compute P error
pPd->Perr = pPd->setpoint - input;
if (pPd->Perr < pPd->Perrmin)
pPd->Perr = pPd->Perrmin;
else if (pPd->Perr > pPd->Perrmax)
pPd->Perr = pPd->Perrmax;
//compute I error
pPd->Ierr += pPd->Perr;
if (pPd->Ierr < pPd->Ierrmin)
pPd->Ierr = pPd->Ierrmin;
else if (pPd->Ierr > pPd->Ierrmax)
pPd->Ierr = pPd->Ierrmax;
//compute D error
pPd->Derr = inputprev - input;
//record last value
inputprev = input;
return ((pPd->Kp*pPd->Perr) + (pPd->Ki*pPd->Ierr) + (pPd->Kd*pPd->Derr));
}