diff --git a/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollers.java b/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollers.java index 30a33b2..8e31597 100644 --- a/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollers.java +++ b/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollers.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.CoastOut; +import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; @@ -82,17 +83,7 @@ public ShooterRollers() { * the build team */ public Command shoot(AngularVelocity targetVelocity) { - - return startEnd( - () -> { - // defines a local function to set motor voltage to make it go - shooterRollerMotor1.setControl( - new VelocityVoltage(targetVelocity.in(RotationsPerSecond))); - }, - () -> { - // defines a local function to stop motor - shooterRollerMotor1.setControl(new CoastOut()); - }); + return shoot(() -> targetVelocity); } /** @@ -105,12 +96,15 @@ public Command shoot(AngularVelocity targetVelocity) { * coasts the motor on end */ public Command shoot(Supplier targetVelocity) { - return runEnd( () -> { - // defines a local function to set motor voltage to make it go - shooterRollerMotor1.setControl( - new VelocityVoltage(targetVelocity.get().in(RotationsPerSecond))); + if (getAngularVelocity().plus(ShooterRollersConstants.BANG_BANG_TOLERANCE).lt(targetVelocity.get())) { + shooterRollerMotor1.setControl(new DutyCycleOut(1)); + } else { + // defines a local function to set motor voltage to make it go + shooterRollerMotor1.setControl( + new VelocityVoltage(targetVelocity.get().in(RotationsPerSecond))); + } }, () -> { // defines a local function to stop motor diff --git a/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollersConstants.java b/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollersConstants.java index c9e1dca..69f4df5 100644 --- a/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollersConstants.java +++ b/src/main/java/frc/robot/subsystems/shooterrollers/ShooterRollersConstants.java @@ -24,6 +24,7 @@ public final class ShooterRollersConstants { public static final String CANBUS_NAME = "subsystems"; public static final MomentOfInertia MOMENT_OF_INERTIA = KilogramSquareMeters.of(0.003072714); public static final AngularVelocity FIXED_FLYWHEEL_VELOCITY = RotationsPerSecond.of(24.25); + public static final AngularVelocity BANG_BANG_TOLERANCE = RotationsPerSecond.of(4); public static final TalonFXConfiguration MOTOR_CONFIGURATION = new TalonFXConfiguration() .withFeedback(new FeedbackConfigs())