diff --git a/src/main/java/frc/team3128/commands/CmdManager.java b/src/main/java/frc/team3128/commands/CmdManager.java index f8ae85e..e0c54ec 100644 --- a/src/main/java/frc/team3128/commands/CmdManager.java +++ b/src/main/java/frc/team3128/commands/CmdManager.java @@ -174,13 +174,30 @@ public static Command ejectNote(){ ); } - public static Command feed(double rpm, double angle){ + // public static Command feed(double rpm, double angle){ + // return sequence( + // shooter.runShooter(rpm), + // swerve.turnInPlace(()-> angle), + // shooter.runKickMotor(KICK_POWER), + // hopper.intake(), + // waitSeconds(1), + // shooter.stopMotors(), + // hopper.runManipulator(0) + // ); + // } + + //better feed logic UNTESTED + public Command feed(double shooterPower, double angle){ return sequence( - shooter.runShooter(rpm), - swerve.turnInPlace(()-> angle), + parallel( + swerve.turnInPlace(()-> angle), + shooter.runShooter(shooterPower) + ), shooter.runKickMotor(KICK_POWER), - hopper.intake(), - waitSeconds(1), + waitSeconds(0.25), + hopper.runManipulator(HOPPER_INTAKE_POWER), + waitUntil(()-> !shooter.noteInRollers() || !hopper.hasObjectPresent()), + waitSeconds(0.5), shooter.stopMotors(), hopper.runManipulator(0) );