diff --git a/src/main/java/frc/robot/CommandSequences.java b/src/main/java/frc/robot/CommandSequences.java index 0fadd31..c2b6d7e 100644 --- a/src/main/java/frc/robot/CommandSequences.java +++ b/src/main/java/frc/robot/CommandSequences.java @@ -25,7 +25,8 @@ public class CommandSequences { Elevator elevator; ArmRotate armRotate; - public CommandSequences(Drive drive, Led led, AlgaeDestage algaeDestage, CoralManip coralManip, Elevator elevator) { + public CommandSequences(Drive drive, Led led, AlgaeDestage algaeDestage, CoralManip coralManip, Elevator elevator, + ArmRotate armRotate) { this.drive = drive; this.led = led; this.algaeDestage = algaeDestage; @@ -38,21 +39,27 @@ public CommandSequences(Drive drive, Led led, AlgaeDestage algaeDestage, CoralMa // Intakes until sensor is tripped, LEDs indicate that game piece is acquired public Command intake() { - return armRotate.moveTo(ArmRotatePresets.INTAKE).andThen(led.elevatorToPreset(), + return armRotate.moveIntaking().andThen(led.elevatorToPreset(), elevator.moveTo(ElevatorPresets.INTAKE), led.elevatorAtPreset(), + armRotate.moveTo(ArmRotatePresets.INTAKE), led.intaking(), coralManip.intake(), led.gamePieceAcquired()); } + public Command intakeBottom() { + return armRotate.moveTo(ArmRotatePresets.INTAKE).alongWith(coralManip.intake()); + } + // Moves elevator to set coral preset - public Command moveElevator(ElevatorPresets level) { - return led.elevatorToPreset().andThen(elevator.moveTo(level), led.elevatorAtPreset()); + public Command moveElevator(ElevatorPresets level, ArmRotatePresets preset) { + return led.elevatorToPreset().andThen(armRotate.moveTo(ArmRotatePresets.OUT), elevator.moveTo(level), + led.elevatorAtPreset(), armRotate.moveTo(preset)); } // Scores on set coral preset, then stows elevator - public Command score() { - return armRotate.moveTo(ArmRotatePresets.SCOREL23).andThen(coralManip.score(), (led.elevatorToPreset()), + public Command score(ArmRotatePresets preset) { + return coralManip.score().andThen((led.elevatorToPreset()), elevator.moveTo(ElevatorPresets.STOW), led.elevatorAtPreset()); } @@ -76,15 +83,15 @@ public Command scoreL1Auto() { } public Command positionL2Auto() { - return moveElevator(ElevatorPresets.SCOREL2); + return moveElevator(ElevatorPresets.SCOREL2, ArmRotatePresets.SCOREL2); } public Command positionL3Auto() { - return moveElevator(ElevatorPresets.SCOREL3); + return moveElevator(ElevatorPresets.SCOREL3, ArmRotatePresets.SCOREL3); } public Command positionL4Auto() { - return moveElevator(ElevatorPresets.SCOREL4); + return moveElevator(ElevatorPresets.SCOREL4, ArmRotatePresets.SCOREL4); } // Scores on L1 preset, then stows elevator diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fdcc8eb..7b6ed0e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -22,9 +22,12 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.maps.RobotMap; +import frc.robot.maps.subsystems.ArmRotateMap.ArmRotatePresets; import frc.robot.maps.subsystems.ElevatorMap.ElevatorPresets; import frc.robot.subsystems.AlgaeDestage; +import frc.robot.subsystems.ArmRotate; import frc.robot.subsystems.CoralManip; import frc.robot.subsystems.DeepClimb; import frc.robot.subsystems.Drive; @@ -37,6 +40,7 @@ public final class Robot extends CommandRobot { private RobotMap map = getRobotMap(RobotMap.class, new RobotMap()); private ButtonXboxController driveController = new ButtonXboxController(0); private ButtonXboxController copilotController = new ButtonXboxController(1); + private Trigger elevatorSafeTrigger; // Helpers final DoubleUnaryOperator driveScaler = getScaler(0.45, 0.25); @@ -54,8 +58,10 @@ public final class Robot extends CommandRobot { private CoralManip coralManip = new CoralManip(map.getCoralManipMap()); private Elevator elevator = new Elevator(map.getElevatorMap()); private DeepClimb deepClimb = new DeepClimb(map.getDeepClimbMap()); + private ArmRotate armRotate = new ArmRotate(map.getArmRotateMap()); - private CommandSequences commandSequences = new CommandSequences(drive, led, algaeDestage, coralManip, elevator); + private CommandSequences commandSequences = new CommandSequences(drive, led, algaeDestage, coralManip, elevator, + armRotate); NetworkTableInstance ntinst = NetworkTableInstance.getDefault(); @@ -70,7 +76,8 @@ public void registerNamedCommands() { NamedCommands.registerCommand("Score Coral L2", commandSequences.scoreCoralAuto(ElevatorPresets.SCOREL2)); NamedCommands.registerCommand("Score Coral L3", commandSequences.scoreCoralAuto(ElevatorPresets.SCOREL3)); NamedCommands.registerCommand("Score Coral L4", commandSequences.scoreL4()); - NamedCommands.registerCommand("Stow", commandSequences.moveElevator(ElevatorPresets.STOW)); + NamedCommands.registerCommand("Stow", + commandSequences.moveElevator(ElevatorPresets.STOW, ArmRotatePresets.STOW)); NamedCommands.registerCommand("Zero Da Elevatah", elevator.zero()); } @@ -83,6 +90,7 @@ public Robot() { super(); registerNamedCommands(); autoChooser = AutoBuilder.buildAutoChooser(); + elevatorSafeTrigger = new Trigger(() -> elevator.elevatorSafeTrigger().getAsBoolean()); } @Override @@ -135,29 +143,39 @@ public void configureButtonBindings() { .whileTrue(drive.robotCentricDrive()); copilotController.a().onTrue(commandSequences.intake()); - - copilotController.x().whileTrue(commandSequences.moveElevator(ElevatorPresets.SCOREL1)) - .onFalse(commandSequences.scoreL1()); - - copilotController.b().whileTrue(commandSequences.moveElevator(ElevatorPresets.SCOREL2)) - .onFalse(commandSequences.score()); - - copilotController.y().whileTrue(commandSequences.moveElevator(ElevatorPresets.SCOREL3)) - .onFalse(commandSequences.score()); - copilotController.leftBumper().whileTrue(commandSequences.moveElevator(ElevatorPresets.SCOREL4)) - .onFalse(commandSequences.scoreL4()); + elevatorSafeTrigger.onTrue(commandSequences.intakeBottom()); + + copilotController.x() + .whileTrue(commandSequences.moveElevator(ElevatorPresets.SCOREL1, ArmRotatePresets.SCOREL1)) + .onFalse(coralManip.score()); + + copilotController.b() + .whileTrue(commandSequences.moveElevator(ElevatorPresets.SCOREL2, ArmRotatePresets.SCOREL2)) + .onFalse(coralManip.score()); + + copilotController.y() + .whileTrue(commandSequences.moveElevator(ElevatorPresets.SCOREL3, ArmRotatePresets.SCOREL3)) + .onFalse(coralManip.score()); + // copilotController.leftBumper() + // .whileTrue(commandSequences.moveElevator(ElevatorPresets.SCOREL4, + // ArmRotatePresets.SCOREL4)) + // .onFalse(commandSequences.scoreL4()); copilotController.back().onTrue(elevator.safeStateCmd()); copilotController.start().onTrue(elevator.zero()); copilotController.getPovButton(POVDirection.RIGHT).whileTrue(elevator.moveTo(ElevatorPresets.SCOREL2)) .onFalse(elevator.safeStateCmd()); copilotController.getPovButton(POVDirection.UP).whileTrue(elevator.moveTo(ElevatorPresets.SCOREL3)) .onFalse(elevator.safeStateCmd()); - copilotController.getPovButton(POVDirection.LEFT).whileTrue(elevator.moveTo(ElevatorPresets.SCOREL1)) - .onFalse(elevator.safeStateCmd()); - copilotController.getPovButton(POVDirection.DOWN).whileTrue(elevator.moveTo(ElevatorPresets.STOW)) + copilotController.getPovButton(POVDirection.LEFT).whileTrue(elevator.moveTo(ElevatorPresets.STOW)) .onFalse(elevator.safeStateCmd()); - - copilotController.rightBumper().whileTrue(coralManip.feed()); + // copilotController.getPovButton(POVDirection.DOWN).whileTrue(elevator.moveTo(ElevatorPresets.STOW)) + // .onFalse(elevator.safeStateCmd()); + copilotController.getPovButton(POVDirection.DOWN).whileTrue(coralManip.feed()); + + copilotController.rightBumper().whileTrue(armRotate.moveTo(ArmRotatePresets.OUT)); + copilotController.leftBumper() + .whileTrue(commandSequences.moveElevator(ElevatorPresets.SCOREL4, ArmRotatePresets.SCOREL4)) + .onFalse(coralManip.score()); } @Override @@ -178,8 +196,12 @@ public Command getAutoCommand() { @Override public void setDefaultCommands() { elevator.setDefaultCommand(elevator.move(RobotUtils.deadbandAxis(.1, () -> -copilotController.getLeftY()))); - deepClimb - .setDefaultCommand(deepClimb.rotate(RobotUtils.deadbandAxis(0.1, () -> copilotController.getRightY()))); + armRotate.setDefaultCommand(armRotate.move(RobotUtils.deadbandAxis(.1, () -> -copilotController.getRightY()))); + + // deepClimb + // .setDefaultCommand(deepClimb.rotate(RobotUtils.deadbandAxis(0.1, () -> + // copilotController.getRightY()))); + } public DoubleUnaryOperator getScaler(double leftRange, double rightRange) { diff --git a/src/main/java/frc/robot/maps/ColdStart.java b/src/main/java/frc/robot/maps/ColdStart.java index 4db3107..6c5bffd 100644 --- a/src/main/java/frc/robot/maps/ColdStart.java +++ b/src/main/java/frc/robot/maps/ColdStart.java @@ -12,6 +12,8 @@ import com.chopshop166.chopshoplib.motors.CSSparkFlex; import com.chopshop166.chopshoplib.motors.CSSparkMax; import com.chopshop166.chopshoplib.motors.SmartMotorControllerGroup; +import com.chopshop166.chopshoplib.sensors.CSEncoder; +import com.chopshop166.chopshoplib.sensors.IEncoder; import com.chopshop166.chopshoplib.sensors.gyro.PigeonGyro2; import com.chopshop166.chopshoplib.states.PIDValues; import com.pathplanner.lib.config.ModuleConfig; @@ -24,6 +26,7 @@ import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Translation2d; @@ -31,8 +34,10 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.AnalogEncoder; +import edu.wpi.first.wpilibj.DutyCycleEncoder; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; +import frc.robot.maps.subsystems.ArmRotateMap; import frc.robot.maps.subsystems.CoralManipMap; import frc.robot.maps.subsystems.DeepClimbMap; import frc.robot.maps.subsystems.ElevatorMap; @@ -148,25 +153,40 @@ public ElevatorMap getElevatorMap() { 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); + new ElevatorMap.ElevatorPresetValues(0, 15, 19.5, 34.5, 57.5, 57.5, 1), + new ValueRange(0, 58.25), new ValueRange(6, 53), pid, feedForward); + } + + @Override + public ArmRotateMap getArmRotateMap() { + CSSparkFlex motor = new CSSparkFlex(10); + DutyCycleEncoder absEncoder = new DutyCycleEncoder(0, 360, 212); + SparkFlexConfig config = new SparkFlexConfig(); + + config.smartCurrentLimit(30); + config.idleMode(IdleMode.kBrake); + config.inverted(true); + config.voltageCompensation(11.5); + motor.getMotorController().configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + 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, + new ValueRange(5, 97), new ValueRange(0, 94), feedForward); } @Override public CoralManipMap getCoralManipMap() { - CSSparkMax leftWheels = new CSSparkMax(9); - CSSparkMax rightWheels = new CSSparkMax(10); + CSSparkMax motor = new CSSparkMax(9); SparkMaxConfig config = new SparkMaxConfig(); config.smartCurrentLimit(30); config.idleMode(IdleMode.kBrake); - leftWheels.getMotorController().configure(config, ResetMode.kResetSafeParameters, - PersistMode.kPersistParameters); - // Right is identical to left, but inverted config.inverted(true); - rightWheels.getMotorController().configure(config, ResetMode.kResetSafeParameters, + motor.getMotorController().configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); CSDigitalInput sensor = new CSDigitalInput(9); - return new CoralManipMap(leftWheels, rightWheels, sensor::get); + return new CoralManipMap(motor, sensor::get); } @Override diff --git a/src/main/java/frc/robot/maps/RobotMap.java b/src/main/java/frc/robot/maps/RobotMap.java index fb4df7e..1cb0f34 100644 --- a/src/main/java/frc/robot/maps/RobotMap.java +++ b/src/main/java/frc/robot/maps/RobotMap.java @@ -11,6 +11,7 @@ import com.chopshop166.chopshoplib.maps.VisionMap; import frc.robot.maps.subsystems.AlgaeDestageMap; +import frc.robot.maps.subsystems.ArmRotateMap; import frc.robot.maps.subsystems.CoralManipMap; import frc.robot.maps.subsystems.DeepClimbMap; import frc.robot.maps.subsystems.ElevatorMap; @@ -50,6 +51,10 @@ public DeepClimbMap getDeepClimbMap() { return new DeepClimbMap(); } + public ArmRotateMap getArmRotateMap() { + return new ArmRotateMap(); + } + public void setupLogging() { // Pull the replay log from AdvantageScope (or prompt the user) String logPath = LogFileUtil.findReplayLog(); diff --git a/src/main/java/frc/robot/maps/subsystems/ArmRotateMap.java b/src/main/java/frc/robot/maps/subsystems/ArmRotateMap.java index ce2423a..fb53214 100644 --- a/src/main/java/frc/robot/maps/subsystems/ArmRotateMap.java +++ b/src/main/java/frc/robot/maps/subsystems/ArmRotateMap.java @@ -5,9 +5,13 @@ 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.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 { @@ -20,19 +24,25 @@ public enum ArmRotatePresets { SCOREL1, - SCOREL23, + SCOREL2, + + SCOREL3, SCOREL4, + OUT, + STOW, HOLD } - public record ArmRotatePresetValues(double intake, double scoreL1, double scoreL23, double scoreL4, double stow) { + 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); + this(0, 0, 0, 0, 0, 0, 0); } public double getValue(ArmRotatePresets preset) { @@ -43,10 +53,14 @@ public double getValue(ArmRotatePresets preset) { return intake; case SCOREL1: return scoreL1; - case SCOREL23: - return scoreL23; + case SCOREL2: + return scoreL2; + case SCOREL3: + return scoreL3; case SCOREL4: return scoreL4; + case OUT: + return out; case STOW: return stow; default: @@ -56,14 +70,21 @@ public double getValue(ArmRotatePresets preset) { } public SmartMotorController motor; - public final IEncoder encoder; + public final DutyCycleEncoder encoder; public final ArmRotatePresetValues armRotatePreset; public final ProfiledPIDController pid; public final ValueRange hardLimits; public final ValueRange softLimits; public final ArmFeedforward armFeedforward; - public ArmRotateMap(SmartMotorController motor, IEncoder encoder, ArmRotatePresetValues armRotatePreset, + public ArmRotateMap() { + this(new SmartMotorController(), new DutyCycleEncoder(0), new ArmRotatePresetValues(), + 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, ProfiledPIDController pid, ValueRange hardLimits, ValueRange softLimits, ArmFeedforward armFeedforward) { this.motor = motor; this.encoder = encoder; @@ -77,13 +98,13 @@ public ArmRotateMap(SmartMotorController motor, IEncoder encoder, ArmRotatePrese @Override public void updateData(Data data) { data.motor.updateData(motor); - data.rotationAbsAngleDegrees = encoder.getAbsolutePosition(); - data.rotatingAngleVelocity = encoder.getRate(); + data.rotationVelocity = (data.rotationAbsAngleDegrees - encoder.get()) / .02; + data.rotationAbsAngleDegrees = encoder.get(); } public static class Data extends DataWrapper { public MotorControllerData motor = new MotorControllerData(); public double rotationAbsAngleDegrees; - public double rotatingAngleVelocity; + public double rotationVelocity; } } \ No newline at end of file diff --git a/src/main/java/frc/robot/maps/subsystems/CoralManipMap.java b/src/main/java/frc/robot/maps/subsystems/CoralManipMap.java index 55a2667..dd43977 100644 --- a/src/main/java/frc/robot/maps/subsystems/CoralManipMap.java +++ b/src/main/java/frc/robot/maps/subsystems/CoralManipMap.java @@ -8,24 +8,21 @@ import com.chopshop166.chopshoplib.motors.SmartMotorController; public class CoralManipMap implements LoggableMap { - public final SmartMotorController leftMotor; - public final SmartMotorController rightMotor; + public final SmartMotorController motor; public final BooleanSupplier sensor; public CoralManipMap() { - this(new SmartMotorController(), new SmartMotorController(), () -> false); + this(new SmartMotorController(), () -> false); } - public CoralManipMap(SmartMotorController leftMotor, SmartMotorController rightMotor, BooleanSupplier sensor) { - this.leftMotor = leftMotor; - this.rightMotor = rightMotor; + public CoralManipMap(SmartMotorController motor, BooleanSupplier sensor) { + this.motor = motor; this.sensor = sensor; } @Override public void updateData(Data data) { - data.leftMotor.updateData(leftMotor); - data.rightMotor.updateData(rightMotor); + data.leftMotor.updateData(motor); data.gamePieceDetected = sensor.getAsBoolean(); } diff --git a/src/main/java/frc/robot/subsystems/ArmRotate.java b/src/main/java/frc/robot/subsystems/ArmRotate.java index fa77b72..9d4b771 100644 --- a/src/main/java/frc/robot/subsystems/ArmRotate.java +++ b/src/main/java/frc/robot/subsystems/ArmRotate.java @@ -59,12 +59,22 @@ public Command move(DoubleSupplier rotateSpeed) { public Command moveTo(ArmRotatePresets level) { PersistenceCheck setPointPersistenceCheck = new PersistenceCheck(30, pid::atGoal); return runOnce(() -> { - pid.reset(getArmAngle(), getData().rotatingAngleVelocity); + this.preset = level; + pid.reset(getArmAngle(), 0.0); }).andThen(run(() -> { - Logger.recordOutput("Pid at goal", pid.atGoal()); + Logger.recordOutput("Arm pid at goal", pid.atGoal()); }).until(setPointPersistenceCheck)).withName("Move To Set Angle"); } + public Command moveIntaking() { + return runOnce(() -> { + this.preset = ArmRotatePresets.OUT; + pid.reset(getArmAngle(), 0.0); + }).andThen(run(() -> { + Logger.recordOutput("Arm pid at goal", pid.atGoal()); + }).until(() -> (getData().rotationAbsAngleDegrees < 67))).withName("Move To Set Angle"); + } + public Command zero() { return runSafe(() -> { getData().motor.setpoint = ZEROING_SPEED; @@ -120,7 +130,7 @@ public void periodic() { @Override public void reset() { - getMap().encoder.reset(); + // Nothing to reset } @Override diff --git a/src/main/java/frc/robot/subsystems/CoralManip.java b/src/main/java/frc/robot/subsystems/CoralManip.java index 9f0dcc7..c053004 100644 --- a/src/main/java/frc/robot/subsystems/CoralManip.java +++ b/src/main/java/frc/robot/subsystems/CoralManip.java @@ -10,8 +10,8 @@ public class CoralManip extends LoggedSubsystem { - private final double RELEASE_SPEEDRIGHT = 0.5; - private final double RELEASE_SPEEDLEFT = 0.1; + private final double RELEASE_SPEEDRIGHT = -0.3; + private final double RELEASE_SPEEDLEFT = -0.1; private final double INTAKE_SPEED = -0.3; private final double RELEASE_DELAY = 1; // private final double SLOW_SPEED = . @@ -51,13 +51,10 @@ public Command feed() { } public Command intake() { - return run(() -> { + return runSafe(() -> { getData().leftMotor.setpoint = INTAKE_SPEED; getData().rightMotor.setpoint = INTAKE_SPEED; - }).until(() -> getData().gamePieceDetected).andThen(waitSeconds(DELAY), runOnce(() -> { - getData().leftMotor.setpoint = HOLD_SPEED; - getData().rightMotor.setpoint = HOLD_SPEED; - })); + }).until(() -> getData().gamePieceDetected); } @Override diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 07b0820..1d96802 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -1,5 +1,6 @@ package frc.robot.subsystems; +import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; @@ -64,25 +65,32 @@ 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()); + hold(); })).until(() -> { return setPointPersistenceCheck.getAsBoolean() && armSafeSub.getAsBoolean(); }).withName("Move to set height"); } + public BooleanSupplier elevatorSafeTrigger() { + return () -> { + return (getData().heightAbsInches < 12 && level == ElevatorPresets.INTAKE); + }; + } + public Command hold() { return runOnce(() -> { holdHeight = getElevatorHeight();