-
Notifications
You must be signed in to change notification settings - Fork 0
/
pid.c
131 lines (94 loc) · 2.5 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
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
#include "pid.h"
PID_ATLAB::PID_ATLAB(double Kp, double Ki, double Kd, double Min, double Max)
{
reset();
PID_ATLAB::SetOutputLimits(Min, Max);
PID_ATLAB::SetTunings(Kp, Ki, Kd);
}
double PID_ATLAB::Compute(double input, double zero, double Setpoint)
{
unsigned long now = millis();
SampleTime = (now - lastTime);
/*Compute all the working error variables*/
error = (Setpoint + zero) - input;
SampleTimeInSec = ((double)SampleTime)/1000;
output = 0;
output += kp * error;
if(abs(ki)>0&&SampleTimeInSec>0)
{
ITerm+= (ki * error*SampleTimeInSec);
ITerm=constrain(ITerm,outMin,outMax);
output+=ITerm;
}
if(abs(kd)>0&&SampleTimeInSec>0)
{
dInput = (input - lastInput);
//dInput = lastDerivative +(SampleTimeInSec / (fCut + SampleTimeInSec)) * ( dInput - lastDerivative);
//lastDerivative = dInput;
derivative=((kd * dInput)/SampleTimeInSec);
output-=derivative;
}
/*Compute PID_ATLAB Output*/
if(output > outMax) output = outMax;
else if(output < outMin) output = outMin;
/*Remember some variables for next time*/
lastInput = input;
lastTime = now;
return output;
}
double PID_ATLAB::ComputeFixedHz(double input, double zero, double sampletime, double Setpoint)
{
unsigned long now = millis();
unsigned long timeChange = (now - lastTime);
if(timeChange>=sampletime)
{
/*Compute all the working error variables*/
error = (Setpoint + zero) - input;
SampleTimeInSec = ((double)sampletime)/1000;
output = 0;
output += kp * error;
if(abs(ki)>0&&SampleTimeInSec>0)
{
ITerm+= (ki * error*SampleTimeInSec);
ITerm=constrain(ITerm,outMin,outMax);
output+=ITerm;
}
if(abs(kd)>0&&SampleTimeInSec>0)
{
dInput = (input - lastInput);
//dInput = lastDerivative +(SampleTimeInSec / (fCut + SampleTimeInSec)) * ( dInput - lastDerivative);
//lastDerivative = dInput;
derivative=((kd * dInput)/SampleTimeInSec);
output-=derivative;
}
if(output > outMax) output = outMax;
else if(output < outMin) output = outMin;
/*Remember some variables for next time*/
lastInput = input;
lastTime = now;
}
return output;
}
void PID_ATLAB::SetTunings(double Kp, double Ki, double Kd)
{
kp = Kp;
ki = Ki;
kd = Kd;
}
void PID_ATLAB::SetOutputLimits(double Min, double Max)
{
if(Min >= Max) return;
outMin = Min;
outMax = Max;
}
void PID_ATLAB::reset()
{
ITerm=0.00;
lastInput=0.00;
output=0.00;
error=0.00;
SampleTimeInSec=0.00;
dInput=0.00;
lastDerivative=0.00;
lastTime=millis();
}