A bunch of changes, mostly related to shooting balls at the hub dynamically, still need a means of doing this based on a single apriltag, in the event the robot pose is unreliable

This commit is contained in:
2026-02-12 16:02:08 -05:00
parent f8429dc899
commit 91a5281202
12 changed files with 181 additions and 135 deletions

View File

@@ -1,6 +1,7 @@
package frc.robot.subsystems;
import java.util.Optional;
import java.util.function.DoubleSupplier;
import org.littletonrobotics.junction.Logger;
@@ -69,8 +70,16 @@ public class IntakePivot extends SubsystemBase {
});
}
public Command manualSpeed(DoubleSupplier speed) {
currentTargetPosition = null;
return run(() -> {
leftMotor.set(speed.getAsDouble() * IntakePivotConstants.kMaxManualSpeedMultiplier);
});
}
public Command stop() {
return maintainPosition(null);
return manualSpeed(() -> 0);
}
public Optional<IntakePivotPosition> getCurrentTargetPosition() {