diff --git a/src/main/java/frc/robot/auto/Paths.java b/src/main/java/frc/robot/auto/Paths.java index c137b1e5..1db17ece 100644 --- a/src/main/java/frc/robot/auto/Paths.java +++ b/src/main/java/frc/robot/auto/Paths.java @@ -142,7 +142,9 @@ public static Command get2BallDefenseMidTarmac1BallSide() { return twoBallTarmacSideSegment .andThen(getShootCommand(5)) .andThen(twoBallTarmacDefenseSideSegment) - .andThen(new InstantCommand(() -> CommandScheduler.getInstance().schedule(outtake))); + .andThen(new InstantCommand(() -> CommandScheduler.getInstance().schedule(outtake))) + .andThen(new WaitCommand(4)) + .andThen(new InstantCommand(() -> CommandScheduler.getInstance().cancel(outtake))); // .andThen(outtake); } diff --git a/src/main/java/frc/robot/commands/shooter/SetShooterPIDVelocityFromState.java b/src/main/java/frc/robot/commands/shooter/SetShooterPIDVelocityFromState.java index 0aa36ce3..da9bcd59 100644 --- a/src/main/java/frc/robot/commands/shooter/SetShooterPIDVelocityFromState.java +++ b/src/main/java/frc/robot/commands/shooter/SetShooterPIDVelocityFromState.java @@ -43,6 +43,7 @@ public void execute() { double pidOutput; + flywheelSubsystem.setTargetVelocity(shooterStateSupplier.get().rpmVelocity); if (shooterStateSupplier.get().rpmVelocity < 3500){ pidOutput = flywheelControllerLow.calculate(flywheelSubsystem.getFlywheelRPM(), shooterStateSupplier.get().rpmVelocity); } else { diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 77e33f51..a430b8fd 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -30,7 +30,7 @@ public class IntakeSubsystem extends SubsystemBase { private final DoubleSolenoid rightIntakeSolenoid; public IntakeSubsystem() { - TalonConfiguration config = new TalonConfiguration(NeutralMode.Coast, InvertType.None); + TalonConfiguration config = new TalonConfiguration(NeutralMode.Coast, InvertType.InvertMotorOutput); intakeMotor = TalonFXFactory.createTalonFX(INTAKE_MOTOR_ID, config, MANI_CAN_BUS); leftintakeSolenoid = new DoubleSolenoid(PNEUMATICS_HUB_ID, PneumaticsModuleType.REVPH, INTAKE_SOLENOID_LEFT_FORWARD, INTAKE_SOLENOID_LEFT_BACKWARD); rightIntakeSolenoid = new DoubleSolenoid(PNEUMATICS_HUB_ID, PneumaticsModuleType.REVPH, INTAKE_SOLENOID_RIGHT_FORWARD, INTAKE_SOLENOID_RIGHT_BACKWARD);