From 11760eb2def0f1a839dd9f90e25181a76ea554b8 Mon Sep 17 00:00:00 2001 From: Powaffle Date: Thu, 13 Aug 2026 01:56:03 -0700 Subject: [PATCH 1/2] homework week 5 --- .../robot/subsystems/ShooterSubsystem.java | 71 ++++++++++++++++--- 1 file changed, 62 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index ee6829c..2b1e652 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -3,15 +3,23 @@ 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 edu.wpi.first.units.measure.Current; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.util.LoggedTalonFX; @@ -28,6 +36,10 @@ public class ShooterSubsystem extends SubsystemBase { * make instance variables of type double for the target roller speed and target hood angle */ + private final VelocityVoltage m_velocityVoltageRequest = new VelocityVoltage(0.0); + private final PositionVoltage m_positionVoltageRequest = new PositionVoltage(0.0); + private final double targetRollerSpeed, 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; @@ -40,29 +52,64 @@ 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(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) + .withKV(Constants.Shooter.Rollers.KV) + .withKS(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() + .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 - - TalonFXConfiguration rollersConfig = new TalonFXConfiguration(); + MotorOutputConfigs rollersOutputConfigs = + new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Coast); + + TalonFXConfiguration rollersConfig = + new TalonFXConfiguration() + .withSlot0(rollersSlot0Configs) + .withCurrentLimits(rollersClConfigs) + .withMotorOutput(rollersOutputConfigs); // Set the Slot0, CurrentLimits, and MotorOutput parameters of `rollersConfig` to the configs you just created. warmup1.getConfigurator().apply(rollersConfig); + 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); + warmup1.setControl(follower); + warmup2.setControl(follower); // use the `setControl` method of warmup1 and 2 to configure them to use this follower configuration. 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) + .withKV(Constants.Shooter.Hood.KV) + .withKS(Constants.Shooter.Hood.KS) + .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,8 +119,14 @@ 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 - + hood.getConfigurator().apply(hoodConfig); // Apply the created configuration to the hood motor MagnetSensorConfigs hoodCANcoderConfig = @@ -83,6 +136,6 @@ 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 85d3510df2b19239c1a574a1a6e267691beead59 Mon Sep 17 00:00:00 2001 From: Powaffle Date: Sun, 23 Aug 2026 15:47:10 -0700 Subject: [PATCH 2/2] homework week 6 --- src/main/java/frc/robot/Constants.java | 14 ++++++----- src/main/java/frc/robot/RobotContainer.java | 24 +++++++++++++++++++ .../robot/commandGroups/ShootBasicHood.java | 21 ++++++++++++++++ 3 files changed, 53 insertions(+), 6 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..fb05f5b 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 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; @@ -744,14 +744,16 @@ public static final class Hood { public static final class Rollers { public static final double TOLERANCE_RPS = 1.0; + public static final double MOTOR_ROTS_PER_WHEEL_ROT = 15.0 / 12.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..6e0c69f 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,8 @@ public class RobotContainer { Constants.hopperOnRobot ? new HopperSubsystem() : null; public final IntakeSubsystem intakeSubsystem = Constants.intakeOnRobot ? new IntakeSubsystem() : null; + public final ShooterSubsystem shooterSubsystem = + Constants.shooterOnRobot ? new ShooterSubsystem() : null; // private final AutoRoutines autoRoutines; // private final AutoChooser autoChooser; @@ -178,6 +182,26 @@ 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)); + + 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())); + } +}