From 08d7913b04ccfcf4a4a8c1377c19037e255187af Mon Sep 17 00:00:00 2001 From: Maryam Date: Thu, 13 Aug 2026 17:14:08 -0700 Subject: [PATCH 1/5] hw done --- .../robot/subsystems/ShooterSubsystem.java | 37 +++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index ee6829c..8daf370 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -3,14 +3,22 @@ import static edu.wpi.first.units.Units.Rotations; import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.MagnetSensorConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.MotorAlignmentValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; +import com.fasterxml.jackson.databind.deser.impl.NullsConstantProvider; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -22,6 +30,12 @@ public class ShooterSubsystem extends SubsystemBase { private final LoggedTalonFX warmup1, warmup2, warmup3, shooter, hood; private final CANcoder hoodEncoder; + private final VelocityVoltage velocityRequest = new VelocityVoltage(0.0); + private final PositionVoltage positionRequest = new PositionVoltage(0.0); + + private double target_roller_speed = 0; + private double target_hood_angle = 0; + /* * make instance variables for necessary closed loop control requests: VelocityVoltage, PositionVoltage. Initialize them with value 0.0. @@ -40,29 +54,46 @@ public ShooterSubsystem() { shooter = warmup3; // warmup1 and warmup2 will be set as "followers" of the "lead" warmup3, so later we can just reference the `shooter` variable // Initialize the hood motor just like the warmup motors. You can find the necessary ID in Constants. + hood = new LoggedTalonFX("ShooterHood", Constants.Shooter.Hood.HOOD_ID, canbus); // Create a variable of type Slot0Configs called rollersSlot0Configs, and initialize it with the pid and feedforward gains found in Constants + Slot0Configs rollersSlot0Configs = new Slot0Configs().withKP(Constants.Shooter.Rollers.KP).withKI(Constants.Shooter.Rollers.KI).withKD(Constants.Shooter.Rollers.KD).withKS(Constants.Shooter.Rollers.KS).withKV(Constants.Shooter.Rollers.KV); // Create a variable of type CurrentLimitsConfigs called rollersClConfigs, and initialize it with the stator and supply limits found in Constants + CurrentLimitsConfigs rollersCLConfigs = new CurrentLimitsConfigs().withStatorCurrentLimit(Constants.Shooter.Rollers.STATOR_CURRENT_LIMIT).withSupplyCurrentLimit(Constants.Shooter.Rollers.SUPPLY_CURRENT_LIMIT); // Create a variable of type MotorOutputConifigs called rollersOutputConfigs, and initialize it to treat clockwise as positive, and neutral mode as coast + MotorOutputConfigs rollersMotorOutputConfigs = new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive).withNeutralMode(NeutralModeValue.Coast); TalonFXConfiguration rollersConfig = new TalonFXConfiguration(); // Set the Slot0, CurrentLimits, and MotorOutput parameters of `rollersConfig` to the configs you just created. + rollersConfig.withSlot0(rollersSlot0Configs); + rollersConfig.withCurrentLimits(rollersCLConfigs); + rollersConfig.withMotorOutput(rollersMotorOutputConfigs); warmup1.getConfigurator().apply(rollersConfig); // I just applied the configuration that you just created to warmup1. Apply it to warmup2 and warmup3 as well. + warmup2.getConfigurator().apply(rollersConfig); + warmup3.getConfigurator().apply(rollersConfig); Follower follower = new Follower(Constants.Shooter.Rollers.WARMUP_3_ID, MotorAlignmentValue.Aligned); // use the `setControl` method of warmup1 and 2 to configure them to use this follower configuration. + warmup1.setControl(follower); + warmup2.setControl(follower); hoodEncoder = new CANcoder(Constants.Shooter.Hood.ENCODER_PORT, Constants.Swerve.CAN_BUS); // Create a variable of type Slot0Configs called hoodSlot0Configs, and initialize it with the pid and feedforward gains found in Constants + Slot0Configs hoodSlot0Configs = new Slot0Configs().withKP(Constants.Shooter.Hood.KP).withKI(Constants.Shooter.Hood.KI).withKD(Constants.Shooter.Hood.KD).withKS(Constants.Shooter.Hood.KS).withKV(Constants.Shooter.Hood.KV).withKG(Constants.Shooter.Hood.KG); + // Create a variable of type CurrentLimitsConfigs called hoodClConfigs, and initialize it with the stator and supply limits found in Constants + CurrentLimitsConfigs hoodCLConfigs = new CurrentLimitsConfigs().withStatorCurrentLimit(Constants.Shooter.Hood.STATOR_CURRENT_LIMIT).withSupplyCurrentLimit(Constants.Shooter.Hood.SUPPLY_CURRENT_LIMIT); + // Create a variable of type MotorOutputConifigs called hoodOutputConfigs, and initialize it to treat counterclockwise as positive, and neutral mode as coast + MotorOutputConfigs hoodOutputConfigs = new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive).withNeutralMode(NeutralModeValue.Coast); + FeedbackConfigs hoodFeedbackConfigs = new FeedbackConfigs() @@ -72,9 +103,14 @@ public ShooterSubsystem() { .withRotorToSensorRatio(Constants.Shooter.Hood.MOTOR_ROTS_PER_ENCODER_ROT); // Create a TalonFXConfiguration called hoodConfig + TalonFXConfiguration hoodConfig = new TalonFXConfiguration(); // Set the Slot0, CurrentLimits, MotorOutput, and Feedback parameters to the device configs above + hoodConfig.withSlot0(hoodSlot0Configs); + hoodConfig.withCurrentLimits(hoodCLConfigs); + hoodConfig.withMotorOutput(hoodOutputConfigs); // Apply the created configuration to the hood motor + hood.getConfigurator().apply(hoodConfig); MagnetSensorConfigs hoodCANcoderConfig = new CANcoderConfiguration() @@ -83,6 +119,7 @@ public ShooterSubsystem() { .withMagnetOffset(Rotations.of(Constants.Shooter.Hood.ENCODER_OFFSET)); // Apply this config to the `hoodEncoder`. Notice how applying a device configuration is similar between motors and other devices + hoodEncoder.getConfigurator().apply(hoodCANcoderConfig); } } From 1e72c458d76c5b66a5e0dd3b27d238e7675ae475 Mon Sep 17 00:00:00 2001 From: Maryam Date: Fri, 21 Aug 2026 13:23:51 -0700 Subject: [PATCH 2/5] added functions, commands, bindings + sum constants --- src/main/java/frc/robot/Constants.java | 15 +- src/main/java/frc/robot/RobotContainer.java | 24 ++ .../robot/commandGroups/ShootBasicHood.java | 21 ++ .../robot/subsystems/ShooterSubsystem.java | 248 ++++++++++++------ 4 files changed, 216 insertions(+), 92 deletions(-) create mode 100644 src/main/java/frc/robot/commandGroups/ShootBasicHood.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c76a54a..b5c44d8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -705,12 +705,12 @@ public static final class Shooter { public static final class Hood { public static final int HOOD_ID = 12; - public static final double KP = 0.0; + public static final double KP = 2000; public static final double KI = 0.0; - public static final double KD = 0.0; - public static final double KV = 0.0; + public static final double KD = 80.0; + public static final double KV = 0.124; public static final double KS = 0.0; - public static final double KG = 0.0; + public static final double KG = 0.4; public static final double STATOR_CURRENT_LIMIT = 40.0; public static final double SUPPLY_CURRENT_LIMIT = 40.0; @@ -742,20 +742,21 @@ public static final class Hood { } public static final class Rollers { - public static final double TOLERANCE_RPS = 1.0; + public static final double TOLERANCE_RPS = 0.1; public static final int WARMUP_1_ID = 10; public static final int WARMUP_2_ID = 9; public static final int WARMUP_3_ID = 11; - public static final double KP = 0.0; + public static final double KP = 0.85; public static final double KI = 0.0; public static final double KD = 0.0; - public static final double KV = 0.0; + public static final double KV = 0.124; public static final double KS = 0.0; public static final double STATOR_CURRENT_LIMIT = 120.0; public static final double SUPPLY_CURRENT_LIMIT = 40.0; + public static final double MOTOR_ROTS_PER_WHEEL_ROTS = 15.0 / 12.0; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 44ec208..42c9337 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.commandGroups.ShootBasicHood; // * KEEP FOR WIN COMMAND TESTING import frc.robot.commands.SwerveCommands.SwerveJoystickCommand; import frc.robot.generated.TunerConstants; @@ -19,6 +20,7 @@ import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.IntakeVisionDetection; +import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.VisionSubsystem; import frc.robot.util.CustomController; import frc.robot.util.VisionUtils; @@ -41,6 +43,7 @@ public class RobotContainer { Constants.hopperOnRobot ? new HopperSubsystem() : null; public final IntakeSubsystem intakeSubsystem = Constants.intakeOnRobot ? new IntakeSubsystem() : null; + public final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(); // private final AutoRoutines autoRoutines; // private final AutoChooser autoChooser; @@ -178,6 +181,27 @@ private void configureBindings() { // joystick.y().onTrue(new InstantCommand(() -> hoodAngle-=0.2)); // joystick.a().onTrue(new InstantCommand(() -> shooterSpeed+=0.5)); // joystick.b().onTrue(new InstantCommand(() -> shooterSpeed-=0.5)); + + // Shooter + shooterSubsystem.setDefaultCommand(shooterSubsystem.runOnce(shooterSubsystem::stopShooter)); + joystick + .rightBumper() + .whileTrue( + new ShootBasicHood( + 44.2, + Constants.Shooter.Hood.MIN_HOOD_POSITION, + shooterSubsystem, + intakeSubsystem, + hopperSubsystem)); + joystick + .rightTrigger() + .whileTrue( + new ShootBasicHood( + 58.2, + Constants.Shooter.Hood.MAX_HOOD_POSITION, + shooterSubsystem, + intakeSubsystem, + hopperSubsystem)); } public static boolean isRedAlliance() { diff --git a/src/main/java/frc/robot/commandGroups/ShootBasicHood.java b/src/main/java/frc/robot/commandGroups/ShootBasicHood.java new file mode 100644 index 0000000..500ad32 --- /dev/null +++ b/src/main/java/frc/robot/commandGroups/ShootBasicHood.java @@ -0,0 +1,21 @@ +package frc.robot.commandGroups; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.ShooterSubsystem; + +public class ShootBasicHood extends ParallelCommandGroup { + public ShootBasicHood( + double speed, + double angle, + ShooterSubsystem shooterSubsystem, + IntakeSubsystem intakeSubsystem, + HopperSubsystem hopperSubsystem) { + addCommands( + shooterSubsystem.shootWithHood(speed, angle), + Commands.waitUntil(shooterSubsystem::isShooterAtSpeed) + .andThen(hopperSubsystem.runHopperUntilInterruptedCommand())); + } +} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 8daf370..b40fef8 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -1,6 +1,7 @@ package frc.robot.subsystems; import static edu.wpi.first.units.Units.Rotations; + import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; @@ -18,108 +19,185 @@ import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; -import com.fasterxml.jackson.databind.deser.impl.NullsConstantProvider; - +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.util.LoggedTalonFX; - public class ShooterSubsystem extends SubsystemBase { - - private final LoggedTalonFX warmup1, warmup2, warmup3, shooter, hood; - private final CANcoder hoodEncoder; - - private final VelocityVoltage velocityRequest = new VelocityVoltage(0.0); - private final PositionVoltage positionRequest = new PositionVoltage(0.0); - - private double target_roller_speed = 0; - private double target_hood_angle = 0; - - - /* - * make instance variables for necessary closed loop control requests: VelocityVoltage, PositionVoltage. Initialize them with value 0.0. - * make instance variables of type double for the target roller speed and target hood angle - */ - - public ShooterSubsystem() { - // The canbus is a communication system that can connect devices like the roboRIO, pdh, and motors. - CANBus canbus = Constants.Swerve.CAN_BUS; - - // LoggedTalonFX is our version of the existing TalonFX class, which automatically logs some motor information - warmup1 = new LoggedTalonFX("ShooterWarmup1", Constants.Shooter.Rollers.WARMUP_1_ID, canbus); - warmup2 = new LoggedTalonFX("ShooterWarmup2", Constants.Shooter.Rollers.WARMUP_2_ID, canbus); - warmup3 = new LoggedTalonFX("ShooterWarmup3", Constants.Shooter.Rollers.WARMUP_3_ID, canbus); - - shooter = warmup3; // warmup1 and warmup2 will be set as "followers" of the "lead" warmup3, so later we can just reference the `shooter` variable - - // Initialize the hood motor just like the warmup motors. You can find the necessary ID in Constants. - hood = new LoggedTalonFX("ShooterHood", Constants.Shooter.Hood.HOOD_ID, canbus); - - // Create a variable of type Slot0Configs called rollersSlot0Configs, and initialize it with the pid and feedforward gains found in Constants - Slot0Configs rollersSlot0Configs = new Slot0Configs().withKP(Constants.Shooter.Rollers.KP).withKI(Constants.Shooter.Rollers.KI).withKD(Constants.Shooter.Rollers.KD).withKS(Constants.Shooter.Rollers.KS).withKV(Constants.Shooter.Rollers.KV); - - // Create a variable of type CurrentLimitsConfigs called rollersClConfigs, and initialize it with the stator and supply limits found in Constants - CurrentLimitsConfigs rollersCLConfigs = new CurrentLimitsConfigs().withStatorCurrentLimit(Constants.Shooter.Rollers.STATOR_CURRENT_LIMIT).withSupplyCurrentLimit(Constants.Shooter.Rollers.SUPPLY_CURRENT_LIMIT); - - // Create a variable of type MotorOutputConifigs called rollersOutputConfigs, and initialize it to treat clockwise as positive, and neutral mode as coast - MotorOutputConfigs rollersMotorOutputConfigs = new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive).withNeutralMode(NeutralModeValue.Coast); - TalonFXConfiguration rollersConfig = new TalonFXConfiguration(); - // Set the Slot0, CurrentLimits, and MotorOutput parameters of `rollersConfig` to the configs you just created. - rollersConfig.withSlot0(rollersSlot0Configs); - rollersConfig.withCurrentLimits(rollersCLConfigs); - rollersConfig.withMotorOutput(rollersMotorOutputConfigs); - - warmup1.getConfigurator().apply(rollersConfig); - // I just applied the configuration that you just created to warmup1. Apply it to warmup2 and warmup3 as well. - warmup2.getConfigurator().apply(rollersConfig); - warmup3.getConfigurator().apply(rollersConfig); - - Follower follower = new Follower(Constants.Shooter.Rollers.WARMUP_3_ID, MotorAlignmentValue.Aligned); - // use the `setControl` method of warmup1 and 2 to configure them to use this follower configuration. - warmup1.setControl(follower); - warmup2.setControl(follower); - - hoodEncoder = new CANcoder(Constants.Shooter.Hood.ENCODER_PORT, Constants.Swerve.CAN_BUS); - - // Create a variable of type Slot0Configs called hoodSlot0Configs, and initialize it with the pid and feedforward gains found in Constants - Slot0Configs hoodSlot0Configs = new Slot0Configs().withKP(Constants.Shooter.Hood.KP).withKI(Constants.Shooter.Hood.KI).withKD(Constants.Shooter.Hood.KD).withKS(Constants.Shooter.Hood.KS).withKV(Constants.Shooter.Hood.KV).withKG(Constants.Shooter.Hood.KG); - - - // Create a variable of type CurrentLimitsConfigs called hoodClConfigs, and initialize it with the stator and supply limits found in Constants - CurrentLimitsConfigs hoodCLConfigs = new CurrentLimitsConfigs().withStatorCurrentLimit(Constants.Shooter.Hood.STATOR_CURRENT_LIMIT).withSupplyCurrentLimit(Constants.Shooter.Hood.SUPPLY_CURRENT_LIMIT); - - - // Create a variable of type MotorOutputConifigs called hoodOutputConfigs, and initialize it to treat counterclockwise as positive, and neutral mode as coast - MotorOutputConfigs hoodOutputConfigs = new MotorOutputConfigs().withInverted(InvertedValue.CounterClockwise_Positive).withNeutralMode(NeutralModeValue.Coast); - - - FeedbackConfigs hoodFeedbackConfigs = + private final LoggedTalonFX warmup1, warmup2, warmup3, shooter, hood; + private final CANcoder hoodEncoder; + + private final VelocityVoltage velocityRequest = new VelocityVoltage(0.0); + private final PositionVoltage positionRequest = new PositionVoltage(0.0); + + private double targetShooterSpeed = 0; + private double targetHoodAngle = 0; + + /* + * make instance variables for necessary closed loop control requests: VelocityVoltage, PositionVoltage. Initialize them with value 0.0. + * make instance variables of type double for the target roller speed and target hood angle + */ + + public ShooterSubsystem() { + // The canbus is a communication system that can connect devices like the roboRIO, pdh, and + // motors. + CANBus canbus = Constants.Swerve.CAN_BUS; + + // LoggedTalonFX is our version of the existing TalonFX class, which automatically logs some + // motor information + warmup1 = new LoggedTalonFX("ShooterWarmup1", Constants.Shooter.Rollers.WARMUP_1_ID, canbus); + warmup2 = new LoggedTalonFX("ShooterWarmup2", Constants.Shooter.Rollers.WARMUP_2_ID, canbus); + warmup3 = new LoggedTalonFX("ShooterWarmup3", Constants.Shooter.Rollers.WARMUP_3_ID, canbus); + + shooter = + warmup3; // warmup1 and warmup2 will be set as "followers" of the "lead" warmup3, so later + // we can just reference the `shooter` variable + + // Initialize the hood motor just like the warmup motors. You can find the necessary ID in + // Constants. + hood = new LoggedTalonFX("ShooterHood", Constants.Shooter.Hood.HOOD_ID, canbus); + + // Create a variable of type Slot0Configs called rollersSlot0Configs, and initialize it with the + // pid and feedforward gains found in Constants + Slot0Configs rollersSlot0Configs = + new Slot0Configs() + .withKP(Constants.Shooter.Rollers.KP) + .withKI(Constants.Shooter.Rollers.KI) + .withKD(Constants.Shooter.Rollers.KD) + .withKS(Constants.Shooter.Rollers.KS) + .withKV(Constants.Shooter.Rollers.KV); + + // Create a variable of type CurrentLimitsConfigs called rollersClConfigs, and initialize it + // with the stator and supply limits found in Constants + CurrentLimitsConfigs rollersCLConfigs = + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Constants.Shooter.Rollers.STATOR_CURRENT_LIMIT) + .withSupplyCurrentLimit(Constants.Shooter.Rollers.SUPPLY_CURRENT_LIMIT); + + // Create a variable of type MotorOutputConifigs called rollersOutputConfigs, and initialize it + // to treat clockwise as positive, and neutral mode as coast + MotorOutputConfigs rollersMotorOutputConfigs = + new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Coast); + + TalonFXConfiguration rollersConfig = new TalonFXConfiguration(); + // Set the Slot0, CurrentLimits, and MotorOutput parameters of `rollersConfig` to the configs + // you just created. + rollersConfig.withSlot0(rollersSlot0Configs); + rollersConfig.withCurrentLimits(rollersCLConfigs); + rollersConfig.withMotorOutput(rollersMotorOutputConfigs); + + warmup1.getConfigurator().apply(rollersConfig); + // I just applied the configuration that you just created to warmup1. Apply it to warmup2 and + // warmup3 as well. + warmup2.getConfigurator().apply(rollersConfig); + warmup3.getConfigurator().apply(rollersConfig); + + Follower follower = + new Follower(Constants.Shooter.Rollers.WARMUP_3_ID, MotorAlignmentValue.Aligned); + // use the `setControl` method of warmup1 and 2 to configure them to use this follower + // configuration. + warmup1.setControl(follower); + warmup2.setControl(follower); + + hoodEncoder = new CANcoder(Constants.Shooter.Hood.ENCODER_PORT, Constants.Swerve.CAN_BUS); + + // Create a variable of type Slot0Configs called hoodSlot0Configs, and initialize it with the + // pid and feedforward gains found in Constants + Slot0Configs hoodSlot0Configs = + new Slot0Configs() + .withKP(Constants.Shooter.Hood.KP) + .withKI(Constants.Shooter.Hood.KI) + .withKD(Constants.Shooter.Hood.KD) + .withKS(Constants.Shooter.Hood.KS) + .withKV(Constants.Shooter.Hood.KV) + .withKG(Constants.Shooter.Hood.KG); + + // Create a variable of type CurrentLimitsConfigs called hoodClConfigs, and initialize it with + // the stator and supply limits found in Constants + CurrentLimitsConfigs hoodCLConfigs = + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Constants.Shooter.Hood.STATOR_CURRENT_LIMIT) + .withSupplyCurrentLimit(Constants.Shooter.Hood.SUPPLY_CURRENT_LIMIT); + + // Create a variable of type MotorOutputConifigs called hoodOutputConfigs, and initialize it to + // treat counterclockwise as positive, and neutral mode as coast + MotorOutputConfigs hoodOutputConfigs = + new MotorOutputConfigs() + .withInverted(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Coast); + + FeedbackConfigs hoodFeedbackConfigs = new FeedbackConfigs() .withFeedbackRemoteSensorID(hoodEncoder.getDeviceID()) .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder) .withSensorToMechanismRatio(Constants.Shooter.Hood.ENCODER_ROTS_PER_HOOD_ROT) .withRotorToSensorRatio(Constants.Shooter.Hood.MOTOR_ROTS_PER_ENCODER_ROT); - // Create a TalonFXConfiguration called hoodConfig - TalonFXConfiguration hoodConfig = new TalonFXConfiguration(); - // Set the Slot0, CurrentLimits, MotorOutput, and Feedback parameters to the device configs above - hoodConfig.withSlot0(hoodSlot0Configs); - hoodConfig.withCurrentLimits(hoodCLConfigs); - hoodConfig.withMotorOutput(hoodOutputConfigs); + // Create a TalonFXConfiguration called hoodConfig + TalonFXConfiguration hoodConfig = new TalonFXConfiguration(); + // Set the Slot0, CurrentLimits, MotorOutput, and Feedback parameters to the device configs + // above + hoodConfig.withSlot0(hoodSlot0Configs); + hoodConfig.withCurrentLimits(hoodCLConfigs); + hoodConfig.withMotorOutput(hoodOutputConfigs); - // Apply the created configuration to the hood motor - hood.getConfigurator().apply(hoodConfig); + // Apply the created configuration to the hood motor + hood.getConfigurator().apply(hoodConfig); - MagnetSensorConfigs hoodCANcoderConfig = + MagnetSensorConfigs hoodCANcoderConfig = new CANcoderConfiguration() .MagnetSensor.withAbsoluteSensorDiscontinuityPoint(Rotations.of(1)) .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) .withMagnetOffset(Rotations.of(Constants.Shooter.Hood.ENCODER_OFFSET)); - // Apply this config to the `hoodEncoder`. Notice how applying a device configuration is similar between motors and other devices - hoodEncoder.getConfigurator().apply(hoodCANcoderConfig); - - } + // Apply this config to the `hoodEncoder`. Notice how applying a device configuration is similar + // between motors and other devices + hoodEncoder.getConfigurator().apply(hoodCANcoderConfig); + } + + public void setHoodAngle(double degrees) { + degrees = + MathUtil.clamp( + degrees, + Constants.Shooter.Hood.MIN_HOOD_POSITION, + Constants.Shooter.Hood.MAX_HOOD_POSITION); + targetHoodAngle = degrees; + hood.setControl(positionRequest.withPosition(degrees / 360)); + } + + public void setShooterSpeed(double velocityRps) { + targetShooterSpeed = velocityRps; + warmup3.setControl( + velocityRequest.withVelocity( + velocityRps * Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROTS)); + } + + public boolean isShooterAtSpeed() { + return Math.abs( + (warmup3.getCachedVelocityRps() / Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROTS) + - targetShooterSpeed) + <= Constants.Shooter.Rollers.TOLERANCE_RPS; + } + + public void stopShooter() { + targetShooterSpeed = 0; + warmup3.stopMotor(); + } + + // Commands + + public Command shootWithHood(double shooterSpeed, double hoodAngle) { + return Commands.runEnd( + () -> { + setHoodAngle(hoodAngle); + setShooterSpeed(shooterSpeed); + }, + this::stopShooter); + } } From dea39522ee32c866750a80385fc2376b9d1a5dec Mon Sep 17 00:00:00 2001 From: Maryam Date: Mon, 24 Aug 2026 20:00:43 -0700 Subject: [PATCH 3/5] sum rando changes idk --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 10 +++---- .../robot/commandGroups/ShootBasicHood.java | 8 ++--- .../robot/subsystems/ShooterSubsystem.java | 29 ++++++++++--------- 4 files changed, 25 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b5c44d8..62bd110 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -742,7 +742,7 @@ public static final class Hood { } public static final class Rollers { - public static final double TOLERANCE_RPS = 0.1; + public static final double TOLERANCE_RPS = 1.0; public static final int WARMUP_1_ID = 10; public static final int WARMUP_2_ID = 9; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 42c9337..0cb1645 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -188,19 +188,17 @@ private void configureBindings() { .rightBumper() .whileTrue( new ShootBasicHood( - 44.2, - Constants.Shooter.Hood.MIN_HOOD_POSITION, + () -> 44.2, + () -> Constants.Shooter.Hood.MIN_HOOD_POSITION, shooterSubsystem, - intakeSubsystem, hopperSubsystem)); joystick .rightTrigger() .whileTrue( new ShootBasicHood( - 58.2, - Constants.Shooter.Hood.MAX_HOOD_POSITION, + () -> 58.2, + () -> Constants.Shooter.Hood.MAX_HOOD_POSITION, shooterSubsystem, - intakeSubsystem, hopperSubsystem)); } diff --git a/src/main/java/frc/robot/commandGroups/ShootBasicHood.java b/src/main/java/frc/robot/commandGroups/ShootBasicHood.java index 500ad32..86ca2f1 100644 --- a/src/main/java/frc/robot/commandGroups/ShootBasicHood.java +++ b/src/main/java/frc/robot/commandGroups/ShootBasicHood.java @@ -5,16 +5,16 @@ import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; +import java.util.function.DoubleSupplier; public class ShootBasicHood extends ParallelCommandGroup { public ShootBasicHood( - double speed, - double angle, + DoubleSupplier speed, + DoubleSupplier angle, ShooterSubsystem shooterSubsystem, - IntakeSubsystem intakeSubsystem, HopperSubsystem hopperSubsystem) { addCommands( - shooterSubsystem.shootWithHood(speed, angle), + shooterSubsystem.shootWithHood(speed.getAsDouble(), angle.getAsDouble()), Commands.waitUntil(shooterSubsystem::isShooterAtSpeed) .andThen(hopperSubsystem.runHopperUntilInterruptedCommand())); } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index b40fef8..d947f44 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -85,12 +85,13 @@ public ShooterSubsystem() { .withInverted(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Coast); - TalonFXConfiguration rollersConfig = new TalonFXConfiguration(); + TalonFXConfiguration rollersConfig = + new TalonFXConfiguration() + .withSlot0(rollersSlot0Configs) + .withCurrentLimits(rollersCLConfigs) + .withMotorOutput(rollersMotorOutputConfigs); // Set the Slot0, CurrentLimits, and MotorOutput parameters of `rollersConfig` to the configs // you just created. - rollersConfig.withSlot0(rollersSlot0Configs); - rollersConfig.withCurrentLimits(rollersCLConfigs); - rollersConfig.withMotorOutput(rollersMotorOutputConfigs); warmup1.getConfigurator().apply(rollersConfig); // I just applied the configuration that you just created to warmup1. Apply it to warmup2 and @@ -140,12 +141,14 @@ public ShooterSubsystem() { .withRotorToSensorRatio(Constants.Shooter.Hood.MOTOR_ROTS_PER_ENCODER_ROT); // Create a TalonFXConfiguration called hoodConfig - TalonFXConfiguration hoodConfig = new TalonFXConfiguration(); + TalonFXConfiguration hoodConfig = + new TalonFXConfiguration() + .withSlot0(hoodSlot0Configs) + .withCurrentLimits(hoodCLConfigs) + .withMotorOutput(hoodOutputConfigs) + .withFeedback(hoodFeedbackConfigs); // Set the Slot0, CurrentLimits, MotorOutput, and Feedback parameters to the device configs // above - hoodConfig.withSlot0(hoodSlot0Configs); - hoodConfig.withCurrentLimits(hoodCLConfigs); - hoodConfig.withMotorOutput(hoodOutputConfigs); // Apply the created configuration to the hood motor hood.getConfigurator().apply(hoodConfig); @@ -168,26 +171,26 @@ public void setHoodAngle(double degrees) { Constants.Shooter.Hood.MIN_HOOD_POSITION, Constants.Shooter.Hood.MAX_HOOD_POSITION); targetHoodAngle = degrees; - hood.setControl(positionRequest.withPosition(degrees / 360)); + hood.setControl(positionRequest.withPosition(degrees / 360.0)); } public void setShooterSpeed(double velocityRps) { targetShooterSpeed = velocityRps; - warmup3.setControl( + shooter.setControl( velocityRequest.withVelocity( velocityRps * Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROTS)); } public boolean isShooterAtSpeed() { return Math.abs( - (warmup3.getCachedVelocityRps() / Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROTS) + (shooter.getCachedVelocityRps() / Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROTS) - targetShooterSpeed) <= Constants.Shooter.Rollers.TOLERANCE_RPS; } public void stopShooter() { targetShooterSpeed = 0; - warmup3.stopMotor(); + shooter.stopMotor(); } // Commands @@ -195,8 +198,8 @@ public void stopShooter() { public Command shootWithHood(double shooterSpeed, double hoodAngle) { return Commands.runEnd( () -> { - setHoodAngle(hoodAngle); setShooterSpeed(shooterSpeed); + setHoodAngle(hoodAngle); }, this::stopShooter); } From fcff30fd1b309ffcd288930054b1490678c7c7be Mon Sep 17 00:00:00 2001 From: Maryam Date: Wed, 2 Sep 2026 20:17:50 -0700 Subject: [PATCH 4/5] changed to shootbasichood command to inline in robotcontainer --- src/main/java/frc/robot/RobotContainer.java | 45 +++++++++++++-------- 1 file changed, 29 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0cb1645..21040cf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,6 +26,7 @@ import frc.robot.util.VisionUtils; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; +import edu.wpi.first.wpilibj2.command.Commands; public class RobotContainer { private BooleanSupplier redside = RobotContainer::isRedAlliance; @@ -184,22 +185,34 @@ private void configureBindings() { // Shooter shooterSubsystem.setDefaultCommand(shooterSubsystem.runOnce(shooterSubsystem::stopShooter)); - joystick - .rightBumper() - .whileTrue( - new ShootBasicHood( - () -> 44.2, - () -> Constants.Shooter.Hood.MIN_HOOD_POSITION, - shooterSubsystem, - hopperSubsystem)); - joystick - .rightTrigger() - .whileTrue( - new ShootBasicHood( - () -> 58.2, - () -> Constants.Shooter.Hood.MAX_HOOD_POSITION, - shooterSubsystem, - hopperSubsystem)); + + // joystick + // .rightBumper() + // .whileTrue( + // new ShootBasicHood( + // () -> 44.2, + // () -> Constants.Shooter.Hood.MIN_HOOD_POSITION, + // shooterSubsystem, + // hopperSubsystem)); + + joystick.rightBumper() + .whileTrue(shooterSubsystem.shootWithHood(44.2, Constants.Shooter.Hood.MIN_HOOD_POSITION) + .alongWith(Commands.waitUntil(shooterSubsystem::isShooterAtSpeed) + .andThen(hopperSubsystem.runHopperUntilInterruptedCommand()))); + // joystick + // .rightTrigger() + // .whileTrue( + // new ShootBasicHood( + // () -> 58.2, + // () -> Constants.Shooter.Hood.MAX_HOOD_POSITION, + // shooterSubsystem, + // hopperSubsystem)); + + joystick.rightTrigger() + .whileTrue(shooterSubsystem.shootWithHood(58.2, Constants.Shooter.Hood.MAX_HOOD_POSITION) + .alongWith(Commands.waitUntil(shooterSubsystem::isShooterAtSpeed) + .andThen(hopperSubsystem.runHopperUntilInterruptedCommand()))); + } public static boolean isRedAlliance() { From e50818dfa1824e4d5bd73eb1144d566221b79b20 Mon Sep 17 00:00:00 2001 From: Maryam Date: Wed, 2 Sep 2026 20:44:43 -0700 Subject: [PATCH 5/5] copied working code into shooterSubsystem but shooter is a little off still --- src/main/java/frc/robot/RobotContainer.java | 28 +++--- .../robot/commandGroups/ShootBasicHood.java | 1 - .../robot/subsystems/ShooterSubsystem.java | 88 +++++++++---------- 3 files changed, 60 insertions(+), 57 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 21040cf..781fc87 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,8 +10,8 @@ // import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.commandGroups.ShootBasicHood; // * KEEP FOR WIN COMMAND TESTING import frc.robot.commands.SwerveCommands.SwerveJoystickCommand; import frc.robot.generated.TunerConstants; @@ -26,7 +26,6 @@ import frc.robot.util.VisionUtils; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; -import edu.wpi.first.wpilibj2.command.Commands; public class RobotContainer { private BooleanSupplier redside = RobotContainer::isRedAlliance; @@ -195,10 +194,14 @@ private void configureBindings() { // shooterSubsystem, // hopperSubsystem)); - joystick.rightBumper() - .whileTrue(shooterSubsystem.shootWithHood(44.2, Constants.Shooter.Hood.MIN_HOOD_POSITION) - .alongWith(Commands.waitUntil(shooterSubsystem::isShooterAtSpeed) - .andThen(hopperSubsystem.runHopperUntilInterruptedCommand()))); + joystick + .rightBumper() + .whileTrue( + shooterSubsystem + .shootWithHood(44.2, Constants.Shooter.Hood.MIN_HOOD_POSITION) + .alongWith( + Commands.waitUntil(shooterSubsystem::isShooterAtSpeed) + .andThen(hopperSubsystem.runHopperUntilInterruptedCommand()))); // joystick // .rightTrigger() // .whileTrue( @@ -208,11 +211,14 @@ private void configureBindings() { // shooterSubsystem, // hopperSubsystem)); - joystick.rightTrigger() - .whileTrue(shooterSubsystem.shootWithHood(58.2, Constants.Shooter.Hood.MAX_HOOD_POSITION) - .alongWith(Commands.waitUntil(shooterSubsystem::isShooterAtSpeed) - .andThen(hopperSubsystem.runHopperUntilInterruptedCommand()))); - + joystick + .rightTrigger() + .whileTrue( + shooterSubsystem + .shootWithHood(58.2, Constants.Shooter.Hood.MAX_HOOD_POSITION) + .alongWith( + Commands.waitUntil(shooterSubsystem::isShooterAtSpeed) + .andThen(hopperSubsystem.runHopperUntilInterruptedCommand()))); } public static boolean isRedAlliance() { diff --git a/src/main/java/frc/robot/commandGroups/ShootBasicHood.java b/src/main/java/frc/robot/commandGroups/ShootBasicHood.java index 86ca2f1..7d38005 100644 --- a/src/main/java/frc/robot/commandGroups/ShootBasicHood.java +++ b/src/main/java/frc/robot/commandGroups/ShootBasicHood.java @@ -3,7 +3,6 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import frc.robot.subsystems.HopperSubsystem; -import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; import java.util.function.DoubleSupplier; diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index d947f44..abf014b 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -21,7 +21,6 @@ import com.ctre.phoenix6.signals.SensorDirectionValue; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.util.LoggedTalonFX; @@ -31,22 +30,24 @@ public class ShooterSubsystem extends SubsystemBase { private final LoggedTalonFX warmup1, warmup2, warmup3, shooter, hood; private final CANcoder hoodEncoder; - private final VelocityVoltage velocityRequest = new VelocityVoltage(0.0); - private final PositionVoltage positionRequest = new PositionVoltage(0.0); - - private double targetShooterSpeed = 0; - private double targetHoodAngle = 0; - /* * make instance variables for necessary closed loop control requests: VelocityVoltage, PositionVoltage. Initialize them with value 0.0. * make instance variables of type double for the target roller speed and target hood angle */ + private VelocityVoltage m_velocityReq = new VelocityVoltage(0.0); + private PositionVoltage m_positionReq = new PositionVoltage(0.0); + + private double targetShooterSpeed, targetHoodAngle; + public ShooterSubsystem() { // The canbus is a communication system that can connect devices like the roboRIO, pdh, and // motors. CANBus canbus = Constants.Swerve.CAN_BUS; + targetShooterSpeed = 0; + targetHoodAngle = 0; + // LoggedTalonFX is our version of the existing TalonFX class, which automatically logs some // motor information warmup1 = new LoggedTalonFX("ShooterWarmup1", Constants.Shooter.Rollers.WARMUP_1_ID, canbus); @@ -73,14 +74,14 @@ public ShooterSubsystem() { // Create a variable of type CurrentLimitsConfigs called rollersClConfigs, and initialize it // with the stator and supply limits found in Constants - CurrentLimitsConfigs rollersCLConfigs = + CurrentLimitsConfigs rollersClConfigs = new CurrentLimitsConfigs() .withStatorCurrentLimit(Constants.Shooter.Rollers.STATOR_CURRENT_LIMIT) .withSupplyCurrentLimit(Constants.Shooter.Rollers.SUPPLY_CURRENT_LIMIT); // Create a variable of type MotorOutputConifigs called rollersOutputConfigs, and initialize it // to treat clockwise as positive, and neutral mode as coast - MotorOutputConfigs rollersMotorOutputConfigs = + MotorOutputConfigs rollersOutputConfigs = new MotorOutputConfigs() .withInverted(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Coast); @@ -88,16 +89,16 @@ public ShooterSubsystem() { TalonFXConfiguration rollersConfig = new TalonFXConfiguration() .withSlot0(rollersSlot0Configs) - .withCurrentLimits(rollersCLConfigs) - .withMotorOutput(rollersMotorOutputConfigs); + .withCurrentLimits(rollersClConfigs) + .withMotorOutput(rollersOutputConfigs); // Set the Slot0, CurrentLimits, and MotorOutput parameters of `rollersConfig` to the configs // you just created. warmup1.getConfigurator().apply(rollersConfig); - // I just applied the configuration that you just created to warmup1. Apply it to warmup2 and - // warmup3 as well. warmup2.getConfigurator().apply(rollersConfig); warmup3.getConfigurator().apply(rollersConfig); + // I just applied the configuration that you just created to warmup1. Apply it to warmup2 and + // warmup3 as well. Follower follower = new Follower(Constants.Shooter.Rollers.WARMUP_3_ID, MotorAlignmentValue.Aligned); @@ -116,12 +117,12 @@ public ShooterSubsystem() { .withKI(Constants.Shooter.Hood.KI) .withKD(Constants.Shooter.Hood.KD) .withKS(Constants.Shooter.Hood.KS) - .withKV(Constants.Shooter.Hood.KV) - .withKG(Constants.Shooter.Hood.KG); + .withKG(Constants.Shooter.Hood.KG) + .withKV(Constants.Shooter.Hood.KV); // Create a variable of type CurrentLimitsConfigs called hoodClConfigs, and initialize it with // the stator and supply limits found in Constants - CurrentLimitsConfigs hoodCLConfigs = + CurrentLimitsConfigs hoodClConfigs = new CurrentLimitsConfigs() .withStatorCurrentLimit(Constants.Shooter.Hood.STATOR_CURRENT_LIMIT) .withSupplyCurrentLimit(Constants.Shooter.Hood.SUPPLY_CURRENT_LIMIT); @@ -141,17 +142,16 @@ public ShooterSubsystem() { .withRotorToSensorRatio(Constants.Shooter.Hood.MOTOR_ROTS_PER_ENCODER_ROT); // Create a TalonFXConfiguration called hoodConfig - TalonFXConfiguration hoodConfig = - new TalonFXConfiguration() - .withSlot0(hoodSlot0Configs) - .withCurrentLimits(hoodCLConfigs) - .withMotorOutput(hoodOutputConfigs) - .withFeedback(hoodFeedbackConfigs); // Set the Slot0, CurrentLimits, MotorOutput, and Feedback parameters to the device configs // above - // Apply the created configuration to the hood motor - hood.getConfigurator().apply(hoodConfig); + hood.getConfigurator() + .apply( + new TalonFXConfiguration() + .withSlot0(hoodSlot0Configs) + .withCurrentLimits(hoodClConfigs) + .withMotorOutput(hoodOutputConfigs) + .withFeedback(hoodFeedbackConfigs)); MagnetSensorConfigs hoodCANcoderConfig = new CANcoderConfiguration() @@ -165,41 +165,39 @@ public ShooterSubsystem() { } public void setHoodAngle(double degrees) { - degrees = - MathUtil.clamp( - degrees, - Constants.Shooter.Hood.MIN_HOOD_POSITION, - Constants.Shooter.Hood.MAX_HOOD_POSITION); targetHoodAngle = degrees; - hood.setControl(positionRequest.withPosition(degrees / 360.0)); + hood.setControl( + m_positionReq.withPosition( + MathUtil.clamp( + targetHoodAngle, + Constants.Shooter.Hood.MIN_HOOD_POSITION, + Constants.Shooter.Hood.MAX_HOOD_POSITION) + / 360.0)); } public void setShooterSpeed(double velocityRps) { targetShooterSpeed = velocityRps; shooter.setControl( - velocityRequest.withVelocity( - velocityRps * Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROTS)); - } - - public boolean isShooterAtSpeed() { - return Math.abs( - (shooter.getCachedVelocityRps() / Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROTS) - - targetShooterSpeed) - <= Constants.Shooter.Rollers.TOLERANCE_RPS; + m_velocityReq.withVelocity( + targetShooterSpeed * Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROTS)); } public void stopShooter() { - targetShooterSpeed = 0; + targetShooterSpeed = 0.0; shooter.stopMotor(); } - // Commands + public boolean isShooterAtSpeed() { + double currentVelocity = + shooter.getCachedVelocityRps() / Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROTS; + return Math.abs(currentVelocity - targetShooterSpeed) <= 1.0; + } - public Command shootWithHood(double shooterSpeed, double hoodAngle) { - return Commands.runEnd( + public Command shootWithHood(double hoodAngleDegrees, double shooterVelocityRps) { + return runEnd( () -> { - setShooterSpeed(shooterSpeed); - setHoodAngle(hoodAngle); + setShooterSpeed(shooterVelocityRps); + setHoodAngle(hoodAngleDegrees); }, this::stopShooter); }