add elevator subsystem

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2025.2.1"
+    id "edu.wpi.first.GradleRIO" version "2025.3.2"
     id 'com.diffplug.spotless' version '6.20.0'
 }
 

src/main/java/frc/robot/AutoRoutines/AutoProducer.java

@@ -23,7 +23,7 @@
 import frc.robot.commands.TootsieSlideCommands.ShootTootsieSlide;
 import frc.robot.subsystems.ArmSubsystem;
 import frc.robot.subsystems.CoralPosition;
-import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.ElevatorSubsystemMD2;
 import frc.robot.subsystems.FunnelSubsystem;
 import frc.robot.subsystems.LedSubsystem;
 import frc.robot.subsystems.SwerveSubsystem;
@@ -36,7 +36,7 @@ public class AutoProducer extends SequentialCommandGroup {
   public AutoProducer(
       SwerveSubsystem driveTrain,
       TootsieSlideSubsystem shooter,
-      ElevatorSubsystem elevator,
+      ElevatorSubsystemMD2 elevator,
       FunnelSubsystem funnel,
       ArmSubsystem arm,
       List<LandmarkPose> autoInformation,
@@ -83,7 +83,7 @@ public AutoProducer(
   }
 
   private void settyCycle(
-      ElevatorSubsystem elevator,
+      ElevatorSubsystemMD2 elevator,
       FunnelSubsystem funnel,
       TootsieSlideSubsystem shooter,
       SwerveSubsystem driveTrain,

src/main/java/frc/robot/Robot.java

@@ -11,7 +11,7 @@
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.commands.ElevatorCommands.ZeroElevatorHardStop;
 import frc.robot.subsystems.CoralPosition;
-import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.ElevatorSubsystemMD2;
 import frc.robot.subsystems.SwerveSubsystem;
 import frc.robot.subsystems.VisionSystem;
 import frc.robot.util.LoggedTalonFX;
@@ -89,7 +89,7 @@ public void disabledInit() {
   @Override
   public void robotInit() {
     DogLog.setOptions(
-        new DogLogOptions().withNtPublish(false).withCaptureDs(true).withLogExtras(true));
+        new DogLogOptions().withNtPublish(true).withCaptureDs(true).withLogExtras(true));
     DogLog.log("PIDArmKP", Constants.Arm.S0C_KP);
     DogLog.log("PIDArmKI", Constants.Arm.S0C_KI);
     DogLog.log("PIDArmKD", Constants.Arm.S0C_KD);
@@ -144,9 +144,9 @@ public void teleopInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
-    if (ElevatorSubsystem.getInstance().isElevatorZeroed() == false) {
+    if (ElevatorSubsystemMD2.getInstance().isElevatorZeroed() == false) {
       CommandScheduler.getInstance()
-          .schedule(new ZeroElevatorHardStop(ElevatorSubsystem.getInstance()));
+          .schedule(new ZeroElevatorHardStop(ElevatorSubsystemMD2.getInstance()));
     }
 
     // CommandScheduler.getInstance();

src/main/java/frc/robot/RobotContainer.java

@@ -38,7 +38,7 @@
 import frc.robot.commands.TransferPieceBetweenFunnelAndElevator;
 import frc.robot.subsystems.ArmSubsystem;
 import frc.robot.subsystems.CoralPosition;
-import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.ElevatorSubsystemMD2;
 import frc.robot.subsystems.FunnelSubsystem;
 import frc.robot.subsystems.LedSubsystem;
 import frc.robot.subsystems.SwerveSubsystem;
@@ -53,7 +53,7 @@ public class RobotContainer {
 
   TootsieSlideSubsystem tootsieSlideSubsystem = TootsieSlideSubsystem.getInstance();
   FunnelSubsystem funnelSubsystem = FunnelSubsystem.getInstance();
-  ElevatorSubsystem elevatorSubsystem = ElevatorSubsystem.getInstance();
+  ElevatorSubsystemMD2 elevatorSubsystem = ElevatorSubsystemMD2.getInstance();
   ArmSubsystem armSubsystem = ArmSubsystem.getInstance();
   LedSubsystem leds = new LedSubsystem();
   // Alliance color
@@ -286,7 +286,7 @@ private void configureBindings() {
                 () ->
                     CoralPosition.isCoralInFunnel()
                         && elevatorSubsystem.atIntake()
-                        && elevatorSubsystem.isAtPosition())
+                        && elevatorSubsystem.isAtTargetHeight())
             .and(RobotModeTriggers.teleop());
 
     funnelCheckout

src/main/java/frc/robot/commandGroups/D2Intake.java

@@ -5,13 +5,13 @@
 import frc.robot.commands.ElevatorCommands.SetElevatorLevel;
 import frc.robot.commands.TootsieSlideCommands.ShootTootsieSlide;
 import frc.robot.commands.TransferPieceBetweenFunnelAndElevator;
-import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.ElevatorSubsystemMD2;
 import frc.robot.subsystems.FunnelSubsystem;
 import frc.robot.subsystems.TootsieSlideSubsystem;
 
 public class D2Intake extends SequentialCommandGroup {
   public D2Intake(
-      ElevatorSubsystem elevatorSubsystem,
+      ElevatorSubsystemMD2 elevatorSubsystem,
       TootsieSlideSubsystem tootsieSlideSubsystem,
       FunnelSubsystem funnelSubsystem) {
     addCommands(

src/main/java/frc/robot/commandGroups/Dealgaenate.java

@@ -10,14 +10,14 @@
 import frc.robot.commands.DaleCommands.ArmToAngleAndSpinFlywheel;
 import frc.robot.commands.ElevatorCommands.SetElevatorLevel;
 import frc.robot.subsystems.ArmSubsystem;
-import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.ElevatorSubsystemMD2;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class Dealgaenate extends SequentialCommandGroup {
   /** Creates a new Dealgaenate. */
-  public Dealgaenate(ArmSubsystem arm, ElevatorSubsystem elevator, ElevatorPositions position) {
+  public Dealgaenate(ArmSubsystem arm, ElevatorSubsystemMD2 elevator, ElevatorPositions position) {
     // Add your commands in the addCommands() call, e.g.
     // addCommands(new FooCommand(), new BarCommand());
 

src/main/java/frc/robot/commandGroups/EjectCoralFR.java

@@ -4,12 +4,12 @@
 import frc.robot.Constants.ElevatorConstants.ElevatorPositions;
 import frc.robot.commands.ElevatorCommands.SetElevatorLevel;
 import frc.robot.commands.TootsieSlideCommands.ShootTootsieSlide;
-import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.ElevatorSubsystemMD2;
 import frc.robot.subsystems.TootsieSlideSubsystem;
 
 public class EjectCoralFR extends SequentialCommandGroup {
   public EjectCoralFR(
-      ElevatorSubsystem elevatorSubsystem, TootsieSlideSubsystem tootsieSlideSubsystem) {
+      ElevatorSubsystemMD2 elevatorSubsystem, TootsieSlideSubsystem tootsieSlideSubsystem) {
     addCommands(
         new SetElevatorLevel(elevatorSubsystem, ElevatorPositions.L1, false)
             .andThen(new ShootTootsieSlide(tootsieSlideSubsystem).withTimeout(0.5)));

src/main/java/frc/robot/commandGroups/ElevatorL4.java

@@ -4,11 +4,11 @@
 import frc.robot.Constants.ElevatorConstants.ElevatorPositions;
 import frc.robot.commands.ElevatorCommands.ElevatorHoldL4;
 import frc.robot.commands.ElevatorCommands.SetElevatorLevel;
-import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.ElevatorSubsystemMD2;
 
 public class ElevatorL4 extends SequentialCommandGroup {
 
-  public ElevatorL4(ElevatorSubsystem elevatorSubsystem, boolean checkIfCoralInTootsie) {
+  public ElevatorL4(ElevatorSubsystemMD2 elevatorSubsystem, boolean checkIfCoralInTootsie) {
 
     addCommands(
         new SetElevatorLevel(elevatorSubsystem, ElevatorPositions.L4, checkIfCoralInTootsie)

src/main/java/frc/robot/commandGroups/Intake.java

@@ -8,7 +8,7 @@
 import frc.robot.Constants.ElevatorConstants.ElevatorPositions;
 import frc.robot.commands.ElevatorCommands.SetElevatorLevel;
 import frc.robot.commands.TransferPieceBetweenFunnelAndElevator;
-import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.ElevatorSubsystemMD2;
 import frc.robot.subsystems.FunnelSubsystem;
 import frc.robot.subsystems.LedSubsystem;
 import frc.robot.subsystems.TootsieSlideSubsystem;
@@ -19,7 +19,7 @@
 public class Intake extends SequentialCommandGroup {
 
   public Intake(
-      ElevatorSubsystem elevatorSubsystem,
+      ElevatorSubsystemMD2 elevatorSubsystem,
       FunnelSubsystem funnelSubsystem,
       TootsieSlideSubsystem tootsieSlideSubsystem,
       LedSubsystem leds) {

src/main/java/frc/robot/commandGroups/JamesHardenScore.java

@@ -10,15 +10,15 @@
 import frc.robot.commands.EndWhenCloseEnough;
 import frc.robot.commands.SwerveCommands.JamesHardenMovement;
 import frc.robot.commands.TootsieSlideCommands.ShootTootsieSlide;
-import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.ElevatorSubsystemMD2;
 import frc.robot.subsystems.LedSubsystem;
 import frc.robot.subsystems.SwerveSubsystem;
 import frc.robot.subsystems.TootsieSlideSubsystem;
 import java.util.function.BooleanSupplier;
 
 public class JamesHardenScore extends SequentialCommandGroup {
   public JamesHardenScore(
-      ElevatorSubsystem elevatorSubsystem,
+      ElevatorSubsystemMD2 elevatorSubsystem,
       TootsieSlideSubsystem tootsieSlideSubsystem,
       SwerveSubsystem swerveSubsystem,
       ElevatorPositions height,
@@ -64,7 +64,7 @@ public JamesHardenScore(
   }
 
   public JamesHardenScore(
-      ElevatorSubsystem elevatorSubsystem,
+      ElevatorSubsystemMD2 elevatorSubsystem,
       TootsieSlideSubsystem tootsieSlideSubsystem,
       SwerveSubsystem swerveSubsystem,
       ElevatorPositions height,

src/main/java/frc/robot/commandGroups/PutUpAndShoot.java

@@ -4,12 +4,12 @@
 import frc.robot.Constants.ElevatorConstants.ElevatorPositions;
 import frc.robot.commands.ElevatorCommands.SetElevatorLevel;
 import frc.robot.commands.TootsieSlideCommands.ShootTootsieSlide;
-import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.ElevatorSubsystemMD2;
 import frc.robot.subsystems.TootsieSlideSubsystem;
 
 public class PutUpAndShoot extends SequentialCommandGroup {
   public PutUpAndShoot(
-      ElevatorSubsystem elevatorSubsystem,
+      ElevatorSubsystemMD2 elevatorSubsystem,
       TootsieSlideSubsystem tootsieSlideSubsystem,
       ElevatorPositions height) {
 

src/main/java/frc/robot/commandGroups/RunFunnelUntilDetectionSafeSmooth.java

@@ -4,14 +4,14 @@
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.commands.FunnelCommands.RampUpFunnel;
 import frc.robot.commands.FunnelCommands.RunFunnelUntilDetectionSafe;
-import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.ElevatorSubsystemMD2;
 import frc.robot.subsystems.FunnelSubsystem;
 import frc.robot.subsystems.LedSubsystem;
 import java.util.function.BooleanSupplier;
 
 public class RunFunnelUntilDetectionSafeSmooth extends SequentialCommandGroup {
   public RunFunnelUntilDetectionSafeSmooth(
-      ElevatorSubsystem elevatorSubsystem, FunnelSubsystem funnelSubsystem, LedSubsystem leds) {
+      ElevatorSubsystemMD2 elevatorSubsystem, FunnelSubsystem funnelSubsystem, LedSubsystem leds) {
 
     addCommands(
         // new WaitCommand(0.1),

src/main/java/frc/robot/commandGroups/ShootL1.java

@@ -4,11 +4,12 @@
 import frc.robot.Constants.ElevatorConstants.ElevatorPositions;
 import frc.robot.commands.ElevatorCommands.SetElevatorLevel;
 import frc.robot.commands.TootsieSlideCommands.ShootSlow;
-import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.ElevatorSubsystemMD2;
 import frc.robot.subsystems.TootsieSlideSubsystem;
 
 public class ShootL1 extends SequentialCommandGroup {
-  public ShootL1(ElevatorSubsystem elevatorSubsystem, TootsieSlideSubsystem tootsieSlideSubsystem) {
+  public ShootL1(
+      ElevatorSubsystemMD2 elevatorSubsystem, TootsieSlideSubsystem tootsieSlideSubsystem) {
     addCommands(
         new SetElevatorLevel(elevatorSubsystem, ElevatorPositions.L1, false)
             .andThen(new ShootSlow(tootsieSlideSubsystem))

src/main/java/frc/robot/commandGroups/ShootL1Funnel.java

@@ -6,13 +6,13 @@
 import frc.robot.commands.ElevatorCommands.SetElevatorLevel;
 import frc.robot.commands.FunnelCommands.ReverseFunnel;
 import frc.robot.commands.TootsieSlideCommands.ReverseTootsie;
-import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.ElevatorSubsystemMD2;
 import frc.robot.subsystems.FunnelSubsystem;
 import frc.robot.subsystems.TootsieSlideSubsystem;
 
 public class ShootL1Funnel extends SequentialCommandGroup {
   public ShootL1Funnel(
-      ElevatorSubsystem elevatorSubsystem,
+      ElevatorSubsystemMD2 elevatorSubsystem,
       TootsieSlideSubsystem tootsieSlideSubsystem,
       FunnelSubsystem funnelSubsystem) {
     // if (CoralPosition.isCoralInTootsieSlide()) {

src/main/java/frc/robot/commandGroups/UnjamFunnelAndIntake.java

@@ -5,13 +5,13 @@
 import frc.robot.commands.ElevatorCommands.SetElevatorLevel;
 import frc.robot.commands.FunnelCommands.RunFunnelAndTootsieInCommand;
 import frc.robot.commands.FunnelCommands.RunFunnelOutUntilUnstuckCommand;
-import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.ElevatorSubsystemMD2;
 import frc.robot.subsystems.FunnelSubsystem;
 import frc.robot.subsystems.TootsieSlideSubsystem;
 
 public class UnjamFunnelAndIntake extends SequentialCommandGroup {
   public UnjamFunnelAndIntake(
-      ElevatorSubsystem elevatorSubsystem,
+      ElevatorSubsystemMD2 elevatorSubsystem,
       FunnelSubsystem funnelSubsystem,
       TootsieSlideSubsystem tootsieSlideSubsystem) {
     addCommands(

src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java

@@ -5,16 +5,16 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants.ElevatorConstants.ElevatorPositions;
 import frc.robot.subsystems.ArmSubsystem;
-import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.ElevatorSubsystemMD2;
 
 public class ArmToAngleAndSpinFlywheel extends Command {
   private final ArmSubsystem armPlusFlywheel;
   private double angle;
   private double tolerance = 5;
-  private ElevatorSubsystem elevatorSubsystem;
+  private ElevatorSubsystemMD2 elevatorSubsystem;
 
   public ArmToAngleAndSpinFlywheel(
-      double angle, ArmSubsystem armSub, ElevatorSubsystem elevatorSubsystem) {
+      double angle, ArmSubsystem armSub, ElevatorSubsystemMD2 elevatorSubsystem) {
     armPlusFlywheel = armSub;
     this.angle = angle;
     this.elevatorSubsystem = elevatorSubsystem;
@@ -34,7 +34,7 @@ public void execute() {
   @Override
   public void end(boolean interrupted) {
     armPlusFlywheel.stopFlywheel(); // Stop the flywheel
-    elevatorSubsystem.elevateTo(ElevatorPositions.safePosition);
+    elevatorSubsystem.setHeight(ElevatorPositions.safePosition.height);
   }
 
   @Override

src/main/java/frc/robot/commands/ElevatorCommands/DefaultElevator.java

@@ -3,15 +3,15 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants.ElevatorConstants.ElevatorPositions;
 import frc.robot.subsystems.CoralPosition;
-import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.ElevatorSubsystemMD2;
 
 public class DefaultElevator extends Command {
-  private final ElevatorSubsystem elevatorSubsystem;
+  private final ElevatorSubsystemMD2 elevatorSubsystem;
 
   // private WindowAverage windowAverage = new WindowAverage();
   // private double currentAverage = 0.0;
 
-  public DefaultElevator(ElevatorSubsystem subsystem) {
+  public DefaultElevator(ElevatorSubsystemMD2 subsystem) {
     elevatorSubsystem = subsystem;
     addRequirements(elevatorSubsystem);
   }
@@ -41,7 +41,7 @@ public void execute() {
     // }
 
     if (!CoralPosition.isCoralInTootsieSlide()) {
-      elevatorSubsystem.elevateTo(ElevatorPositions.Intake);
+      elevatorSubsystem.setHeight(ElevatorPositions.Intake.height);
     }
   }
 

src/main/java/frc/robot/commands/ElevatorCommands/ElevatorHoldL4.java

@@ -2,20 +2,20 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants.ElevatorConstants.ElevatorPositions;
-import frc.robot.subsystems.ElevatorSubsystem;
+import frc.robot.subsystems.ElevatorSubsystemMD2;
 
 public class ElevatorHoldL4 extends Command {
-  private ElevatorSubsystem elevatorSubsystem;
+  private ElevatorSubsystemMD2 elevatorSubsystem;
 
-  public ElevatorHoldL4(ElevatorSubsystem elevatorSubsystem) {
+  public ElevatorHoldL4(ElevatorSubsystemMD2 elevatorSubsystem) {
     this.elevatorSubsystem = elevatorSubsystem;
     addRequirements(elevatorSubsystem);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    elevatorSubsystem.elevateTo(ElevatorPositions.LIMIT_OF_TRAVEL);
+    elevatorSubsystem.setHeight(ElevatorPositions.LIMIT_OF_TRAVEL.height);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -31,6 +31,6 @@ public void end(boolean interrupted) {}
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return elevatorSubsystem.isAtPosition();
+    return elevatorSubsystem.isAtTargetHeight();
   }
 }