Example Elevator System

public class LiftSubsystem extends SubsystemBase {
    private final TalonFX leftMotor;
    private final TalonFX rightMotor;

    private final PositionVoltage positionVoltage;
    private final DutyCycleOut dutyCycleOut;
    private final Follower follower;

    private double wantedPosition;
    private Debouncer positionDebouncer = new Debouncer(0.1, DebounceType.kRising);


    public LiftSubsystem() {
        leftMotor = new TalonFX(ElevatorConstants.leftMotorId);
        rightMotor = new TalonFX(ElevatorConstants.rightMotorId);
        positionVoltage = new PositionVoltage(0);
        dutyCycleOut = new DutyCycleOut(0);
        follower = new Follower(ElevatorConstants.leftMotorId, true);

        TalonFxUtils.applyDefaultConfigs(leftMotor);
        TalonFxUtils.applyDefaultConfigs(rightMotor);

        TalonFXConfigurator leftConfigurator = leftMotor.getConfigurator();
        TalonFxUtils.configSoftLimits(leftMotor, ElevatorConstants.softLimit, 0, true);

        TalonFXConfiguration configs = new TalonFXConfiguration();

        Slot0Configs pidConfigs = configs.Slot0;
        pidConfigs.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign;
        pidConfigs.kP = ElevatorConstants.Gains.kP;
        pidConfigs.kI = ElevatorConstants.Gains.kI;
        pidConfigs.kD = ElevatorConstants.Gains.kD;
        pidConfigs.kS = ElevatorConstants.Gains.kS;
        pidConfigs.kG = ElevatorConstants.Gains.kG;
        pidConfigs.GravityType = GravityTypeValue.Elevator_Static;

        leftConfigurator.apply(configs);

        leftMotor.setNeutralMode(NeutralModeValue.Brake);
        rightMotor.setNeutralMode(NeutralModeValue.Brake);
    }

   public Command goToPosition(DoubleSupplier position) {
        return Commands.run(() -> {
            wantedPosition = position.getAsDouble();
            leftMotor.setControl(positionVoltage.withPosition(position));
            rightMotor.setControl(follower);
        }, this);
    }

    public Command home() {
        return goToPosition(() -> 0);
    }

    public void stop() {
        leftMotor.stopMotor();
        rightMotor.stopMotor();
    }
}