-
Notifications
You must be signed in to change notification settings - Fork 0
/
calculatePID.java
94 lines (80 loc) · 2.93 KB
/
calculatePID.java
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
/*
* calculate the PID value function
*/
private final void calculatePID() {
double kProportional = this.getProportionalConstant();
double kIntegral = this.getIntegralConstant();
double kDerivative = this.getDerivativeConstant();
//current time counts
long now = Clock.ticks();
double setPt = this.getSetPoint().getValue();
double controlVar = this.getAverageIn().getValue();
boolean fault = false;
if (Double.isNaN(setPt) || Double.isNaN(controlVar)) {
fault = true;
}
if (this.lastExecuteTime == 0L) {
this.lastExecuteTime = now;
}
long delta = now - this.lastExecuteTime;
double deltaSecs = (double) delta / 1000.0D;
double error;
if (!this.getEnabled()) {
error = 0.0D;
syncEffectiveLoad(error);
if (this.getDirection()) {
// this.kPkIconst=this.Kp*this.Ki / 60.0D;
this.errorSum = -(error / this.kPkIconst);
} else {
this.errorSum = error / this.kPkIconst;
}
this.lastExecuteTime = 0L;
} else if(!fault){
error = this.getSetPoint().getValue() - this.getInPoint().getValue();
double proportionalGain;
if(kIntegral != 0.0D){
proportionalGain = deltaSecs*error;
this.errorSum += proportionalGain;
this.kPkIconst = kProportional * kIntegral / 60.0D;
if(this.getDirection()){
if(-this.errorSum > maximumOutput /this.kPkIconst){
this.errorSum = -(maximumOutput / this.kPkIconst);
}else if(-this.errorSum < minimumOutput / this.kPkIconst){
this.errorSum = -(minimumOutput / this.kPkIconst);
}
}else if(this.errorSum > maximumOutput / this.kPkIconst){
this.errorSum = maximumOutput / this.kPkIconst;
}else if(this.errorSum < minimumOutput / this.kPkIconst){
this.errorSum = minimumOutput / this.kPkIconst;
}
}
if (Double.isNaN(this.errorSum) || Double.isInfinite(this.errorSum)) {
this.errorSum = 0.0D;
}
proportionalGain = error * kProportional;
double integralGain = 0.0D;
integralGain = kProportional * kIntegral * this.errorSum / 60.0D;
double derivativeGain = kProportional * kDerivative * (error - this.lastError) / deltaSecs;
this.lastError = error;
double pv = proportionalGain + integralGain + derivativeGain;
if(!Double.isNaN(pv)){
if(this.getDirection()){
pv = -pv;
}
if(pv > maximumOutput){
pv = maximumOutput;
}else if(pv < minimumOutput){
pv = minimumOutput;
}
// synchronize the result value
sync(pv);
this.lastExecuteTime = now;
}
}
}
private long lastExecuteTime;
private double kPkIconst;
private double errorSum=0.0D;
private double lastError = 0.0D;
private double maximumOutput = 100.0D;
private double minimumOutput = 0.0D;