[elevator] Motion profiling and misc refactoring

src/main/java/org/curtinfrc/frc2025/Autos.java

@@ -14,7 +14,7 @@
 import org.curtinfrc.frc2025.subsystems.drive.DriveConstants.DriveSetpoints;
 import org.curtinfrc.frc2025.subsystems.ejector.Ejector;
 import org.curtinfrc.frc2025.subsystems.elevator.Elevator;
-import org.curtinfrc.frc2025.subsystems.elevator.ElevatorConstants.ElevatorSetpoints;
+import org.curtinfrc.frc2025.subsystems.elevator.Elevator.ElevatorSetpoints;
 import org.curtinfrc.frc2025.subsystems.intake.Intake;
 
 public class Autos {

src/main/java/org/curtinfrc/frc2025/Constants.java

@@ -7,15 +7,15 @@
 import edu.wpi.first.util.struct.StructSerializable;
 import edu.wpi.first.wpilibj.RobotBase;
 import org.curtinfrc.frc2025.subsystems.drive.DriveConstants.DriveSetpoints;
-import org.curtinfrc.frc2025.subsystems.elevator.ElevatorConstants.ElevatorSetpoints;
+import org.curtinfrc.frc2025.subsystems.elevator.Elevator.ElevatorSetpoints;
 
 /**
  * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running
  * on a roboRIO. Change the value of "simMode" to switch between "sim" (physics sim) and "replay"
  * (log replay from a file).
  */
 public final class Constants {
-  public static final RobotType robotType = RobotType.COMPBOT;
+  public static final RobotType robotType = RobotType.SIMBOT;
   public static final double ROBOT_X = 0.705;
   public static final double ROBOT_Y = 0.730;
   public static final double FIELD_LENGTH = aprilTagLayout.getFieldLength();

src/main/java/org/curtinfrc/frc2025/Robot.java

@@ -44,7 +44,7 @@
 import org.curtinfrc.frc2025.subsystems.ejector.EjectorIOComp;
 import org.curtinfrc.frc2025.subsystems.ejector.EjectorIOSim;
 import org.curtinfrc.frc2025.subsystems.elevator.Elevator;
-import org.curtinfrc.frc2025.subsystems.elevator.ElevatorConstants.ElevatorSetpoints;
+import org.curtinfrc.frc2025.subsystems.elevator.Elevator.ElevatorSetpoints;
 import org.curtinfrc.frc2025.subsystems.elevator.ElevatorIO;
 import org.curtinfrc.frc2025.subsystems.elevator.ElevatorIOComp;
 import org.curtinfrc.frc2025.subsystems.elevator.ElevatorIOSim;
@@ -281,6 +281,13 @@ public Robot() {
             drive,
             drive::logTrajectory);
 
+    autoChooser.addCmd(
+        "Elevator L2",
+        () -> elevator.goToSetpoint(ElevatorSetpoints.L2, () -> true).until(elevator.atSetpoint));
+    autoChooser.addCmd(
+        "Elevator L3",
+        () -> elevator.goToSetpoint(ElevatorSetpoints.L3, () -> true).until(elevator.atSetpoint));
+
     autoChooser.addRoutine("Test Path", () -> Autos.path("Test Path", factory, drive));
     autoChooser.addRoutine(
         "One Piece Centre", () -> Autos.onePieceCentre(factory, drive, ejector, elevator, intake));

src/main/java/org/curtinfrc/frc2025/subsystems/elevator/Elevator.java

@@ -1,42 +1,78 @@
 package org.curtinfrc.frc2025.subsystems.elevator;
 
-import static org.curtinfrc.frc2025.subsystems.elevator.ElevatorConstants.*;
-
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.util.struct.Struct;
+import edu.wpi.first.util.struct.StructGenerator;
+import edu.wpi.first.util.struct.StructSerializable;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import java.util.function.BooleanSupplier;
 import java.util.function.Supplier;
-import org.curtinfrc.frc2025.subsystems.elevator.ElevatorConstants.ElevatorSetpoints;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
 public class Elevator extends SubsystemBase {
+  private static final double TOLERANCE = 0.005;
+  public static double CLIMB_KP = 45;
+  public static double CLIMB_KD = 0;
+
+  public static enum ElevatorSetpoints implements StructSerializable {
+    L1(0),
+    L2(0.221),
+    AlgaePopLow(0),
+    L3(0.611),
+    AlgaePopHigh(0.38),
+    BASE(0.01),
+    climbPrep(0.3),
+    climbAttempt(0.5),
+    climbed(0.01);
+
+    public final double setpointMetres;
+
+    ElevatorSetpoints(double setpointMetres) {
+      this.setpointMetres = setpointMetres;
+    }
+
+    public static ElevatorSetpoints getPopPoint(ElevatorSetpoints point) {
+      switch (point) {
+        case L2:
+          return AlgaePopLow;
+        case L3:
+          return AlgaePopHigh;
+        default:
+          return BASE;
+      }
+    }
+
+    public static final Struct<ElevatorSetpoints> struct =
+        StructGenerator.genEnum(ElevatorSetpoints.class);
+  }
+
   private final ElevatorIO io;
   private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged();
-  private final PIDController pid = new PIDController(kP, 0, kD);
-  private final PIDController climbPID = new PIDController(climbkP, climbkI, climbkD);
+  private final PIDController climbPID = new PIDController(CLIMB_KP, 0, CLIMB_KD);
   private ElevatorSetpoints setpoint = ElevatorSetpoints.BASE;
 
   public final Trigger isNotAtCollect = new Trigger(() -> setpoint != ElevatorSetpoints.BASE);
-  public final Trigger toZero = new Trigger(() -> inputs.hominSensor);
-  public final Trigger atSetpoint = new Trigger(pid::atSetpoint);
+  public final Trigger atSetpoint;
   public final Trigger atClimbSetpoint = new Trigger(climbPID::atSetpoint);
-  public final Trigger algaePop =
-      new Trigger(
-          () ->
-              setpoint == ElevatorSetpoints.AlgaePopHigh
-                  || setpoint == ElevatorSetpoints.AlgaePopLow);
 
   public Elevator(ElevatorIO io) {
     this.io = io;
-    pid.setTolerance(tolerance);
-    climbPID.setTolerance(tolerance);
+    climbPID.setTolerance(TOLERANCE);
+
+    atSetpoint =
+        new Trigger(
+                () -> {
+                  var error = Math.abs(inputs.positionMetres - setpoint.setpointMetres);
+                  var velocity = Math.abs(inputs.velocityMetresPerSecond);
+                  return error < TOLERANCE && velocity < 0.1;
+                })
+            .debounce(0.01);
   }
 
   @Override
@@ -46,65 +82,44 @@ public void periodic() {
     Logger.recordOutput("Elevator/isNotAtCollect", isNotAtCollect.getAsBoolean());
     Logger.recordOutput("Elevator/setpoint", ElevatorSetpoints.struct, setpoint);
     Logger.recordOutput("Elevator/AtSetpoint", atSetpoint.getAsBoolean());
-    Logger.recordOutput("Elevator/ActualError", pid.getError());
 
     // if (inputs.hominSensor) {
     //   io.zero();
     // }
   }
 
+  private void goToTarget(double targetMetres) {
+    io.setPosition(targetMetres);
+  }
+
   public Command goToSetpoint(Supplier<ElevatorSetpoints> point, BooleanSupplier safe) {
-    return Commands.either(
-            run(
-                () -> {
-                  setpoint = point.get();
-                  var out =
-                      pid.calculate(
-                          io.positionRotationsToMetres(inputs.positionRotations),
-                          setpoint.setpoint);
-                  Logger.recordOutput("Elevator/Output", out);
-                  Logger.recordOutput("Elevator/Error", pid.getError());
-                  Logger.recordOutput("Elevator/ClimberPID", false);
-                  io.setVoltage(out);
-                }),
-            Commands.none(),
-            safe)
-        .repeatedly()
-        .withName("GoToSetpoint");
+    return run(() -> {
+          setpoint = point.get();
+          if (safe.getAsBoolean()) {
+            goToTarget(point.get().setpointMetres);
+          }
+        })
+        .withName("GoToDynamicSetpoint");
   }
 
   public Command goToSetpoint(ElevatorSetpoints point, BooleanSupplier safe) {
-    return Commands.either(
-            run(
-                () -> {
-                  setpoint = point;
-                  var out =
-                      pid.calculate(
-                          io.positionRotationsToMetres(inputs.positionRotations),
-                          setpoint.setpoint);
-                  Logger.recordOutput("Elevator/Output", out);
-                  Logger.recordOutput("Elevator/Error", pid.getError());
-                  Logger.recordOutput("Elevator/ClimberPID", false);
-                  io.setVoltage(out);
-                }),
-            Commands.none(),
-            safe)
-        .repeatedly()
-        .withName("GoToSetpoint");
+    return run(() -> {
+          setpoint = point;
+          if (safe.getAsBoolean()) {
+            goToTarget(point.setpointMetres);
+          }
+        })
+        .withName("GoToStaticSetpoint");
   }
 
   public Command goToClimberSetpoint(ElevatorSetpoints point, BooleanSupplier safe) {
-    return Commands.either(
-        run(
-            () -> {
-              setpoint = point;
-              var out =
-                  climbPID.calculate(
-                      io.positionRotationsToMetres(inputs.positionRotations), setpoint.setpoint);
-              io.setVoltage(MathUtil.clamp(out, -4, 4));
-            }),
-        Commands.none(),
-        safe);
+    return run(
+        () -> {
+          if (safe.getAsBoolean()) {
+            var out = climbPID.calculate(inputs.positionMetres, point.setpointMetres);
+            io.setVoltage(MathUtil.clamp(out, -4, 4));
+          }
+        });
   }
 
   public Command zero() {
@@ -117,10 +132,6 @@ public Command stop() {
 
   @AutoLogOutput(key = "Elevator/Height")
   public Pose3d getHeight() {
-    return new Pose3d(
-        0,
-        0,
-        io.positionRotationsToMetres(inputs.positionRotations),
-        new Rotation3d(Math.PI / 2, 0, Math.PI / 2));
+    return new Pose3d(0, 0, inputs.positionMetres, new Rotation3d(Math.PI / 2, 0, Math.PI / 2));
   }
 }

src/main/java/org/curtinfrc/frc2025/subsystems/elevator/ElevatorIO.java

@@ -8,22 +8,16 @@ public interface ElevatorIO {
   public static class ElevatorIOInputs {
     public double appliedVolts;
     public double currentAmps;
-    public double positionRotations;
-    public double angularVelocityRotationsPerMinute;
+    public double positionMetres;
+    public double velocityMetresPerSecond;
     public boolean hominSensor;
   }
 
   public default void updateInputs(ElevatorIOInputs inputs) {}
 
   public default void setVoltage(double volts) {}
 
-  public default void zero() {}
-
-  public default double positionRotationsToMetres(double rotations) {
-    return rotations;
-  }
+  public default void setPosition(double positionMetres) {}
 
-  public default double positionMetresToRotations(double metres) {
-    return metres;
-  }
+  public default void zero() {}
 }