public double getPositionError() { return getPDController().getPositionError(); }
public double getRateError() { return getPDController().getRateError(); }
public double getProportionalGain() { return getPDController().getProportionalGain(); }
public double getDerivativeGain() { return getPDController().getDerivativeGain(); }
public double getPositionDeadband() { return getPDController().getPositionDeadband(); }
public double computeForAngles(double currentPosition, double desiredPosition, double currentRate, double desiredRate, double deltaTime) { getPDController().computeForAngles(currentPosition, desiredPosition, currentRate, desiredRate); return computeIntegralEffortAndAddPDEffort(deltaTime); }
public double compute(double currentPosition, double desiredPosition, double currentRate, double desiredRate, double deltaTime) { getPDController().compute(currentPosition, desiredPosition, currentRate, desiredRate); return computeIntegralEffortAndAddPDEffort(deltaTime); }
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; }