diff --git a/src/main/java/frc/robot/maps/ColdStart.java b/src/main/java/frc/robot/maps/ColdStart.java index 8b4164c..d91bd69 100644 --- 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 index fb53214..272ac8d 100644 --- 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 { @@ -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 { } 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 index 9d4b771..cf8e518 100644 --- 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,