public double getDerivativeGain() { return getPDController().getDerivativeGain(); }
private double applyDeadband(double positionError) { if (positionError >= getPositionDeadband()) positionError -= getPositionDeadband(); else if (positionError <= -getPositionDeadband()) positionError += getPositionDeadband(); else positionError = 0.0; return positionError; }
public double getPositionError() { return getPDController().getPositionError(); }
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; }
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 getRateError() { return getPDController().getRateError(); }
public double getProportionalGain() { return getPDController().getProportionalGain(); }
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); }
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(); }
public double getPositionDeadband() { return getPDController().getPositionDeadband(); }