feat: isFeederAtSetpoint

flamingchickens1540 · Feb 5, 2024 · 3fd56d7 · 3fd56d7
1 parent f942fee
commit 3fd56d7
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 1 deletion.
diff --git 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
@@ -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);