Arm Rotate Map presets use a lambda

chopshop-166 · Feb 22, 2025 · b187799 · b187799
1 parent 8c7633d
commit b187799
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 40 deletions.
diff --git a/src/main/java/frc/robot/maps/ColdStart.java b/src/main/java/frc/robot/maps/ColdStart.java
@@ -177,8 +177,15 @@ public ArmRotateMap getArmRotateMap() {
         ProfiledPIDController pid = new ProfiledPIDController(0.025, 0, 0, new Constraints(90, 300));
         pid.setTolerance(1);
         ArmFeedforward feedForward = new ArmFeedforward(0.04, 0.0, 0.0018);
-        return new ArmRotateMap(motor, absEncoder,
-                new ArmRotateMap.ArmRotatePresetValues(96.3, 66, 66, 66, 66, 66, 96.3), pid,
+
+        ArmRotateMap.PresetValue presets = p -> switch (p) {
+            case INTAKE -> 96.3;
+            case SCOREL1, SCOREL2, SCOREL3, SCOREL4, OUT -> 66;
+            case STOW -> 96.3;
+            default -> Double.NaN;
+        };
+
+        return new ArmRotateMap(motor, absEncoder, presets, pid,
                 new ValueRange(5, 97), new ValueRange(0, 94), feedForward);
     }
 

diff --git a/src/main/java/frc/robot/maps/subsystems/ArmRotateMap.java b/src/main/java/frc/robot/maps/subsystems/ArmRotateMap.java
@@ -1,18 +1,17 @@
 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;
 import com.chopshop166.chopshoplib.logging.data.MotorControllerData;
 import com.chopshop166.chopshoplib.motors.SmartMotorController;
-import com.chopshop166.chopshoplib.sensors.IAbsolutePosition;
-import com.chopshop166.chopshoplib.sensors.IEncoder;
-import com.chopshop166.chopshoplib.sensors.MockEncoder;
 
+import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.wpilibj.DutyCycleEncoder;
-import edu.wpi.first.math.controller.ArmFeedforward;
 
 public class ArmRotateMap implements LoggableMap<ArmRotateMap.Data> {
 
@@ -38,53 +37,25 @@ public enum ArmRotatePresets {
 
     }
 
-    public record ArmRotatePresetValues(double intake, double scoreL1, double scoreL2, double scoreL3, double scoreL4,
-            double out,
-            double stow) {
-        public ArmRotatePresetValues() {
-            this(0, 0, 0, 0, 0, 0, 0);
-        }
-
-        public double getValue(ArmRotatePresets 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 OUT:
-                    return out;
-                case STOW:
-                    return stow;
-                default:
-                    return 0;
-            }
-        }
+    public interface PresetValue extends ToDoubleFunction<ArmRotatePresets> {
     }
 
     public SmartMotorController motor;
     public final DutyCycleEncoder encoder;
-    public final ArmRotatePresetValues armRotatePreset;
+    public final PresetValue armRotatePreset;
     public final ProfiledPIDController pid;
     public final ValueRange hardLimits;
     public final ValueRange softLimits;
     public final ArmFeedforward armFeedforward;
 
     public ArmRotateMap() {
-        this(new SmartMotorController(), new DutyCycleEncoder(0), new ArmRotatePresetValues(),
+        this(new SmartMotorController(), new DutyCycleEncoder(0), p -> Double.NaN,
                 new ProfiledPIDController(0, 0, 0, new Constraints(0, 0)), new ValueRange(0, 0), new ValueRange(0, 0),
                 new ArmFeedforward(0, 0, 0));
 
     }
 
-    public ArmRotateMap(SmartMotorController motor, DutyCycleEncoder encoder, ArmRotatePresetValues armRotatePreset,
+    public ArmRotateMap(SmartMotorController motor, DutyCycleEncoder encoder, PresetValue armRotatePreset,
             ProfiledPIDController pid, ValueRange hardLimits, ValueRange softLimits, ArmFeedforward armFeedforward) {
         this.motor = motor;
         this.encoder = encoder;

diff --git a/src/main/java/frc/robot/subsystems/ArmRotate.java b/src/main/java/frc/robot/subsystems/ArmRotate.java
@@ -86,7 +86,7 @@ public Command moveToZero() {
             getData().motor.setpoint = ZEROING_SPEED;
             preset = ArmRotatePresets.OFF;
         }).until(() -> {
-            return getArmAngle() > getMap().armRotatePreset.getValue(ArmRotatePresets.STOW);
+            return getArmAngle() > getMap().armRotatePreset.applyAsDouble(ArmRotatePresets.STOW);
         });
     }
 
@@ -114,7 +114,7 @@ public void periodic() {
         armSafePub.set(getData().rotationAbsAngleDegrees >= SAFE_ANGLE);
         if (preset != ArmRotatePresets.OFF) {
             double targetHeight = preset == ArmRotatePresets.HOLD ? holdAngle
-                    : getMap().armRotatePreset.getValue(preset);
+                    : getMap().armRotatePreset.applyAsDouble(preset);
             double setpoint = pid.calculate(getArmAngle(), new State(targetHeight, 0));
             setpoint += getMap().armFeedforward.calculate(
                     pid.getSetpoint().position,