better intake that actually works

chopshop-166 · Feb 22, 2025 · e2601ca · e2601ca
1 parent a7df5cc
commit e2601ca
Showing 1 changed file with 5 additions and 4 deletions.
diff --git a/src/main/java/frc/robot/subsystems/CoralManip.java b/src/main/java/frc/robot/subsystems/CoralManip.java
@@ -63,12 +63,13 @@ public Command betterintake() {
         return run(() -> {
             getData().motor.setpoint = INTAKE_SPEED;
         }).until(() -> getData().gamePieceDetected).andThen(
-                resetCmd(), safeStateCmd(), startSafe(() -> {
+                startSafe(() -> {
                     getData().motor.setpoint = ALIGNMENT_SPEED;
-                }).until(() -> {
-                    return getMap().motor.getEncoder().getDistance() > ALIGNMENT_DISTANCE;
+                }).until(() -> !getData().gamePieceDetected),
+                startSafe(() -> {
+                    getData().motor.setpoint = ALIGNMENT_SPEED;
+                }).until(() -> getData().gamePieceDetected));
 
-                }));
     }
 
     @Override