From a7bfd24c2628f5e8c9df09562114c43665c4f75a Mon Sep 17 00:00:00 2001 From: fatema Date: Thu, 13 Aug 2026 16:04:19 -0700 Subject: [PATCH 1/4] shooter --- .../robot/subsystems/ShooterSubsystem.java | 74 +++++++++++++------ 1 file changed, 52 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index ee6829c..845fb26 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -3,13 +3,20 @@ 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.VelocityVoltage; +import com.ctre.phoenix6.controls.PositionVoltage; 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 edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -22,47 +29,68 @@ public class ShooterSubsystem extends SubsystemBase { private final LoggedTalonFX warmup1, warmup2, warmup3, shooter, hood; private final CANcoder hoodEncoder; + VelocityVoltage mVelocityVoltage = new VelocityVoltage(0.0); + PositionVoltage mPositionVoltage = new PositionVoltage(0.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 - */ + double targetRoller; + double targetAngle; 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 + shooter = warmup3; - // Initialize the hood motor just like the warmup motors. You can find the necessary ID in Constants. + hood = new LoggedTalonFX("Hood", 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(); + rollersSlot0Configs.kP = Constants.Shooter.Rollers.KP; + rollersSlot0Configs.kI = Constants.Shooter.Rollers.KI; + rollersSlot0Configs.kD = Constants.Shooter.Rollers.KD; + rollersSlot0Configs.kV = Constants.Shooter.Rollers.KV; + rollersSlot0Configs.kS = Constants.Shooter.Rollers.KS; - // Create a variable of type CurrentLimitsConfigs called rollersClConfigs, and initialize it with the stator and supply limits found in Constants + CurrentLimitsConfigs rollersCLConfigs = new CurrentLimitsConfigs(); + rollersCLConfigs.StatorCurrentLimit = Constants.Shooter.Rollers.STATOR_CURRENT_LIMIT; + rollersCLConfigs.SupplyCurrentLimit = 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 rollersOutputConfigs = new MotorOutputConfigs(); + rollersOutputConfigs.Inverted = InvertedValue.Clockwise_Positive; + rollersOutputConfigs.NeutralMode = NeutralModeValue.Coast; TalonFXConfiguration rollersConfig = new TalonFXConfiguration(); - // Set the Slot0, CurrentLimits, and MotorOutput parameters of `rollersConfig` to the configs you just created. + rollersConfig.Slot0 = rollersSlot0Configs; + rollersConfig.CurrentLimits = rollersCLConfigs; + rollersConfig.MotorOutput = rollersOutputConfigs; 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(); + hoodSlot0Configs.kP = Constants.Shooter.Hood.KP; + hoodSlot0Configs.kI = Constants.Shooter.Hood.KI; + hoodSlot0Configs.kD = Constants.Shooter.Hood.KD; + hoodSlot0Configs.kV = Constants.Shooter.Hood.KV; + hoodSlot0Configs.kS = Constants.Shooter.Hood.KS; + hoodSlot0Configs.kG = 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(); + hoodClConfigs.StatorCurrentLimit = Constants.Shooter.Hood.STATOR_CURRENT_LIMIT; + hoodClConfigs.SupplyCurrentLimit = 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(); + hoodOutputConfigs.Inverted = InvertedValue.CounterClockwise_Positive; + hoodOutputConfigs.NeutralMode = NeutralModeValue.Coast; FeedbackConfigs hoodFeedbackConfigs = new FeedbackConfigs() @@ -71,18 +99,20 @@ public ShooterSubsystem() { .withSensorToMechanismRatio(Constants.Shooter.Hood.ENCODER_ROTS_PER_HOOD_ROT) .withRotorToSensorRatio(Constants.Shooter.Hood.MOTOR_ROTS_PER_ENCODER_ROT); - // Create a TalonFXConfiguration called hoodConfig - // Set the Slot0, CurrentLimits, MotorOutput, and Feedback parameters to the device configs above + TalonFXConfiguration hoodConfig = new TalonFXConfiguration(); + hoodConfig.Slot0 = hoodSlot0Configs; + hoodConfig.CurrentLimits = hoodClConfigs; + hoodConfig.MotorOutput = hoodOutputConfigs; - // Apply the created configuration to the hood motor + hood.getConfigurator().apply(hoodConfig); 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); } } From bd712e57c81a566b61545315042526193883acac Mon Sep 17 00:00:00 2001 From: fatema Date: Sun, 23 Aug 2026 21:55:35 -0700 Subject: [PATCH 2/4] hw 6 --- src/main/java/frc/robot/Constants.java | 17 ++++--- src/main/java/frc/robot/RobotContainer.java | 7 +++ .../robot/commandGroups/ShootBasicHood.java | 13 +++++ .../robot/subsystems/ShooterSubsystem.java | 51 +++++++++++++------ 4 files changed, 67 insertions(+), 21 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..756fa42 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -703,14 +703,17 @@ public static enum TargetingMode { public static final class Shooter { public static final class Hood { + public static final double MIN_HOOD_ANGLE = 3.8; + public static final double MAX_HOOD_ANGLE = 18.369; + 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 KD = 80.0; public static final double KV = 0.0; - public static final double KS = 0.0; - public static final double KG = 0.0; + public static final double KS = 0.124; + 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,16 +745,18 @@ public static final class Hood { } public static final class Rollers { + public static final double MOTOR_ROTS_PER_WHEEL_ROT = 15.0/12.0; + public static final double TOLERANCE_RPS = 1.0; 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; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 44ec208..00a8ed8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,12 +13,14 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; // * KEEP FOR WIN COMMAND TESTING import frc.robot.commands.SwerveCommands.SwerveJoystickCommand; +import frc.robot.commandGroups.ShootBasicHood; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.FuelGaugeDetection; 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; @@ -123,6 +126,10 @@ private void configureBindings() { joystick.x().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric)); drivetrain.setDefaultCommand(swerveJoystickDefaultCommand); + shooterSubsystem.setDefaultCommand(shooterSubsystem.runOnce(shooterSubsystem::stopShooter)); + joystick.rightBumper().whileTrue(new ShootBasicHood(44.2, Constants.Shooter.Hood.MIN_HOOD_ANGLE, shooterSubsystem, hopperSubsystem)); + joystick.rightTrigger().whileTrue(new ShootBasicHood(58.2, Constants.Shooter.Hood.MAX_HOOD_ANGLE, shooterSubsystem, hopperSubsystem)); + // Intake intakeSubsystem.setDefaultCommand(intakeSubsystem.intakeDefault()); joystick.leftBumper().whileTrue(intakeSubsystem.intakeUntilInterruptedCommand()); 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..a7155f7 --- /dev/null +++ b/src/main/java/frc/robot/commandGroups/ShootBasicHood.java @@ -0,0 +1,13 @@ +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.ShooterSubsystem; + +public class ShootBasicHood extends ParallelCommandGroup{ + public ShootBasicHood(double speed, double angle, ShooterSubsystem shooter, HopperSubsystem hopper){ + addCommands(shooter.shootWithHood(speed, angle), Commands.waitUntil(shooter::isShooterAtSpeed) + .andThen(hopper.runHopperUntilInterruptedCommand())); + } +} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 845fb26..92f01b4 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -19,25 +19,26 @@ import com.ctre.phoenix6.signals.NeutralModeValue; 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.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; VelocityVoltage mVelocityVoltage = new VelocityVoltage(0.0); PositionVoltage mPositionVoltage = new PositionVoltage(0.0); - double targetRoller; - double targetAngle; + double targetShooterSpeed = 0; + double targetHoodAngle = 0; public ShooterSubsystem() { CANBus canbus = Constants.Swerve.CAN_BUS; - + 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); @@ -92,12 +93,11 @@ public ShooterSubsystem() { hoodOutputConfigs.Inverted = InvertedValue.CounterClockwise_Positive; hoodOutputConfigs.NeutralMode = 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); + 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); TalonFXConfiguration hoodConfig = new TalonFXConfiguration(); hoodConfig.Slot0 = hoodSlot0Configs; @@ -106,13 +106,34 @@ public ShooterSubsystem() { hood.getConfigurator().apply(hoodConfig); - MagnetSensorConfigs hoodCANcoderConfig = - new CANcoderConfiguration() - .MagnetSensor.withAbsoluteSensorDiscontinuityPoint(Rotations.of(1)) + MagnetSensorConfigs hoodCANcoderConfig = new CANcoderConfiguration().MagnetSensor + .withAbsoluteSensorDiscontinuityPoint(Rotations.of(1)) .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) .withMagnetOffset(Rotations.of(Constants.Shooter.Hood.ENCODER_OFFSET)); - + hoodEncoder.getConfigurator().apply(hoodCANcoderConfig); + } + public void setHoodAngle(double degrees) { + targetHoodAngle = degrees; + hood.setControl(mPositionVoltage.withPosition(MathUtil.clamp(degrees, Constants.Shooter.Hood.MIN_HOOD_ANGLE, Constants.Shooter.Hood.MAX_HOOD_ANGLE)/360)); } + public void setShooterSpeed(double velocityRps){ + targetShooterSpeed = velocityRps; + shooter.setControl(mVelocityVoltage.withVelocity(velocityRps/360)); + } + public void stopShooter(){ + targetShooterSpeed = 0.0; + shooter.stopMotor(); + } + public boolean isShooterAtSpeed(){ + if(Math.abs((shooter.getCachedVelocityRps()/360) - targetShooterSpeed) <= 1.0){ + return true; + } + return false; + } + public Command shootWithHood(double targetSpeed, double targetAngle){ + return runEnd(() -> {setShooterSpeed(targetSpeed); setHoodAngle(targetAngle);}, this::stopShooter); + } + } From 3e0430a4b629be49a64e1ffb7a8a7e23ba3a9841 Mon Sep 17 00:00:00 2001 From: fatema Date: Mon, 24 Aug 2026 18:42:30 -0700 Subject: [PATCH 3/4] hw 6 again --- src/main/java/frc/robot/Constants.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 14 +- .../robot/commandGroups/ShootBasicHood.java | 11 +- .../robot/subsystems/ShooterSubsystem.java | 191 ++++++++++-------- 4 files changed, 124 insertions(+), 96 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 756fa42..02552e5 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -705,7 +705,7 @@ public static final class Shooter { public static final class Hood { public static final double MIN_HOOD_ANGLE = 3.8; public static final double MAX_HOOD_ANGLE = 18.369; - + public static final int HOOD_ID = 12; public static final double KP = 2000; @@ -745,7 +745,7 @@ public static final class Hood { } public static final class Rollers { - public static final double MOTOR_ROTS_PER_WHEEL_ROT = 15.0/12.0; + public static final double MOTOR_ROTS_PER_WHEEL_ROT = 15.0 / 12.0; public static final double TOLERANCE_RPS = 1.0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 00a8ed8..be9e35e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,8 +12,8 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; // * KEEP FOR WIN COMMAND TESTING -import frc.robot.commands.SwerveCommands.SwerveJoystickCommand; import frc.robot.commandGroups.ShootBasicHood; +import frc.robot.commands.SwerveCommands.SwerveJoystickCommand; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.FuelGaugeDetection; @@ -127,8 +127,16 @@ private void configureBindings() { drivetrain.setDefaultCommand(swerveJoystickDefaultCommand); shooterSubsystem.setDefaultCommand(shooterSubsystem.runOnce(shooterSubsystem::stopShooter)); - joystick.rightBumper().whileTrue(new ShootBasicHood(44.2, Constants.Shooter.Hood.MIN_HOOD_ANGLE, shooterSubsystem, hopperSubsystem)); - joystick.rightTrigger().whileTrue(new ShootBasicHood(58.2, Constants.Shooter.Hood.MAX_HOOD_ANGLE, shooterSubsystem, hopperSubsystem)); + joystick + .rightBumper() + .whileTrue( + new ShootBasicHood( + 44.2, Constants.Shooter.Hood.MIN_HOOD_ANGLE, shooterSubsystem, hopperSubsystem)); + joystick + .rightTrigger() + .whileTrue( + new ShootBasicHood( + 58.2, Constants.Shooter.Hood.MAX_HOOD_ANGLE, shooterSubsystem, hopperSubsystem)); // Intake intakeSubsystem.setDefaultCommand(intakeSubsystem.intakeDefault()); diff --git a/src/main/java/frc/robot/commandGroups/ShootBasicHood.java b/src/main/java/frc/robot/commandGroups/ShootBasicHood.java index a7155f7..aca1d24 100644 --- a/src/main/java/frc/robot/commandGroups/ShootBasicHood.java +++ b/src/main/java/frc/robot/commandGroups/ShootBasicHood.java @@ -5,9 +5,12 @@ import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.ShooterSubsystem; -public class ShootBasicHood extends ParallelCommandGroup{ - public ShootBasicHood(double speed, double angle, ShooterSubsystem shooter, HopperSubsystem hopper){ - addCommands(shooter.shootWithHood(speed, angle), Commands.waitUntil(shooter::isShooterAtSpeed) +public class ShootBasicHood extends ParallelCommandGroup { + public ShootBasicHood( + double speed, double angle, ShooterSubsystem shooter, HopperSubsystem hopper) { + addCommands( + shooter.shootWithHood(speed, angle), + Commands.waitUntil(shooter::isShooterAtSpeed) .andThen(hopper.runHopperUntilInterruptedCommand())); - } + } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 92f01b4..352a8e6 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; @@ -10,15 +11,14 @@ import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.VelocityVoltage; 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 edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -27,113 +27,130 @@ public class ShooterSubsystem extends SubsystemBase { - private final LoggedTalonFX warmup1, warmup2, warmup3, shooter, hood; - private final CANcoder hoodEncoder; + private final LoggedTalonFX warmup1, warmup2, warmup3, shooter, hood; + private final CANcoder hoodEncoder; - VelocityVoltage mVelocityVoltage = new VelocityVoltage(0.0); - PositionVoltage mPositionVoltage = new PositionVoltage(0.0); + VelocityVoltage mVelocityVoltage = new VelocityVoltage(0.0); + PositionVoltage mPositionVoltage = new PositionVoltage(0.0); - double targetShooterSpeed = 0; - double targetHoodAngle = 0; + double targetShooterSpeed = 0; + double targetHoodAngle = 0; - public ShooterSubsystem() { - CANBus canbus = Constants.Swerve.CAN_BUS; + public ShooterSubsystem() { + CANBus canbus = Constants.Swerve.CAN_BUS; - 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); + 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; + shooter = warmup3; - hood = new LoggedTalonFX("Hood", Constants.Shooter.Hood.HOOD_ID, canbus); + hood = new LoggedTalonFX("Hood", Constants.Shooter.Hood.HOOD_ID, canbus); - Slot0Configs rollersSlot0Configs = new Slot0Configs(); - rollersSlot0Configs.kP = Constants.Shooter.Rollers.KP; - rollersSlot0Configs.kI = Constants.Shooter.Rollers.KI; - rollersSlot0Configs.kD = Constants.Shooter.Rollers.KD; - rollersSlot0Configs.kV = Constants.Shooter.Rollers.KV; - rollersSlot0Configs.kS = Constants.Shooter.Rollers.KS; + Slot0Configs rollersSlot0Configs = new Slot0Configs(); + rollersSlot0Configs.kP = Constants.Shooter.Rollers.KP; + rollersSlot0Configs.kI = Constants.Shooter.Rollers.KI; + rollersSlot0Configs.kD = Constants.Shooter.Rollers.KD; + rollersSlot0Configs.kV = Constants.Shooter.Rollers.KV; + rollersSlot0Configs.kS = Constants.Shooter.Rollers.KS; - CurrentLimitsConfigs rollersCLConfigs = new CurrentLimitsConfigs(); - rollersCLConfigs.StatorCurrentLimit = Constants.Shooter.Rollers.STATOR_CURRENT_LIMIT; - rollersCLConfigs.SupplyCurrentLimit = Constants.Shooter.Rollers.SUPPLY_CURRENT_LIMIT; + CurrentLimitsConfigs rollersCLConfigs = new CurrentLimitsConfigs(); + rollersCLConfigs.StatorCurrentLimit = Constants.Shooter.Rollers.STATOR_CURRENT_LIMIT; + rollersCLConfigs.SupplyCurrentLimit = Constants.Shooter.Rollers.SUPPLY_CURRENT_LIMIT; - MotorOutputConfigs rollersOutputConfigs = new MotorOutputConfigs(); - rollersOutputConfigs.Inverted = InvertedValue.Clockwise_Positive; - rollersOutputConfigs.NeutralMode = NeutralModeValue.Coast; + MotorOutputConfigs rollersOutputConfigs = new MotorOutputConfigs(); + rollersOutputConfigs.Inverted = InvertedValue.Clockwise_Positive; + rollersOutputConfigs.NeutralMode = NeutralModeValue.Coast; - TalonFXConfiguration rollersConfig = new TalonFXConfiguration(); - rollersConfig.Slot0 = rollersSlot0Configs; - rollersConfig.CurrentLimits = rollersCLConfigs; - rollersConfig.MotorOutput = rollersOutputConfigs; + TalonFXConfiguration rollersConfig = new TalonFXConfiguration(); + rollersConfig.Slot0 = rollersSlot0Configs; + rollersConfig.CurrentLimits = rollersCLConfigs; + rollersConfig.MotorOutput = rollersOutputConfigs; - warmup1.getConfigurator().apply(rollersConfig); - warmup2.getConfigurator().apply(rollersConfig); - warmup3.getConfigurator().apply(rollersConfig); + warmup1.getConfigurator().apply(rollersConfig); + warmup2.getConfigurator().apply(rollersConfig); + warmup3.getConfigurator().apply(rollersConfig); - Follower follower = new Follower(Constants.Shooter.Rollers.WARMUP_3_ID, MotorAlignmentValue.Aligned); - warmup1.setControl(follower); - warmup2.setControl(follower); + Follower follower = + new Follower(Constants.Shooter.Rollers.WARMUP_3_ID, MotorAlignmentValue.Aligned); + warmup1.setControl(follower); + warmup2.setControl(follower); - hoodEncoder = new CANcoder(Constants.Shooter.Hood.ENCODER_PORT, Constants.Swerve.CAN_BUS); + hoodEncoder = new CANcoder(Constants.Shooter.Hood.ENCODER_PORT, Constants.Swerve.CAN_BUS); - Slot0Configs hoodSlot0Configs = new Slot0Configs(); - hoodSlot0Configs.kP = Constants.Shooter.Hood.KP; - hoodSlot0Configs.kI = Constants.Shooter.Hood.KI; - hoodSlot0Configs.kD = Constants.Shooter.Hood.KD; - hoodSlot0Configs.kV = Constants.Shooter.Hood.KV; - hoodSlot0Configs.kS = Constants.Shooter.Hood.KS; - hoodSlot0Configs.kG = Constants.Shooter.Hood.KG; + Slot0Configs hoodSlot0Configs = new Slot0Configs(); + hoodSlot0Configs.kP = Constants.Shooter.Hood.KP; + hoodSlot0Configs.kI = Constants.Shooter.Hood.KI; + hoodSlot0Configs.kD = Constants.Shooter.Hood.KD; + hoodSlot0Configs.kV = Constants.Shooter.Hood.KV; + hoodSlot0Configs.kS = Constants.Shooter.Hood.KS; + hoodSlot0Configs.kG = Constants.Shooter.Hood.KG; - CurrentLimitsConfigs hoodClConfigs = new CurrentLimitsConfigs(); - hoodClConfigs.StatorCurrentLimit = Constants.Shooter.Hood.STATOR_CURRENT_LIMIT; - hoodClConfigs.SupplyCurrentLimit = Constants.Shooter.Hood.SUPPLY_CURRENT_LIMIT; + CurrentLimitsConfigs hoodClConfigs = new CurrentLimitsConfigs(); + hoodClConfigs.StatorCurrentLimit = Constants.Shooter.Hood.STATOR_CURRENT_LIMIT; + hoodClConfigs.SupplyCurrentLimit = Constants.Shooter.Hood.SUPPLY_CURRENT_LIMIT; - MotorOutputConfigs hoodOutputConfigs = new MotorOutputConfigs(); - hoodOutputConfigs.Inverted = InvertedValue.CounterClockwise_Positive; - hoodOutputConfigs.NeutralMode = NeutralModeValue.Coast; + MotorOutputConfigs hoodOutputConfigs = new MotorOutputConfigs(); + hoodOutputConfigs.Inverted = InvertedValue.CounterClockwise_Positive; + hoodOutputConfigs.NeutralMode = 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); + 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); - TalonFXConfiguration hoodConfig = new TalonFXConfiguration(); - hoodConfig.Slot0 = hoodSlot0Configs; - hoodConfig.CurrentLimits = hoodClConfigs; - hoodConfig.MotorOutput = hoodOutputConfigs; + TalonFXConfiguration hoodConfig = new TalonFXConfiguration(); + hoodConfig.Slot0 = hoodSlot0Configs; + hoodConfig.CurrentLimits = hoodClConfigs; + hoodConfig.MotorOutput = hoodOutputConfigs; - hood.getConfigurator().apply(hoodConfig); + hood.getConfigurator().apply(hoodConfig); - MagnetSensorConfigs hoodCANcoderConfig = new CANcoderConfiguration().MagnetSensor - .withAbsoluteSensorDiscontinuityPoint(Rotations.of(1)) + MagnetSensorConfigs hoodCANcoderConfig = + new CANcoderConfiguration() + .MagnetSensor.withAbsoluteSensorDiscontinuityPoint(Rotations.of(1)) .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) .withMagnetOffset(Rotations.of(Constants.Shooter.Hood.ENCODER_OFFSET)); - hoodEncoder.getConfigurator().apply(hoodCANcoderConfig); - } - - public void setHoodAngle(double degrees) { - targetHoodAngle = degrees; - hood.setControl(mPositionVoltage.withPosition(MathUtil.clamp(degrees, Constants.Shooter.Hood.MIN_HOOD_ANGLE, Constants.Shooter.Hood.MAX_HOOD_ANGLE)/360)); - } - public void setShooterSpeed(double velocityRps){ - targetShooterSpeed = velocityRps; - shooter.setControl(mVelocityVoltage.withVelocity(velocityRps/360)); - } - public void stopShooter(){ - targetShooterSpeed = 0.0; - shooter.stopMotor(); - } - public boolean isShooterAtSpeed(){ - if(Math.abs((shooter.getCachedVelocityRps()/360) - targetShooterSpeed) <= 1.0){ - return true; - } - return false; - } - public Command shootWithHood(double targetSpeed, double targetAngle){ - return runEnd(() -> {setShooterSpeed(targetSpeed); setHoodAngle(targetAngle);}, this::stopShooter); + hoodEncoder.getConfigurator().apply(hoodCANcoderConfig); + } + + public void setHoodAngle(double degrees) { + targetHoodAngle = degrees; + hood.setControl( + mPositionVoltage.withPosition( + MathUtil.clamp( + degrees, + Constants.Shooter.Hood.MIN_HOOD_ANGLE, + Constants.Shooter.Hood.MAX_HOOD_ANGLE) + / 360)); + } + + public void setShooterSpeed(double velocityRps) { + targetShooterSpeed = velocityRps; + shooter.setControl(mVelocityVoltage.withVelocity(velocityRps / 360)); + } + + public void stopShooter() { + targetShooterSpeed = 0.0; + shooter.stopMotor(); + } + + public boolean isShooterAtSpeed() { + if (Math.abs((shooter.getCachedVelocityRps() / 360) - targetShooterSpeed) <= 1.0) { + return true; } - + return false; + } + + public Command shootWithHood(double targetSpeed, double targetAngle) { + return runEnd( + () -> { + setShooterSpeed(targetSpeed); + setHoodAngle(targetAngle); + }, + this::stopShooter); + } } From a5cdba8c92172b3cb025115ce7eeb5fe0a94e7a4 Mon Sep 17 00:00:00 2001 From: fatema Date: Wed, 2 Sep 2026 20:16:56 -0700 Subject: [PATCH 4/4] changed shootbasichood --- src/main/java/frc/robot/RobotContainer.java | 189 ++++++++++---------- 1 file changed, 92 insertions(+), 97 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index be9e35e..54f3fd4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,6 +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.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; // * KEEP FOR WIN COMMAND TESTING import frc.robot.commandGroups.ShootBasicHood; @@ -28,79 +30,71 @@ import java.util.function.DoubleSupplier; public class RobotContainer { - private BooleanSupplier redside = RobotContainer::isRedAlliance; - - // private Field2d field = new Field2d(); - private final Telemetry logger = - new Telemetry(Constants.Swerve.PHYSICAL_MAX_SPEED_METERS_PER_SECOND); - - private final CommandXboxController joystick = new CommandXboxController(0); - private final CustomController secondController = new CustomController(4); - - public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); - - public final HopperSubsystem hopperSubsystem = - 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; - - public final VisionSubsystem visionFrontRight = - Constants.visionOnRobot - ? new VisionSubsystem(Constants.Vision.VisionCamera.FRONT_RIGHT_CAM, drivetrain) - : null; - public final VisionSubsystem visionFrontLeft = - Constants.visionOnRobot - ? new VisionSubsystem(Constants.Vision.VisionCamera.FRONT_LEFT_CAM, drivetrain) - : null; - public final VisionSubsystem visionRearRight = - Constants.visionOnRobot - ? new VisionSubsystem(Constants.Vision.VisionCamera.REAR_RIGHT_CAM, drivetrain) - : null; - public final VisionSubsystem visionRearLeft = - Constants.visionOnRobot - ? new VisionSubsystem(Constants.Vision.VisionCamera.REAR_LEFT_CAM, drivetrain) - : null; - - public final FuelGaugeDetection visionFuelGauge = - Constants.fuelGaugeOnRobot - ? new FuelGaugeDetection(Constants.FuelGaugeDetection.FuelGaugeCamera.FUEL_GAUGE_CAM) - : null; - public final IntakeVisionDetection visionIntake = - Constants.intakeVisionOnRobot - ? new IntakeVisionDetection(Constants.IntakeVision.IntakeVisionCamera.INTAKE_CAM) - : null; - - // * KEEP FOR INTERMAP TESTING - // private double hoodAngle = 18.369; - // private double shooterSpeed = 58.0; - - public RobotContainer() { - // autoRoutines = new AutoRoutines(intakeSubsystem, lebron, hopperSubsystem, drivetrain, - // redside); - // autoChooser = autoRoutines.getAutoChooser(); - // SmartDashboard.putData("Auto Chooser", autoChooser); - // SmartDashboard.putData("Elastic/Field2d", field); - configureBindings(); - } + private BooleanSupplier redside = RobotContainer::isRedAlliance; + + // private Field2d field = new Field2d(); + private final Telemetry logger = new Telemetry(Constants.Swerve.PHYSICAL_MAX_SPEED_METERS_PER_SECOND); + + private final CommandXboxController joystick = new CommandXboxController(0); + private final CustomController secondController = new CustomController(4); + + public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + + public final HopperSubsystem hopperSubsystem = 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; + + public final VisionSubsystem visionFrontRight = Constants.visionOnRobot + ? new VisionSubsystem(Constants.Vision.VisionCamera.FRONT_RIGHT_CAM, drivetrain) + : null; + public final VisionSubsystem visionFrontLeft = Constants.visionOnRobot + ? new VisionSubsystem(Constants.Vision.VisionCamera.FRONT_LEFT_CAM, drivetrain) + : null; + public final VisionSubsystem visionRearRight = Constants.visionOnRobot + ? new VisionSubsystem(Constants.Vision.VisionCamera.REAR_RIGHT_CAM, drivetrain) + : null; + public final VisionSubsystem visionRearLeft = Constants.visionOnRobot + ? new VisionSubsystem(Constants.Vision.VisionCamera.REAR_LEFT_CAM, drivetrain) + : null; + + public final FuelGaugeDetection visionFuelGauge = Constants.fuelGaugeOnRobot + ? new FuelGaugeDetection(Constants.FuelGaugeDetection.FuelGaugeCamera.FUEL_GAUGE_CAM) + : null; + public final IntakeVisionDetection visionIntake = Constants.intakeVisionOnRobot + ? new IntakeVisionDetection(Constants.IntakeVision.IntakeVisionCamera.INTAKE_CAM) + : null; + + // * KEEP FOR INTERMAP TESTING + // private double hoodAngle = 18.369; + // private double shooterSpeed = 58.0; + + public RobotContainer() { + // autoRoutines = new AutoRoutines(intakeSubsystem, lebron, hopperSubsystem, + // drivetrain, + // redside); + // autoChooser = autoRoutines.getAutoChooser(); + // SmartDashboard.putData("Auto Chooser", autoChooser); + // SmartDashboard.putData("Elastic/Field2d", field); + configureBindings(); + } - // public void doTelemetry() { - // logger.telemeterize(drivetrain.getCurrentState()); + // public void doTelemetry() { + // logger.telemeterize(drivetrain.getCurrentState()); - // String commandName = "nah"; + // String commandName = "nah"; - // if (drivetrain.getCurrentCommand() != null) { - // commandName = drivetrain.getCurrentCommand().getName(); - // } - // DogLog.log("Robot/SwerveDriveCommand", commandName); - // } + // if (drivetrain.getCurrentCommand() != null) { + // commandName = drivetrain.getCurrentCommand().getName(); + // } + // DogLog.log("Robot/SwerveDriveCommand", commandName); + // } - // public CommandSwerveDrivetrain getDrivetrain() { - // return drivetrain; - // } + // public CommandSwerveDrivetrain getDrivetrain() { + // return drivetrain; + // } private void configureBindings() { // Swerve @@ -129,14 +123,15 @@ private void configureBindings() { shooterSubsystem.setDefaultCommand(shooterSubsystem.runOnce(shooterSubsystem::stopShooter)); joystick .rightBumper() - .whileTrue( - new ShootBasicHood( - 44.2, Constants.Shooter.Hood.MIN_HOOD_ANGLE, shooterSubsystem, hopperSubsystem)); + .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_ANGLE, shooterSubsystem, hopperSubsystem)); + shooterSubsystem.shootWithHood(58.2, Constants.Shooter.Hood.MAX_HOOD_POSITION).alongWith( + Commands.waitUntil(shooterSubsystem::isShooterAtSpeed) + .andThen(hopperSubsystem.runHopperUntilInterruptedCommand()))); // Intake intakeSubsystem.setDefaultCommand(intakeSubsystem.intakeDefault()); @@ -195,34 +190,34 @@ private void configureBindings() { // joystick.b().onTrue(new InstantCommand(() -> shooterSpeed-=0.5)); } - public static boolean isRedAlliance() { - return DriverStation.getAlliance().isEmpty() - ? false - : DriverStation.getAlliance().get() == Alliance.Red; - } + public static boolean isRedAlliance() { + return DriverStation.getAlliance().isEmpty() + ? false + : DriverStation.getAlliance().get() == Alliance.Red; + } - public void visionPeriodic() { - VisionUtils.visionPeriodic( - visionFrontRight, visionFrontLeft, visionRearRight, visionRearLeft, drivetrain); - } + public void visionPeriodic() { + VisionUtils.visionPeriodic( + visionFrontRight, visionFrontLeft, visionRearRight, visionRearLeft, drivetrain); + } - public void doTelemetry() { - logger.telemeterize(drivetrain.getCurrentState()); + public void doTelemetry() { + logger.telemeterize(drivetrain.getCurrentState()); - String commandName = "nah"; + String commandName = "nah"; - if (drivetrain.getCurrentCommand() != null) { - commandName = drivetrain.getCurrentCommand().getName(); + if (drivetrain.getCurrentCommand() != null) { + commandName = drivetrain.getCurrentCommand().getName(); + } } - } - public boolean inAllianceSide() { - return redside.getAsBoolean() - ? drivetrain.getPose().getX() > Constants.Landmarks.RED_HUB.getX() - : drivetrain.getPose().getX() < Constants.Landmarks.BLUE_HUB.getX(); - } + public boolean inAllianceSide() { + return redside.getAsBoolean() + ? drivetrain.getPose().getX() > Constants.Landmarks.RED_HUB.getX() + : drivetrain.getPose().getX() < Constants.Landmarks.BLUE_HUB.getX(); + } - // public Command getAutonomousCommand() { - // return autoChooser.selectedCommand(); - // } + // public Command getAutonomousCommand() { + // return autoChooser.selectedCommand(); + // } }