|
@@ -123,14 +123,8 @@ public class PidLambdaContainer {
|
|
conversion=1d;
|
|
conversion=1d;
|
|
}
|
|
}
|
|
latestRealCPA=realCost/conversion;
|
|
latestRealCPA=realCost/conversion;
|
|
- et=cpa-latestRealCPA;
|
|
|
|
- sumE=cacheItem.sumError+et;
|
|
|
|
- ve=et-(cpa-cacheItem.latestRealCpa);
|
|
|
|
- lambdaNew=kp*et+ki*sumE+kd*ve;
|
|
|
|
|
|
+ lambdaNew =cacheItem.calculate(kp,ki,kd,cpa,latestRealCPA);
|
|
|
|
|
|
- if(cpa+lambdaNew<=1) {
|
|
|
|
- lambdaNew=cpa-1;
|
|
|
|
- }
|
|
|
|
cacheItem.lambda=lambdaNew;
|
|
cacheItem.lambda=lambdaNew;
|
|
cacheItem.latestRealCpa=latestRealCPA;
|
|
cacheItem.latestRealCpa=latestRealCPA;
|
|
cacheItem.sumError=sumE;
|
|
cacheItem.sumError=sumE;
|
|
@@ -214,5 +208,25 @@ public class PidLambdaContainer {
|
|
public double sumError=0d;
|
|
public double sumError=0d;
|
|
|
|
|
|
public double latestRealCpa=0d;
|
|
public double latestRealCpa=0d;
|
|
|
|
+
|
|
|
|
+ private double processVariable; // 处理变量
|
|
|
|
+ private double integral; // 积分项
|
|
|
|
+ private double lastError; // 上一个误差
|
|
|
|
+
|
|
|
|
+ public double calculate(double kp, double ki, double kd, double setPoint,double currentValue) {
|
|
|
|
+ processVariable = currentValue;
|
|
|
|
+ double error = setPoint - processVariable;
|
|
|
|
+
|
|
|
|
+ integral += error;
|
|
|
|
+ double derivative = (error - lastError) / 1; // 假设采样间隔为1
|
|
|
|
+ lastError = error;
|
|
|
|
+ return kp * error + ki * integral + kd * derivative;
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+
|
|
|
|
+ public void reset() {
|
|
|
|
+ integral = 0;
|
|
|
|
+ lastError = 0;
|
|
|
|
+ }
|
|
}
|
|
}
|
|
}
|
|
}
|