public void compute( FrameVector output, FrameOrientation desiredOrientation, FrameVector desiredAngularVelocity, FrameVector currentAngularVelocity, FrameVector feedForward) { computeProportionalTerm(desiredOrientation); if (currentAngularVelocity != null) computeDerivativeTerm(desiredAngularVelocity, currentAngularVelocity); computeIntegralTerm(); output.setToZero(proportionalTerm.getReferenceFrame()); output.add(proportionalTerm); output.add(derivativeTerm); output.add(integralTerm); // Limit the max acceleration of the feedback, but not of the feedforward... // JEP changed 150430 based on Atlas hitting limit stops. double feedbackAngularActionMagnitude = output.length(); double maximumAction = gains.getMaximumFeedback(); if (feedbackAngularActionMagnitude > maximumAction) { output.scale(maximumAction / feedbackAngularActionMagnitude); } feedbackAngularAction.set(output); rateLimitedFeedbackAngularAction.update(); rateLimitedFeedbackAngularAction.getFrameTuple(output); feedForward.changeFrame(bodyFrame); output.add(feedForward); }