From 3be87340f8784ef5610aa26bedb0513225aa915f Mon Sep 17 00:00:00 2001 From: Matt Soucy Date: Sat, 22 Feb 2025 10:04:21 -0500 Subject: [PATCH] Change elevator presets to use a lambda --- src/main/java/frc/robot/maps/ColdStart.java | 17 ++++++-- .../robot/maps/subsystems/ElevatorMap.java | 39 ++++--------------- .../java/frc/robot/subsystems/Elevator.java | 13 ++++--- 3 files changed, 27 insertions(+), 42 deletions(-) diff --git a/src/main/java/frc/robot/maps/ColdStart.java b/src/main/java/frc/robot/maps/ColdStart.java index 4db3107..d847e4a 100644 --- 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 index 78f3e05..e70806c 100644 --- 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 { } 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 index 07b0820..b24cf5e 100644 --- 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,