Example Wrist System

public class WristSubsystem extends SubsystemBase {
    private final TalonFX wristMotor;
    private final CANCoder cancoder;
    private double wristHoldingAngle = 0;
    private double forwardLimit = WristConstants.forwardSoftLimit;
    private double revLimit = WristConstants.revSoftLimit;
    private DoubleSupplier liftPos;
    private DoubleSupplier liftWantedPos;
    private PIDController WristPID = new PIDController(WristConstants.WristPID[0], WristConstants.WristPID[1], WristConstants.WristPID[2]);

    public WristSubsystem(DoubleSupplier liftPos, DoubleSupplier liftWantedPos) {
        wristMotor = new TalonFX(WristConstants.wristId, "canivore");
        cancoder = new CANCoder(WristConstants.cancoderId, "canivore");
        configDevice();
        setHoldingAngle(forwardLimit);
    }

    private void configDevice() {
        wristMotor.configFactoryDefault();
        cancoder.configFactoryDefault();
        setNeutralMode(NeutralMode.Brake);
        cancoder.configMagnetOffset(WristConstants.cancoderOffset);
        wristMotor.setInverted(true);
        wristMotor.configPeakOutputForward(0.3);
        wristMotor.configPeakOutputReverse(-0.25);
    } 

    public void setNeutralMode(NeutralMode mode) {
        wristMotor.setNeutralMode(mode);
    }

    public double getAbsolutePosition() {
        return cancoder.getAbsolutePosition();
    }

    public void stop() {
        wristMotor.set(ControlMode.PercentOutput, 0);
    }

    public void goToAngle(double position) {
        update(position);
    }

    public void holdPosition() {
        update(wristHoldingAngle);
    }

    private void update(double setPoint) {
        double clampedSetPoint = MathUtil.clamp(setPoint, revLimit, forwardLimit);
        double output = WristPID.calculate(cancoder.getAbsolutePosition(), clampedSetPoint);
        wristMotor.set(ControlMode.PercentOutput, output);
    }

    public boolean isAtPosition() {
        return WristPID.atSetpoint();
    }

    public void setHoldingAngle(double position) {
        wristHoldingAngle = position;
    }

    public void run(double percentOut) {
        wristMotor.set(ControlMode.PercentOutput, percentOut);
    }

    public void resetPID() {
        WristPID.reset();
    }

    boolean avoidingFloor = false;
    @Override
    public void periodic() {
        if(wristMotor.getMotorOutputPercent() > 0 ) {
            if(cancoder.getAbsolutePosition() >= forwardLimit) {
                stop();
            }
        } else if(wristMotor.getMotorOutputPercent() < 0) {
            if(cancoder.getAbsolutePosition() <= revLimit) {
                stop();
            }
        }
    }
}