public double getProportionalGain() { return getPDController().getProportionalGain(); }
public double compute(double currentPosition, double desiredPosition, double currentRate, double desiredRate) { positionError.set(applyDeadband(desiredPosition - currentPosition)); rateError.set(desiredRate - currentRate); actionP.set(getProportionalGain() * positionError.getDoubleValue()); actionD.set(getDerivativeGain() * rateError.getDoubleValue()); return actionP.getDoubleValue() + actionD.getDoubleValue(); }
public double computeForAngles(double currentPosition, double desiredPosition, double currentRate, double desiredRate) { // System.out.println("PGain: " + proportionalGain.getDoubleValue() + "DGain: " + derivativeGain.getDoubleValue()); this.positionError.set(applyDeadband(AngleTools.computeAngleDifferenceMinusPiToPi(desiredPosition, currentPosition))); rateError.set(desiredRate - currentRate); actionP.set(getProportionalGain() * positionError.getDoubleValue()); actionD.set(getDerivativeGain() * rateError.getDoubleValue()); return actionP.getDoubleValue() + actionD.getDoubleValue(); }
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; }