Change elevator presets to use a lambda

chopshop-166 · Feb 22, 2025 · 3be8734 · 3be8734
1 parent 8325974
commit 3be8734
Show file tree

Hide file tree

Showing 3 changed files with 27 additions and 42 deletions.
diff --git a/src/main/java/frc/robot/maps/ColdStart.java b/src/main/java/frc/robot/maps/ColdStart.java
@@ -145,11 +145,20 @@ public ElevatorMap getElevatorMap() {
 
         var elevatorMotors = new SmartMotorControllerGroup(leftMotor, rightMotor);
 
+        ElevatorMap.PresetValues presets = preset -> switch (preset) {
+            case STOW -> 1;
+            case INTAKE -> 16.6;
+            case SCOREL1 -> 5;
+            case SCOREL2 -> 14;
+            case SCOREL3 -> 29.5;
+            case SCOREL4 -> 56;
+            case HIGHESTPOINT -> 57.5;
+            default -> Double.NaN;
+        };
+
         elevatorMotors.validateEncoderRate(.2, 10);
-        return new ElevatorMap(
-                elevatorMotors, leftMotor.getEncoder(),
-                new ElevatorMap.ElevatorPresetValues(16.6, 5, 14, 29.5, 56, 57.5, 1),
-                new ValueRange(0, 57.5), new ValueRange(6, 53), pid, feedForward);
+        return new ElevatorMap(elevatorMotors, leftMotor.getEncoder(),
+                presets, new ValueRange(0, 57.5), new ValueRange(6, 53), pid, feedForward);
     }
 
     @Override

diff --git a/src/main/java/frc/robot/maps/subsystems/ElevatorMap.java b/src/main/java/frc/robot/maps/subsystems/ElevatorMap.java
@@ -1,5 +1,7 @@
 package frc.robot.maps.subsystems;
 
+import java.util.function.ToDoubleFunction;
+
 import com.chopshop166.chopshoplib.ValueRange;
 import com.chopshop166.chopshoplib.logging.DataWrapper;
 import com.chopshop166.chopshoplib.logging.LoggableMap;
@@ -34,56 +36,29 @@ public enum ElevatorPresets {
         HOLD
     }
 
-    public record ElevatorPresetValues(double intake, double scoreL1, double scoreL2, double scoreL3, double scoreL4,
-            double highestPoint, double stow) {
-        public ElevatorPresetValues() {
-            this(0, 0, 0, 0, 0, 0, 0);
-        }
-
-        public double getValue(ElevatorPresets preset) {
-            switch (preset) {
-                case OFF:
-                    return Double.NaN;
-                case INTAKE:
-                    return intake;
-                case SCOREL1:
-                    return scoreL1;
-                case SCOREL2:
-                    return scoreL2;
-                case SCOREL3:
-                    return scoreL3;
-                case SCOREL4:
-                    return scoreL4;
-                case HIGHESTPOINT:
-                    return highestPoint;
-                case STOW:
-                    return stow;
-                default:
-                    return 0;
-            }
-        }
+    public interface PresetValues extends ToDoubleFunction<ElevatorPresets> {
     }
 
     public final SmartMotorController motor;
     public final IEncoder encoder;
-    public final ElevatorPresetValues elevatorPreset;
+    public final PresetValues presetValues;
     public final ValueRange hardLimits;
     public final ValueRange softLimits;
     public final ProfiledPIDController pid;
     public final ElevatorFeedforward feedForward;
 
     public ElevatorMap() {
-        this(new SmartMotorController(), new MockEncoder(), new ElevatorPresetValues(),
+        this(new SmartMotorController(), new MockEncoder(), p -> Double.NaN,
                 new ValueRange(0, 0), new ValueRange(0, 0), new ProfiledPIDController(0, 0, 0, new Constraints(0, 0)),
                 new ElevatorFeedforward(0, 0, 0));
     }
 
     public ElevatorMap(SmartMotorController motor, IEncoder encoder,
-            ElevatorPresetValues elevatorPreset, ValueRange hardLimits, ValueRange softLimits,
+            PresetValues presetValues, ValueRange hardLimits, ValueRange softLimits,
             ProfiledPIDController pid, ElevatorFeedforward feedForward) {
         this.motor = motor;
         this.encoder = encoder;
-        this.elevatorPreset = elevatorPreset;
+        this.presetValues = presetValues;
         this.hardLimits = hardLimits;
         this.softLimits = softLimits;
         this.pid = pid;

diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java
@@ -64,17 +64,17 @@ public Command zero() {
         return startSafe(() -> {
             getMap().motor.resetValidators();
             level = ElevatorPresets.OFF;
-                getData().motor.setpoint = ZEROING_SPEED;
-            
+            getData().motor.setpoint = ZEROING_SPEED;
+
         }).until(() -> getMap().motor.validate() && armSafeSub.getAsBoolean()).andThen(resetCmd());
     }
 
     public Command moveTo(ElevatorPresets level) {
         PersistenceCheck setPointPersistenceCheck = new PersistenceCheck(30, pid::atGoal);
         return runOnce(() -> {
-                this.level = level;
-                pid.reset(getElevatorHeight(), getData().liftingHeightVelocity);
-            
+            this.level = level;
+            pid.reset(getElevatorHeight(), getData().liftingHeightVelocity);
+
         }).andThen(run(() -> {
             Logger.recordOutput("PID at goal", pid.atGoal());
         })).until(() -> {
@@ -122,7 +122,8 @@ public void periodic() {
         super.periodic();
         heightPub.set(getData().heightAbsInches / getMap().hardLimits.max());
         if (level != ElevatorPresets.OFF) {
-            double targetHeight = level == ElevatorPresets.HOLD ? holdHeight : getMap().elevatorPreset.getValue(level);
+            double targetHeight = level == ElevatorPresets.HOLD ? holdHeight
+                    : getMap().presetValues.applyAsDouble(level);
             double setpoint = pid.calculate(getElevatorHeight(), new State(targetHeight, 0));
             setpoint += getMap().feedForward.calculate(
                     pid.getSetpoint().position,