Basic elevator subsystem code added

Elevator.constants class added to constants (Gear ratio needed)

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

@@ -565,5 +565,8 @@ public static final class AutoAlign {
     public static final double AMP_TARGET_ANGLE = 0;
     public static final PathConstraints CONSTRAINTS =
         new PathConstraints(3, 3, Units.degreesToRadians(540), Units.degreesToRadians(720));
-  }
+    }
+          public static class Elevator{ //Placeholder elevator constant (update as needed)
+            public static final double GEAR_RATIO = 0;
+    }
 }

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

@@ -1,5 +1,67 @@
 package frc.robot.subsystems;
 
-public class ElevatorSubsystem {
-
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.Elevator;
+
+// private PIDController controller;
+
+public class ElevatorSubsystem extends SubsystemBase {
+
+  private TalonFX motor;
+  private PIDController controller;
+  private double targetHeight;
+  private double motorPower;
+  private double currentHeight = 0;
+  private final ShuffleboardTab ElevatorTab = Shuffleboard.getTab("Elevator");
+
+  /** Creates a new ElevatorSubsystem. */
+  public ElevatorSubsystem() {
+
+    motor = new TalonFX(15);
+
+    motor.clearStickyFaults();
+
+    controller = new PIDController(0.4, 0, 0.0125);
+
+    ElevatorTab.addNumber("Current Motor Power", () -> this.motorPower);
+    ElevatorTab.addNumber("Target Height", () -> this.targetHeight);
+    // ElevatorTab.addNumber("PID output", () -> this.controller);
+    ElevatorTab.addNumber("Current Height", () -> this.currentHeight);
+    ElevatorTab.add(controller);
+
+    targetHeight = 0;
+    currentHeight = 0;
+  }
+
+  public static double inchesToRotations(double height) {
+    return height / Elevator.GEAR_RATIO;
+  }
+
+  public static double rotationsToInches(double rotations) {
+    return rotations * Elevator.GEAR_RATIO;
+  }
+
+  public void setTargetHeight(double targetHeight) {
+    this.targetHeight = targetHeight;
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+    motorPower = controller.calculate(rotationsToInches(0), targetHeight); //Placeholder for rotations
+
+  }
+
+  public boolean nearTargetHeight() {
+    if (targetHeight - 0.5 <= currentHeight && currentHeight <= targetHeight + 0.5) {
+      return true;
+    }
+    return false;
+  }
 }