-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy pathPID.h
78 lines (62 loc) · 2.13 KB
/
PID.h
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
#ifndef PID_
#define PID_
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
typedef struct{
float set_point;
float KP;
float KD;
float KI;
float output;
float er;
float pre_er;
float pre_pre_er;
float p_er;
float p_pre_er;
float p_pre_pre_er;
float sampling_time;
float P_term;
float I_term;
float D_term;
float I_limit;
float fb;//feedback
float pre_fb;
float pre_pre_fb;
}PID;
void PID_init();
void PID_controller(float feeback, PID *ptr_pid)
{
ptr_pid->p_er=(ptr_pid->KP)+(ptr_pid->KI)*(ptr_pid->sampling_time)/(2.0)+(ptr_pid->KD)/(ptr_pid->sampling_time);
ptr_pid->p_pre_er=-(ptr_pid->KP)+(ptr_pid->KI)*(ptr_pid->sampling_time)/(2.0)-(2.0)*(ptr_pid->KD)/(ptr_pid->sampling_time);
ptr_pid->p_pre_pre_er=(ptr_pid->KD)/(ptr_pid->sampling_time);
ptr_pid->er=ptr_pid->set_point-feeback;
ptr_pid->output += ptr_pid->er*ptr_pid->p_er + ptr_pid->pre_er*ptr_pid->p_pre_er + ptr_pid->pre_pre_er*ptr_pid->p_pre_pre_er;
ptr_pid->pre_pre_er=ptr_pid->pre_er;
ptr_pid->pre_er=ptr_pid->er;
}
void PD_controller(float feeback, PID *ptr_pid)
{
ptr_pid->p_er=(ptr_pid->KP)+(ptr_pid->KD)/(ptr_pid->sampling_time);
ptr_pid->p_pre_er=-(ptr_pid->KP)-(2.0)*(ptr_pid->KD)/(ptr_pid->sampling_time);
ptr_pid->p_pre_pre_er=(ptr_pid->KD)/(ptr_pid->sampling_time);
ptr_pid->er=ptr_pid->set_point-feeback;
ptr_pid->output += ptr_pid->er*ptr_pid->p_er + ptr_pid->pre_er*ptr_pid->p_pre_er + ptr_pid->pre_pre_er*ptr_pid->p_pre_pre_er;
ptr_pid->pre_pre_er=ptr_pid->pre_er;
ptr_pid->pre_er=ptr_pid->er;
}
void PID_type_3(float feedback, PID *ptr_pid)
{
ptr_pid->er=ptr_pid->set_point - feedback;
ptr_pid->fb=feedback;
ptr_pid->P_term=ptr_pid->KP*ptr_pid->er;
// if(ptr_pid->er==0)
// ptr_pid->I_term=0;
// else{
ptr_pid->I_term += ptr_pid->KI*ptr_pid->sampling_time*(ptr_pid->er+ptr_pid->pre_er)/2.0;
ptr_pid->I_term=constrain(ptr_pid->I_term,-ptr_pid->I_limit,ptr_pid->I_limit);
// }
ptr_pid->D_term = ptr_pid->KD*(-ptr_pid->fb + ptr_pid->pre_fb)/ptr_pid->sampling_time;
ptr_pid->pre_er=ptr_pid->er;
ptr_pid->pre_fb=ptr_pid->fb;
ptr_pid->output=ptr_pid->P_term+ptr_pid->I_term+ptr_pid->D_term;
}
#endif