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(); }