Add absolute encoder to climber

simgui.json

@@ -6,6 +6,12 @@
     },
     "windows": {
       "/SmartDashboard/Field": {
+        "bottom": 1476,
+        "height": 8.210550308227539,
+        "left": 150,
+        "right": 2961,
+        "top": 79,
+        "width": 16.541748046875,
         "window": {
           "visible": true
         }

src/main/java/frc/robot/Constants.java

@@ -40,6 +40,7 @@ public final class Constants {
 
   public static class constClimber {
     public static final NeutralModeValue CLIMBER_NEUTRAL_MODE = NeutralModeValue.Brake;
+    public static final boolean ABS_ENCODER_INVERT = false;
   }
 
   public static class constControllers {

src/main/java/frc/robot/RobotMap.java

@@ -14,6 +14,7 @@ public static class mapControllers {
   // MOTORS: 30 -> 39
   public static class mapClimber {
     public static final int CLIMBER_MOTOR_CAN = 30;
+    public static final int CLIMBER_ABSOLUTE_ENCODER_DIO = 2;
   }
 
   // MOTORS: 50 -> 59

src/main/java/frc/robot/RobotPreferences.java

@@ -33,6 +33,8 @@ public static final class climberPref {
         "climberMotorUpSpeed", 1);
     public static final SN_DoublePreference climberMotorDownSpeed = new SN_DoublePreference(
         "climberMotorDownSpeed", -1);
+    public static final SN_BooleanPreference climberInverted = new SN_BooleanPreference("climberInverted", false);
+
     public static final SN_DoublePreference climberS = new SN_DoublePreference("climberS", 0);
     public static final SN_DoublePreference climberV = new SN_DoublePreference("climberV", 0.12);
     public static final SN_DoublePreference climberP = new SN_DoublePreference("climberP", 0.3);

src/main/java/frc/robot/subsystems/Climber.java

@@ -7,22 +7,27 @@
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.hardware.TalonFX;
 import frc.robot.Constants.constClimber;
-
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.RobotMap.mapClimber;
 import frc.robot.RobotPreferences.climberPref;
 
 import com.ctre.phoenix6.controls.NeutralOut;
+import com.ctre.phoenix6.controls.VoltageOut;
 
 public class Climber extends SubsystemBase {
   TalonFX climberMotor;
 
+  double absoluteEncoderOffset;
+  VoltageOut voltageRequest;
   TalonFXConfiguration climberConfig;
+  DutyCycleEncoder absoluteEncoder;
 
   public Climber() {
     climberMotor = new TalonFX(mapClimber.CLIMBER_MOTOR_CAN, "rio");
-
+    absoluteEncoder = new DutyCycleEncoder(mapClimber.CLIMBER_ABSOLUTE_ENCODER_DIO);
     climberConfig = new TalonFXConfiguration();
     configure();
   }
@@ -58,8 +63,41 @@ public void setNeutralOutput() {
     climberMotor.setControl(new NeutralOut());
   }
 
+  public void setClimberSoftwareLimits(boolean reverse, boolean forward) {
+    climberConfig.SoftwareLimitSwitch.ReverseSoftLimitEnable = reverse;
+    climberConfig.SoftwareLimitSwitch.ForwardSoftLimitEnable = forward;
+    climberMotor.getConfigurator().apply(climberConfig);
+    climberMotor.setInverted(climberPref.climberInverted.getValue());
+  }
+
+  public void setClimberVoltage(double voltage) {
+    climberMotor.setControl(voltageRequest.withOutput(voltage));
+  }
+
+  public double getRawAbsoluteEncoder() {
+    return absoluteEncoder.getAbsolutePosition();
+  }
+
+  public double getAbsoluteEncoder() {
+    double rotations = getRawAbsoluteEncoder();
+
+    rotations -= absoluteEncoderOffset;
+
+    return rotations;
+  }
+
+  public void resetClimberToAbsolutePosition() {
+    climberMotor.setPosition((constClimber.ABS_ENCODER_INVERT) ? -getAbsoluteEncoder() : getAbsoluteEncoder());
+  }
+
+  public double getClimberVelocity() {
+    return Units.rotationsToDegrees(climberMotor.getVelocity().getValueAsDouble());
+  }
+
   @Override
   public void periodic() {
+    SmartDashboard.putNumber("Climber/Absolute Encoder Raw Value (Rotations)", getRawAbsoluteEncoder());
+    SmartDashboard.putNumber("Climber/Offset Absolute Encoder Value (Rotations)", getAbsoluteEncoder());
     SmartDashboard.putNumber("Climber/Motor position(Rotations)", getPosition());
     SmartDashboard.putNumber("Climber/Motor percent output", climberMotor.get());
     // This method will be called once per scheduler run