diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 4d632b8c..97731693 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -78,6 +78,7 @@ public static class Indexer { public static final double INTAKE_MOI = 0.025; public static final double FEEDER_MOI = 0.025; public static final int BEAM_BREAK_ID = 0; + public static final double VELOCITY_ERR_TOLERANCE_RPM = 10; } diff --git a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java index 41dc8eee..0b41a0f5 100644 --- a/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java +++ b/src/main/java/org/team1540/robot2024/subsystems/indexer/Indexer.java @@ -1,5 +1,6 @@ package org.team1540.robot2024.subsystems.indexer; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -52,7 +53,10 @@ public Command feedToShooter() { return Commands.runOnce(() -> io.setFeederVelocity(1200), this); } - // TODO: Add method to check if feeder is spun up + public boolean isFeederAtSetpoint() { + return Math.abs(inputs.feederVelocityError) < VELOCITY_ERR_TOLERANCE_RPM; +// return MathUtil.isNear(inputs.setpointRPM, inputs.feederVelocityRPM, VELOCITY_ERR_TOLERANCE_RPM); + } public void stopFeeder() { io.setFeederVoltage(0);