Try driving around while the intake dislodges itself. Bet it doesn't work.

Using timed while loops in a command is terrible practice because the whole assumption made by the command-based framework is that each command runs quickly once per frame (40 Hz? 60 Hz? I don't remember and I think I might have been wrong even when I was in FRC), allowing the robot to loop through everything it needs to do. The way DislodgeIntake is currently written, however, it will block execution until the intake is dislodged, preventing any other commands from running to update the robot's state. For example, try to start the robot moving forwards while the intake dislodges itself. More dangerously, try to stop the robot moving forward while the intake dislodges itself. Since DislodgeIntake.initialize() is blocking, the drive command won't be able to pass the controller input to the drive motors until the intake is dislodged, and the drive train will continue doing whatever it was doing when the intake dislodge command was scheduled.

Instead of using timed while loops to make the intake do things for a certain amount of time, the proper practice is to use a command group that will schedule timed commands in sequence, allowing other commands to continue running while the intake dislodges itself. The dislodge logic isn't a trivial case of a command sequence, but it's totally doable as one. Check out ConditionalCommand and Command.withTimeout() as good places to start.

FRC5892 / 2021-Infinite-Recharge-2.0

Try driving around while the intake dislodges itself. Bet it doesn't work. #2