feat: Delay testing the encoder

chopshop-166 · Feb 16, 2025 · 79d84ba · 79d84ba
1 parent cf62c06
commit 79d84ba
Showing 1 changed file with 8 additions and 2 deletions.
diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java
@@ -5,6 +5,7 @@
 import org.littletonrobotics.junction.Logger;
 
 import com.chopshop166.chopshoplib.PersistenceCheck;
+import com.chopshop166.chopshoplib.boxes.GenericBox;
 import com.chopshop166.chopshoplib.logging.LoggedSubsystem;
 
 import edu.wpi.first.math.controller.ProfiledPIDController;
@@ -24,6 +25,7 @@ public class Elevator extends LoggedSubsystem<Data, ElevatorMap> {
     final double SLOW_DOWN_COEF = 0.5;
     final double LOWER_SPEED = -0.15;
     final double ZEROING_SPEED = -0.1;
+    final int ZEROING_DELAY = 5;
     double holdHeight = 0;
 
     NetworkTableInstance instance = NetworkTableInstance.getDefault();
@@ -55,11 +57,15 @@ public Command move(DoubleSupplier liftSpeed) {
     }
 
     public Command zero() {
-        return startSafe(() -> {
+        final GenericBox<Integer> box = new GenericBox<>(0);
+        return runOnce(() -> {
             getMap().motor.resetValidators();
             level = ElevatorPresets.OFF;
             getData().motor.setpoint = ZEROING_SPEED;
-        }).until(() -> getMap().motor.validate()).andThen(resetCmd());
+            box.accept(0);
+        }).andThen(run(() -> {
+            box.accept(box.get() + 1);
+        }).until(() -> box.get() > ZEROING_DELAY && getMap().motor.errored())).andThen(safeStateCmd(), resetCmd());
     }
 
     public Command moveToZero() {