public double getRateError() { return getPDController().getRateError(); }
private double computeIntegralEffortAndAddPDEffort(double deltaTime) { double outputSignal = (getPDController().getProportionalGain() * getPDController().getPositionError()) + (getPDController().getDerivativeGain() * getPDController().getRateError()); if (getIntegralGain() < 1.0e-5) { cumulativeError.set(0.0); } else { // LIMIT THE MAX INTEGRAL ERROR SO WON'T WIND UP double maxError = getMaxIntegralError(); double errorAfterLeak = getPDController().getPositionError() * deltaTime + getIntegralLeakRatio() * cumulativeError.getDoubleValue(); cumulativeError.set(MathTools.clamp(errorAfterLeak, maxError)); actionI.set(getIntegralGain() * cumulativeError.getDoubleValue()); outputSignal += actionI.getDoubleValue(); } double maximumOutput = Math.abs(getMaximumFeedback()); outputSignal = MathTools.clamp(outputSignal, maximumOutput); return outputSignal; }