From e8c5ac30a1f0de91f055c496836c12d8f466af21 Mon Sep 17 00:00:00 2001 From: ItsGetWreckt Date: Wed, 12 Aug 2026 15:45:55 -0700 Subject: [PATCH 1/8] week 5 hw --- .../robot/subsystems/ShooterSubsystem.java | 193 ++++++++++++------ 1 file changed, 126 insertions(+), 67 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index ee6829c..b839f69 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -3,86 +3,145 @@ 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.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; 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; - - - /* - * 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. - - // Create a variable of type Slot0Configs called rollersSlot0Configs, and initialize it with the pid and feedforward gains found in Constants - - // Create a variable of type CurrentLimitsConfigs called rollersClConfigs, and initialize it with the stator and supply limits found in Constants - - // 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(); - // 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. - - 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. - - 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 - - // Create a variable of type CurrentLimitsConfigs called hoodClConfigs, and initialize it with the stator and supply limits found in Constants - - // Create a variable of type MotorOutputConifigs called hoodOutputConfigs, and initialize it to treat counterclockwise as positive, and neutral mode as 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 - // Set the Slot0, CurrentLimits, MotorOutput, and Feedback parameters to the device configs above - - // Apply the created configuration to the hood motor - - 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 - } + private final LoggedTalonFX warmup1, warmup2, warmup3, shooter, hood; + private final CANcoder hoodEncoder; + + /* + * 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("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() + .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 + MotorOutputConfigs rollersOutputConfigs = new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Coast); + + // Set the Slot0, CurrentLimits, and MotorOutput parameters of `rollersConfig` + // to the configs you just created. + TalonFXConfiguration rollersConfig = new TalonFXConfiguration() + .withSlot0(rollersSlot0Configs) + .withCurrentLimits(rollersClConfigs) + .withMotorOutput(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() + .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() + .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 + // Set the Slot0, CurrentLimits, MotorOutput, and Feedback parameters to the + // device configs above + TalonFXConfiguration hoodConfig = new TalonFXConfiguration() + .withSlot0(hoodSlot0Configs) + .withCurrentLimits(hoodClConfigs) + .withMotorOutput(hoodOutputConfigs) + .withFeedback(hoodFeedbackConfigs); + + // 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 7f846fe8f5950b1740f47befbc7830f9ad8d2579 Mon Sep 17 00:00:00 2001 From: ItsGetWreckt Date: Wed, 12 Aug 2026 16:06:37 -0700 Subject: [PATCH 2/8] Forgot to do something with the hw --- src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index b839f69..cba4dde 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -33,6 +33,10 @@ public class ShooterSubsystem extends SubsystemBase { * hood angle */ + private final VelocityVoltage m_velocityVoltageRequest = new VelocityVoltage(0.0); + private final PositionVoltage m_positionVoltageRequest = new PositionVoltage(0.0); + private final double target_roller_speed, target_hood_angle; + public ShooterSubsystem() { // The canbus is a communication system that can connect devices like the // roboRIO, pdh, and motors. From 59d8b788684207b5db5c6eb9f7e03955a527f30a Mon Sep 17 00:00:00 2001 From: ItsGetWreckt Date: Wed, 12 Aug 2026 16:10:30 -0700 Subject: [PATCH 3/8] fixed some errors --- src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index cba4dde..8af5208 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -10,6 +10,8 @@ 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; @@ -35,7 +37,7 @@ public class ShooterSubsystem extends SubsystemBase { private final VelocityVoltage m_velocityVoltageRequest = new VelocityVoltage(0.0); private final PositionVoltage m_positionVoltageRequest = new PositionVoltage(0.0); - private final double target_roller_speed, target_hood_angle; + private double target_roller_speed, target_hood_angle; public ShooterSubsystem() { // The canbus is a communication system that can connect devices like the From 520d2d61c39f263649a974264112ed8b4d4dd1ea Mon Sep 17 00:00:00 2001 From: ItsGetWreckt Date: Sun, 16 Aug 2026 15:08:53 -0700 Subject: [PATCH 4/8] Finish week 6 hw --- src/main/java/frc/robot/Constants.java | 162 ++++++++-------- src/main/java/frc/robot/RobotContainer.java | 175 +++++++++--------- .../robot/commandGroups/ShootBasicHood.java | 18 ++ .../robot/subsystems/ShooterSubsystem.java | 38 +++- 4 files changed, 216 insertions(+), 177 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 fe1260d..4379473 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -64,8 +64,7 @@ public static final class Arm { public static final double STATOR_CURRENT_LIMIT = 80.0; public static final double SUPPLY_CURRENT_LIMIT = 60.0; - public static final double MOTOR_ROTS_PER_ARM_ROT = - (12.0 / 1.0) * (42.0 / 36.0) * (30.0 / 18.0) * (32.0 / 20.0); + public static final double MOTOR_ROTS_PER_ARM_ROT = (12.0 / 1.0) * (42.0 / 36.0) * (30.0 / 18.0) * (32.0 / 20.0); public static final double ARM_ROTS_PER_MOTOR_ROT = 1.0 / MOTOR_ROTS_PER_ARM_ROT; public static final double ARM_DEGREES_PER_MOTOR_ROT = 360.0 / MOTOR_ROTS_PER_ARM_ROT; public static final double MOTOR_ROTS_PER_ARM_DEGREE = MOTOR_ROTS_PER_ARM_ROT / 360.0; @@ -94,13 +93,11 @@ public static final class Rollers { public static final double MOTOR_ROTS_PER_ROLLERS_ROT = 2.0; // 8.0 / 3.0; public static final double ROLLER_ROTS_PER_MOTOR_ROT = 1.0 / MOTOR_ROTS_PER_ROLLERS_ROT; public static final double DESIGNED_SURFACE_SPEED_FT_PER_SEC = 25.0; - public static final double DESIGNED_SURFACE_SPEED_METERS_PER_SEC = - DESIGNED_SURFACE_SPEED_FT_PER_SEC * 0.3048; - public static final double DESIGNED_SURFACE_SPEED_IN_PER_SEC = - DESIGNED_SURFACE_SPEED_FT_PER_SEC * 12.0; + public static final double DESIGNED_SURFACE_SPEED_METERS_PER_SEC = DESIGNED_SURFACE_SPEED_FT_PER_SEC * 0.3048; + public static final double DESIGNED_SURFACE_SPEED_IN_PER_SEC = DESIGNED_SURFACE_SPEED_FT_PER_SEC * 12.0; // public static final double TARGET_ROLLER_RPM = - // (DESIGNED_SURFACE_SPEED_IN_PER_SEC * 60.0) / ROLLER_CIRCUMFERENCE_INCHES; + // (DESIGNED_SURFACE_SPEED_IN_PER_SEC * 60.0) / ROLLER_CIRCUMFERENCE_INCHES; public static final double TARGET_ROLLER_RPM = 2700; public static final double TARGET_ROLLER_RPS = TARGET_ROLLER_RPM / 60.0; public static final double TARGET_MOTOR_RPS = TARGET_ROLLER_RPS * MOTOR_ROTS_PER_ROLLERS_ROT; @@ -113,8 +110,7 @@ public static class Swerve { public static final double FF_RADIUS_M2 = 0.1; public static final double MAX_TRANSLATIONAL_MOVEMENT_SQUARED = 0.0625; public static final SwerveType WHICH_SWERVE_ROBOT = SwerveType.COBRA; - public static final CANBus CAN_BUS = - new CANBus(WHICH_SWERVE_ROBOT.CANBUS_NAME, "./logs/example.hoot"); + public static final CANBus CAN_BUS = new CANBus(WHICH_SWERVE_ROBOT.CANBUS_NAME, "./logs/example.hoot"); // the distance over the bump in meters public static final double TARGET_POS_ERROR = 0.07; @@ -125,6 +121,7 @@ public static enum SwerveLevel { L2(6.75, 21.428571428571427), L3(6.12, 21.428571428571427), FIVEN_L3(5.2734375, 26.09090909091); + public final double DRIVE_GEAR_RATIO, STEER_GEAR_RATIO; SwerveLevel(double drive, double steer) { @@ -140,6 +137,7 @@ public static enum SwerveDrivePIDValues { // //0.12301 JAMES_HARDEN(0.36, 0d, 0d, 0.2425, 0.11560693641, 0), // 0.041539 //0.12301 COBRA(0.25, 0d, 0d, 0.2d, 0.124, 0d); // 0.041539 //0.12301 + public final double KP, KI, KD, KS, KV, KA; SwerveDrivePIDValues(double KP, double KI, double KD, double KS, double KV, double KA) { @@ -157,6 +155,7 @@ public static enum SwerveSteerPIDValues { PROTO(20d, 0d, 0d, 0d, 0d, 0d), JAMES_HARDEN(38.982d, 2.4768d, 0d, 0.23791d, 0d, 0.1151d), COBRA(100d, 0d, 0.5, 0.1, 2.49, 0d); + public final double KP, KI, KD, KS, KV, KA; SwerveSteerPIDValues(double KP, double KI, double KD, double KS, double KV, double KA) { @@ -212,6 +211,7 @@ public static enum SwerveDriveToPoseProfileValues { PROTO(0.5, 0.5, 0.2, 0.2), JAMES_HARDEN(0.5, 0.5, 0.2, 0.2), COBRA(5, 8, 1.9, 10); + public final double maxVelocityLinear, maxAccelerationLinear, maxVelocityAngular, @@ -234,6 +234,7 @@ public static enum ChoreoPIDValues { PROTO(0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d), JAMES_HARDEN(0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d), COBRA(3.467, 0, 0, 3.567, 0, 0, 2.5, 0, 0); // 3.75 m/s^2 max accel while turning max, + public final double kPX, kIX, kDX, kPY, kIY, kDY, kPR, kIR, kDR; ChoreoPIDValues( @@ -263,6 +264,7 @@ public static enum RobotDimensions { PROTO(Inches.of(22.52), Inches.of(22.834)), // length, width JAMES_HARDEN(Inches.of(26.75), Inches.of(22.75)), // length, width COBRA(Inches.of(29.0), Inches.of(26.0)); // length, width + public final Distance length, width; RobotDimensions(Distance length, Distance width) { @@ -349,6 +351,7 @@ public static enum SwerveType { BumperThickness.COBRA, 3.5714285714285716, false); + public final Angle FRONT_LEFT_ENCODER_OFFSET, FRONT_RIGHT_ENCODER_OFFSET, BACK_LEFT_ENCODER_OFFSET, @@ -407,11 +410,10 @@ public static enum SwerveType { public static final double TELE_DRIVE_FAST_MODE_SPEED_PERCENT = 0.7; public static final double TELE_DRIVE_SLOW_MODE_SPEED_PERCENT = 0.3; public static final double TELE_DRIVE_MAX_ACCELERATION_METERS_PER_SECOND_PER_SECOND = 8; - public static final double TELE_DRIVE_PERCENT_SPEED_RANGE = - (TELE_DRIVE_FAST_MODE_SPEED_PERCENT - TELE_DRIVE_SLOW_MODE_SPEED_PERCENT); + public static final double TELE_DRIVE_PERCENT_SPEED_RANGE = (TELE_DRIVE_FAST_MODE_SPEED_PERCENT + - TELE_DRIVE_SLOW_MODE_SPEED_PERCENT); public static final double TELE_DRIVE_MAX_ANGULAR_RATE_RADIANS_PER_SECOND = 10.917; - public static final double TELE_DRIVE_MAX_ANGULAR_ACCELERATION_RADIANS_PER_SECOND_PER_SECOND = - 26.971; + public static final double TELE_DRIVE_MAX_ANGULAR_ACCELERATION_RADIANS_PER_SECOND_PER_SECOND = 26.971; public static class Auto { public static final double TIME_FOR_OUTPOST_INTAKE = 3.0; @@ -436,7 +438,8 @@ public static enum ShootPos { RightShoot } - public static enum ClimbPos {} + public static enum ClimbPos { + } public static enum Depot { DepotStart, @@ -464,8 +467,7 @@ public static class Hopper { public static final int MOTOR_2_PORT = 13; // TODO: subject to change, ask Jeff - public static final double TARGET_SURFACE_SPEED_MPS = - 2.63; // TARGET_SURFACE_SPEED_FPS * 0.3048; + public static final double TARGET_SURFACE_SPEED_MPS = 2.63; // TARGET_SURFACE_SPEED_FPS * 0.3048; // TODO: Tune these public static final double kP = 0.8; @@ -483,8 +485,7 @@ public static class Hopper { public static final double FLOOR_SPEED_TOLERANCE_MPS = 0.05; - public static final InterpolatingDoubleTreeMap HOPPER_SPEED_MAP = - new InterpolatingDoubleTreeMap(); + public static final InterpolatingDoubleTreeMap HOPPER_SPEED_MAP = new InterpolatingDoubleTreeMap(); } public static class Vision { @@ -510,8 +511,7 @@ public static void updateFallbackCamera(VisionCamera cam) { public static final double DISTANCE_COEFFICIENT_THETA = 0.9; - public static final double ANGLE_COEFFICIENT_X = - 0.5; // noise growth per radian of viewing angle + public static final double ANGLE_COEFFICIENT_X = 0.5; // noise growth per radian of viewing angle public static final double ANGLE_COEFFICIENT_Y = 0.5; public static final double ANGLE_COEFFICIENT_THETA = 0.5; @@ -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.0; 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; @@ -721,14 +721,13 @@ public static final class Hood { public static final double MOTOR_ROTS_PER_DEGREE = 1.0 / HOOD_DEGREES_PER_MOTOR_ROT; public static final double MOTOR_ROTS_PER_ENCODER_ROT = 12.0; - public static final double ENCODER_ROTS_PER_HOOD_ROT = - MOTOR_ROTS_PER_HOOD_ROT / MOTOR_ROTS_PER_ENCODER_ROT; + public static final double ENCODER_ROTS_PER_HOOD_ROT = MOTOR_ROTS_PER_HOOD_ROT / MOTOR_ROTS_PER_ENCODER_ROT; public static final double HOOD_ROTS_PER_ENCODER_ROT = 1.0 / ENCODER_ROTS_PER_HOOD_ROT; public static final double HOOD_TOLERANCE_DEG = 0.5; - public static final double MIN_HOOD_POSITION = 3.8; - public static final double MAX_HOOD_POSITION = 18.369; + public static final double MIN_HOOD_ANGLE = 3.8; + public static final double MAX_HOOD_ANGLE = 18.369; public static double ENCODER_OFFSET = 0.296; // TODO public static final int ENCODER_PORT = 18; @@ -748,16 +747,17 @@ public static final class Rollers { 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_ROT = 15.0 / 12.0; + public static final double WHEEL_ROTS_PER_MOTOR_ROT = 1.0 / MOTOR_ROTS_PER_WHEEL_ROT; public static final double SHOOTER_WHEEL_DIAMETER = 3.0; } @@ -812,69 +812,55 @@ public enum AxisID { } public static class Landmarks { - public static Pose2d BLUE_HUB = - new Pose2d(4.621390342712402, 4.032095909118652, new Rotation2d()); - public static Pose2d RED_HUB = - new Pose2d(11.917659759521484, 4.032095909118652, new Rotation2d()); - - public static Pose2d RED_TOWER_R = - new Pose2d(14.871597290039062, 4.749175071716309, new Rotation2d(0)); - public static Pose2d RED_TOWER_L = - new Pose2d(14.871597290039062, 3.892498254776001, new Rotation2d(0)); - public static Pose2d BLUE_TOWER_R = - new Pose2d(1.6428194046020508, 3.320095539093017, new Rotation2d(Math.PI)); - public static Pose2d BLUE_TOWER_L = - new Pose2d(1.6428194046020508, 4.1721110343933105, new Rotation2d(Math.PI)); + public static Pose2d BLUE_HUB = new Pose2d(4.621390342712402, 4.032095909118652, new Rotation2d()); + public static Pose2d RED_HUB = new Pose2d(11.917659759521484, 4.032095909118652, new Rotation2d()); + + public static Pose2d RED_TOWER_R = new Pose2d(14.871597290039062, 4.749175071716309, new Rotation2d(0)); + public static Pose2d RED_TOWER_L = new Pose2d(14.871597290039062, 3.892498254776001, new Rotation2d(0)); + public static Pose2d BLUE_TOWER_R = new Pose2d(1.6428194046020508, 3.320095539093017, new Rotation2d(Math.PI)); + public static Pose2d BLUE_TOWER_L = new Pose2d(1.6428194046020508, 4.1721110343933105, new Rotation2d(Math.PI)); // public static Pose2d RED_LEFT_INTAKE_TO_BUMP = - // new Pose2d( - // new Translation2d(10.908942222595215, 2.54630184173584), - // new Rotation2d(1.5707963267948966)); + // new Pose2d( + // new Translation2d(10.908942222595215, 2.54630184173584), + // new Rotation2d(1.5707963267948966)); // public static Pose2d RED_RIGHT_INTAKE_TO_BUMP = - // new Pose2d( - // new Translation2d(10.921140670776367, 5.613290309906006), - // new Rotation2d(-1.5707963267948966)); + // new Pose2d( + // new Translation2d(10.921140670776367, 5.613290309906006), + // new Rotation2d(-1.5707963267948966)); // public static Pose2d BLUE_LEFT_INTAKE_TO_BUMP = - // new Pose2d( - // new Translation2d(5.6342058181762695, 5.505496978759766), - // new Rotation2d(-1.5649821399611368)); + // new Pose2d( + // new Translation2d(5.6342058181762695, 5.505496978759766), + // new Rotation2d(-1.5649821399611368)); // public static Pose2d BLUE_RIGHT_INTAKE_TO_BUMP = - // new Pose2d( - // new Translation2d(5.624283313751221, 2.4593770503997803), - // new Rotation2d(1.57873264137917)); - public static Pose2d RED_LEFT_INTAKE_TO_BUMP = - new Pose2d( - new Translation2d(10.908942222595215, 2.2220301628112793), - new Rotation2d(1.5707963267948966)); - public static Pose2d RED_RIGHT_INTAKE_TO_BUMP = - new Pose2d( - new Translation2d(10.921140670776367, 5.914291858673096), - new Rotation2d(-1.5707963267948966)); - public static Pose2d BLUE_LEFT_INTAKE_TO_BUMP = - new Pose2d( - new Translation2d(5.6342058181762695, 5.8613409996032715), - new Rotation2d(-1.5649821399611368)); - public static Pose2d BLUE_RIGHT_INTAKE_TO_BUMP = - new Pose2d( - new Translation2d(5.624283313751221, 2.281606435775757), - new Rotation2d(1.57873264137917)); - - public static Pose2d RED_LEFT_SHOOT_TO_BUMP = - new Pose2d( - new Translation2d(12.9309492111206055, 2.3620588779449463), - new Rotation2d(1.5707963267948966)); - public static Pose2d RED_RIGHT_SHOOT_TO_BUMP = - new Pose2d( - new Translation2d(12.951465606689453, 5.671574592590332), - new Rotation2d(-1.5707963267948966)); - public static Pose2d BLUE_LEFT_SHOOT_TO_BUMP = - new Pose2d( - new Translation2d(3.60064959526062, 5.671574592590332), - new Rotation2d(-1.5649821399611368)); - public static Pose2d BLUE_RIGHT_SHOOT_TO_BUMP = - new Pose2d( - new Translation2d(3.60064959526062, 2.3620588779449463), - new Rotation2d(1.57873264137917)); + // new Pose2d( + // new Translation2d(5.624283313751221, 2.4593770503997803), + // new Rotation2d(1.57873264137917)); + public static Pose2d RED_LEFT_INTAKE_TO_BUMP = new Pose2d( + new Translation2d(10.908942222595215, 2.2220301628112793), + new Rotation2d(1.5707963267948966)); + public static Pose2d RED_RIGHT_INTAKE_TO_BUMP = new Pose2d( + new Translation2d(10.921140670776367, 5.914291858673096), + new Rotation2d(-1.5707963267948966)); + public static Pose2d BLUE_LEFT_INTAKE_TO_BUMP = new Pose2d( + new Translation2d(5.6342058181762695, 5.8613409996032715), + new Rotation2d(-1.5649821399611368)); + public static Pose2d BLUE_RIGHT_INTAKE_TO_BUMP = new Pose2d( + new Translation2d(5.624283313751221, 2.281606435775757), + new Rotation2d(1.57873264137917)); + + public static Pose2d RED_LEFT_SHOOT_TO_BUMP = new Pose2d( + new Translation2d(12.9309492111206055, 2.3620588779449463), + new Rotation2d(1.5707963267948966)); + public static Pose2d RED_RIGHT_SHOOT_TO_BUMP = new Pose2d( + new Translation2d(12.951465606689453, 5.671574592590332), + new Rotation2d(-1.5707963267948966)); + public static Pose2d BLUE_LEFT_SHOOT_TO_BUMP = new Pose2d( + new Translation2d(3.60064959526062, 5.671574592590332), + new Rotation2d(-1.5649821399611368)); + public static Pose2d BLUE_RIGHT_SHOOT_TO_BUMP = new Pose2d( + new Translation2d(3.60064959526062, 2.3620588779449463), + new Rotation2d(1.57873264137917)); public static Pose2d BLUE_PASSING_L = new Pose2d(2.111, 6.003, new Rotation2d()); public static Pose2d BLUE_PASSING_R = new Pose2d(2.111, 2.007, new Rotation2d()); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 565cfc0..46cc0ae 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; 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; @@ -34,76 +35,69 @@ 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 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; - - -// 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; + 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; + // private double hoodAngle = 18.369; + // private double shooterSpeed = 58.0; public RobotContainer() { - // autoRoutines = new AutoRoutines(intakeSubsystem, lebron, hopperSubsystem, drivetrain, redside); + // 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 @@ -112,19 +106,18 @@ private void configureBindings() { DoubleSupplier rotationFunction = () -> -joystick.getRightX(); DoubleSupplier speedFunction = () -> 1d; - SwerveJoystickCommand swerveJoystickDefaultCommand = - new SwerveJoystickCommand( - frontBackFunction, - leftRightFunction, - rotationFunction, - speedFunction, - () -> true, - joystick.leftTrigger()::getAsBoolean, - () -> false, - redside, - drivetrain, - () -> false, - () -> false); + SwerveJoystickCommand swerveJoystickDefaultCommand = new SwerveJoystickCommand( + frontBackFunction, + leftRightFunction, + rotationFunction, + speedFunction, + () -> true, + joystick.leftTrigger()::getAsBoolean, + () -> false, + redside, + drivetrain, + () -> false, + () -> false); joystick.x().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric)); drivetrain.setDefaultCommand(swerveJoystickDefaultCommand); @@ -144,33 +137,33 @@ private void configureBindings() { // * KEEP FOR WIN COMMAND // joystick - // .a() - // .whileTrue( - // new DriveToAndShoot( - // () -> (new Pose2d(new Translation2d(3.0, 5.0), new Rotation2d())), - // lebron, - // intakeSubsystem, - // hopperSubsystem, - // drivetrain, - // redside)); + // .a() + // .whileTrue( + // new DriveToAndShoot( + // () -> (new Pose2d(new Translation2d(3.0, 5.0), new Rotation2d())), + // lebron, + // intakeSubsystem, + // hopperSubsystem, + // drivetrain, + // redside)); // joystick - // .a() - // .whileTrue( - // new DriveToPose( - // drivetrain, - // () -> - // new Pose2d( - // new Translation2d(2.462480068206787, 2.26101016998291), new + // .a() + // .whileTrue( + // new DriveToPose( + // drivetrain, + // () -> + // new Pose2d( + // new Translation2d(2.462480068206787, 2.26101016998291), new // Rotation2d()))); // * KEEP FOR INTERMAP TESTING // joystick - // .rightTrigger() - // .whileTrue( - // lebron - // .shootAtSpeedHoodCommand(() -> shooterSpeed, () -> hoodAngle) - // .alongWith(Commands.waitUntil(lebron::isAtSpeed).andThen + // .rightTrigger() + // .whileTrue( + // lebron + // .shootAtSpeedHoodCommand(() -> shooterSpeed, () -> hoodAngle) + // .alongWith(Commands.waitUntil(lebron::isAtSpeed).andThen // // (hopperSubsystem.runHopperUntilInterruptedCommand().alongWith(Commands.waitSeconds(0.4).andThen(intakeSubsystem.powerRetractRollersCommand()))))); @@ -184,6 +177,12 @@ 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_ANGLE, shooterSubsystem, + intakeSubsystem, hopperSubsystem)); + joystick.rightTrigger().whileTrue(new ShootBasicHood(58.2, Constants.Shooter.Hood.MAX_HOOD_ANGLE, shooterSubsystem, + intakeSubsystem, hopperSubsystem)); } public static boolean isRedAlliance() { @@ -213,7 +212,7 @@ public boolean inAllianceSide() { : drivetrain.getPose().getX() < Constants.Landmarks.BLUE_HUB.getX(); } -// public Command getAutonomousCommand() { -// return autoChooser.selectedCommand(); -// } + // public Command getAutonomousCommand() { + // return autoChooser.selectedCommand(); + // } } 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..b5ff587 --- /dev/null +++ b/src/main/java/frc/robot/commandGroups/ShootBasicHood.java @@ -0,0 +1,18 @@ +package frc.robot.commandGroups; + +import java.util.function.DoubleSupplier; + +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 8af5208..13ca681 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -19,6 +19,8 @@ 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; @@ -37,7 +39,7 @@ public class ShooterSubsystem extends SubsystemBase { private final VelocityVoltage m_velocityVoltageRequest = new VelocityVoltage(0.0); private final PositionVoltage m_positionVoltageRequest = new PositionVoltage(0.0); - private double target_roller_speed, target_hood_angle; + private double target_roller_speed = 0.0, target_hood_angle = 0.0; public ShooterSubsystem() { // The canbus is a communication system that can connect devices like the @@ -150,4 +152,38 @@ public ShooterSubsystem() { hoodEncoder.getConfigurator().apply(hoodCANcoderConfig); } + + public void setHoodAngle(double degrees) { + target_hood_angle = degrees; + hood.setControl(m_positionVoltageRequest + .withPosition(MathUtil.clamp(degrees / 360.0, Constants.Shooter.Hood.MIN_HOOD_ANGLE, + Constants.Shooter.Hood.MAX_HOOD_ANGLE))); + } + + public void setShooterSpeed(double velocityRps) { + target_roller_speed = velocityRps; + shooter.setControl(m_velocityVoltageRequest + .withVelocity(velocityRps * Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROT)); + } + + public void stopShooter() { + target_roller_speed = 0.0; + shooter.stopMotor(); + } + + public boolean isShooterAtSpeed() { + double currentVelocity = shooter.getCachedVelocityRps() + / Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROT; + return Math.abs(currentVelocity - target_roller_speed) <= 1.0; + } + + public Command shootWithHood(double targetShooterSpeed, double targetHoodAngle) { + return runEnd( + () -> { + setShooterSpeed(targetShooterSpeed); + setHoodAngle(targetHoodAngle); + }, + this::stopShooter); + } + } From 57b3656b249f0468a4e3e2c6a235185890080e7c Mon Sep 17 00:00:00 2001 From: ItsGetWreckt Date: Sun, 16 Aug 2026 15:12:16 -0700 Subject: [PATCH 5/8] Build --- src/main/java/frc/robot/Constants.java | 120 ++++--- src/main/java/frc/robot/RobotContainer.java | 105 +++--- .../robot/commandGroups/ShootBasicHood.java | 19 +- .../SwerveCommands/SwerveJoystickCommand.java | 5 +- .../robot/subsystems/ShooterSubsystem.java | 333 +++++++++--------- 5 files changed, 314 insertions(+), 268 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4379473..b3cf6d8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -64,7 +64,8 @@ public static final class Arm { public static final double STATOR_CURRENT_LIMIT = 80.0; public static final double SUPPLY_CURRENT_LIMIT = 60.0; - public static final double MOTOR_ROTS_PER_ARM_ROT = (12.0 / 1.0) * (42.0 / 36.0) * (30.0 / 18.0) * (32.0 / 20.0); + public static final double MOTOR_ROTS_PER_ARM_ROT = + (12.0 / 1.0) * (42.0 / 36.0) * (30.0 / 18.0) * (32.0 / 20.0); public static final double ARM_ROTS_PER_MOTOR_ROT = 1.0 / MOTOR_ROTS_PER_ARM_ROT; public static final double ARM_DEGREES_PER_MOTOR_ROT = 360.0 / MOTOR_ROTS_PER_ARM_ROT; public static final double MOTOR_ROTS_PER_ARM_DEGREE = MOTOR_ROTS_PER_ARM_ROT / 360.0; @@ -93,8 +94,10 @@ public static final class Rollers { public static final double MOTOR_ROTS_PER_ROLLERS_ROT = 2.0; // 8.0 / 3.0; public static final double ROLLER_ROTS_PER_MOTOR_ROT = 1.0 / MOTOR_ROTS_PER_ROLLERS_ROT; public static final double DESIGNED_SURFACE_SPEED_FT_PER_SEC = 25.0; - public static final double DESIGNED_SURFACE_SPEED_METERS_PER_SEC = DESIGNED_SURFACE_SPEED_FT_PER_SEC * 0.3048; - public static final double DESIGNED_SURFACE_SPEED_IN_PER_SEC = DESIGNED_SURFACE_SPEED_FT_PER_SEC * 12.0; + public static final double DESIGNED_SURFACE_SPEED_METERS_PER_SEC = + DESIGNED_SURFACE_SPEED_FT_PER_SEC * 0.3048; + public static final double DESIGNED_SURFACE_SPEED_IN_PER_SEC = + DESIGNED_SURFACE_SPEED_FT_PER_SEC * 12.0; // public static final double TARGET_ROLLER_RPM = // (DESIGNED_SURFACE_SPEED_IN_PER_SEC * 60.0) / ROLLER_CIRCUMFERENCE_INCHES; @@ -110,7 +113,8 @@ public static class Swerve { public static final double FF_RADIUS_M2 = 0.1; public static final double MAX_TRANSLATIONAL_MOVEMENT_SQUARED = 0.0625; public static final SwerveType WHICH_SWERVE_ROBOT = SwerveType.COBRA; - public static final CANBus CAN_BUS = new CANBus(WHICH_SWERVE_ROBOT.CANBUS_NAME, "./logs/example.hoot"); + public static final CANBus CAN_BUS = + new CANBus(WHICH_SWERVE_ROBOT.CANBUS_NAME, "./logs/example.hoot"); // the distance over the bump in meters public static final double TARGET_POS_ERROR = 0.07; @@ -121,7 +125,6 @@ public static enum SwerveLevel { L2(6.75, 21.428571428571427), L3(6.12, 21.428571428571427), FIVEN_L3(5.2734375, 26.09090909091); - public final double DRIVE_GEAR_RATIO, STEER_GEAR_RATIO; SwerveLevel(double drive, double steer) { @@ -137,7 +140,6 @@ public static enum SwerveDrivePIDValues { // //0.12301 JAMES_HARDEN(0.36, 0d, 0d, 0.2425, 0.11560693641, 0), // 0.041539 //0.12301 COBRA(0.25, 0d, 0d, 0.2d, 0.124, 0d); // 0.041539 //0.12301 - public final double KP, KI, KD, KS, KV, KA; SwerveDrivePIDValues(double KP, double KI, double KD, double KS, double KV, double KA) { @@ -155,7 +157,6 @@ public static enum SwerveSteerPIDValues { PROTO(20d, 0d, 0d, 0d, 0d, 0d), JAMES_HARDEN(38.982d, 2.4768d, 0d, 0.23791d, 0d, 0.1151d), COBRA(100d, 0d, 0.5, 0.1, 2.49, 0d); - public final double KP, KI, KD, KS, KV, KA; SwerveSteerPIDValues(double KP, double KI, double KD, double KS, double KV, double KA) { @@ -211,7 +212,6 @@ public static enum SwerveDriveToPoseProfileValues { PROTO(0.5, 0.5, 0.2, 0.2), JAMES_HARDEN(0.5, 0.5, 0.2, 0.2), COBRA(5, 8, 1.9, 10); - public final double maxVelocityLinear, maxAccelerationLinear, maxVelocityAngular, @@ -234,7 +234,6 @@ public static enum ChoreoPIDValues { PROTO(0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d), JAMES_HARDEN(0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d), COBRA(3.467, 0, 0, 3.567, 0, 0, 2.5, 0, 0); // 3.75 m/s^2 max accel while turning max, - public final double kPX, kIX, kDX, kPY, kIY, kDY, kPR, kIR, kDR; ChoreoPIDValues( @@ -264,7 +263,6 @@ public static enum RobotDimensions { PROTO(Inches.of(22.52), Inches.of(22.834)), // length, width JAMES_HARDEN(Inches.of(26.75), Inches.of(22.75)), // length, width COBRA(Inches.of(29.0), Inches.of(26.0)); // length, width - public final Distance length, width; RobotDimensions(Distance length, Distance width) { @@ -351,7 +349,6 @@ public static enum SwerveType { BumperThickness.COBRA, 3.5714285714285716, false); - public final Angle FRONT_LEFT_ENCODER_OFFSET, FRONT_RIGHT_ENCODER_OFFSET, BACK_LEFT_ENCODER_OFFSET, @@ -410,10 +407,11 @@ public static enum SwerveType { public static final double TELE_DRIVE_FAST_MODE_SPEED_PERCENT = 0.7; public static final double TELE_DRIVE_SLOW_MODE_SPEED_PERCENT = 0.3; public static final double TELE_DRIVE_MAX_ACCELERATION_METERS_PER_SECOND_PER_SECOND = 8; - public static final double TELE_DRIVE_PERCENT_SPEED_RANGE = (TELE_DRIVE_FAST_MODE_SPEED_PERCENT - - TELE_DRIVE_SLOW_MODE_SPEED_PERCENT); + public static final double TELE_DRIVE_PERCENT_SPEED_RANGE = + (TELE_DRIVE_FAST_MODE_SPEED_PERCENT - TELE_DRIVE_SLOW_MODE_SPEED_PERCENT); public static final double TELE_DRIVE_MAX_ANGULAR_RATE_RADIANS_PER_SECOND = 10.917; - public static final double TELE_DRIVE_MAX_ANGULAR_ACCELERATION_RADIANS_PER_SECOND_PER_SECOND = 26.971; + public static final double TELE_DRIVE_MAX_ANGULAR_ACCELERATION_RADIANS_PER_SECOND_PER_SECOND = + 26.971; public static class Auto { public static final double TIME_FOR_OUTPOST_INTAKE = 3.0; @@ -438,8 +436,7 @@ public static enum ShootPos { RightShoot } - public static enum ClimbPos { - } + public static enum ClimbPos {} public static enum Depot { DepotStart, @@ -467,7 +464,8 @@ public static class Hopper { public static final int MOTOR_2_PORT = 13; // TODO: subject to change, ask Jeff - public static final double TARGET_SURFACE_SPEED_MPS = 2.63; // TARGET_SURFACE_SPEED_FPS * 0.3048; + public static final double TARGET_SURFACE_SPEED_MPS = + 2.63; // TARGET_SURFACE_SPEED_FPS * 0.3048; // TODO: Tune these public static final double kP = 0.8; @@ -485,7 +483,8 @@ public static class Hopper { public static final double FLOOR_SPEED_TOLERANCE_MPS = 0.05; - public static final InterpolatingDoubleTreeMap HOPPER_SPEED_MAP = new InterpolatingDoubleTreeMap(); + public static final InterpolatingDoubleTreeMap HOPPER_SPEED_MAP = + new InterpolatingDoubleTreeMap(); } public static class Vision { @@ -511,7 +510,8 @@ public static void updateFallbackCamera(VisionCamera cam) { public static final double DISTANCE_COEFFICIENT_THETA = 0.9; - public static final double ANGLE_COEFFICIENT_X = 0.5; // noise growth per radian of viewing angle + public static final double ANGLE_COEFFICIENT_X = + 0.5; // noise growth per radian of viewing angle public static final double ANGLE_COEFFICIENT_Y = 0.5; public static final double ANGLE_COEFFICIENT_THETA = 0.5; @@ -721,7 +721,8 @@ public static final class Hood { public static final double MOTOR_ROTS_PER_DEGREE = 1.0 / HOOD_DEGREES_PER_MOTOR_ROT; public static final double MOTOR_ROTS_PER_ENCODER_ROT = 12.0; - public static final double ENCODER_ROTS_PER_HOOD_ROT = MOTOR_ROTS_PER_HOOD_ROT / MOTOR_ROTS_PER_ENCODER_ROT; + public static final double ENCODER_ROTS_PER_HOOD_ROT = + MOTOR_ROTS_PER_HOOD_ROT / MOTOR_ROTS_PER_ENCODER_ROT; public static final double HOOD_ROTS_PER_ENCODER_ROT = 1.0 / ENCODER_ROTS_PER_HOOD_ROT; public static final double HOOD_TOLERANCE_DEG = 0.5; @@ -761,7 +762,6 @@ public static final class Rollers { public static final double WHEEL_ROTS_PER_MOTOR_ROT = 1.0 / MOTOR_ROTS_PER_WHEEL_ROT; public static final double SHOOTER_WHEEL_DIAMETER = 3.0; } - } public static class OI { @@ -812,13 +812,19 @@ public enum AxisID { } public static class Landmarks { - public static Pose2d BLUE_HUB = new Pose2d(4.621390342712402, 4.032095909118652, new Rotation2d()); - public static Pose2d RED_HUB = new Pose2d(11.917659759521484, 4.032095909118652, new Rotation2d()); - - public static Pose2d RED_TOWER_R = new Pose2d(14.871597290039062, 4.749175071716309, new Rotation2d(0)); - public static Pose2d RED_TOWER_L = new Pose2d(14.871597290039062, 3.892498254776001, new Rotation2d(0)); - public static Pose2d BLUE_TOWER_R = new Pose2d(1.6428194046020508, 3.320095539093017, new Rotation2d(Math.PI)); - public static Pose2d BLUE_TOWER_L = new Pose2d(1.6428194046020508, 4.1721110343933105, new Rotation2d(Math.PI)); + public static Pose2d BLUE_HUB = + new Pose2d(4.621390342712402, 4.032095909118652, new Rotation2d()); + public static Pose2d RED_HUB = + new Pose2d(11.917659759521484, 4.032095909118652, new Rotation2d()); + + public static Pose2d RED_TOWER_R = + new Pose2d(14.871597290039062, 4.749175071716309, new Rotation2d(0)); + public static Pose2d RED_TOWER_L = + new Pose2d(14.871597290039062, 3.892498254776001, new Rotation2d(0)); + public static Pose2d BLUE_TOWER_R = + new Pose2d(1.6428194046020508, 3.320095539093017, new Rotation2d(Math.PI)); + public static Pose2d BLUE_TOWER_L = + new Pose2d(1.6428194046020508, 4.1721110343933105, new Rotation2d(Math.PI)); // public static Pose2d RED_LEFT_INTAKE_TO_BUMP = // new Pose2d( @@ -836,31 +842,39 @@ public static class Landmarks { // new Pose2d( // new Translation2d(5.624283313751221, 2.4593770503997803), // new Rotation2d(1.57873264137917)); - public static Pose2d RED_LEFT_INTAKE_TO_BUMP = new Pose2d( - new Translation2d(10.908942222595215, 2.2220301628112793), - new Rotation2d(1.5707963267948966)); - public static Pose2d RED_RIGHT_INTAKE_TO_BUMP = new Pose2d( - new Translation2d(10.921140670776367, 5.914291858673096), - new Rotation2d(-1.5707963267948966)); - public static Pose2d BLUE_LEFT_INTAKE_TO_BUMP = new Pose2d( - new Translation2d(5.6342058181762695, 5.8613409996032715), - new Rotation2d(-1.5649821399611368)); - public static Pose2d BLUE_RIGHT_INTAKE_TO_BUMP = new Pose2d( - new Translation2d(5.624283313751221, 2.281606435775757), - new Rotation2d(1.57873264137917)); - - public static Pose2d RED_LEFT_SHOOT_TO_BUMP = new Pose2d( - new Translation2d(12.9309492111206055, 2.3620588779449463), - new Rotation2d(1.5707963267948966)); - public static Pose2d RED_RIGHT_SHOOT_TO_BUMP = new Pose2d( - new Translation2d(12.951465606689453, 5.671574592590332), - new Rotation2d(-1.5707963267948966)); - public static Pose2d BLUE_LEFT_SHOOT_TO_BUMP = new Pose2d( - new Translation2d(3.60064959526062, 5.671574592590332), - new Rotation2d(-1.5649821399611368)); - public static Pose2d BLUE_RIGHT_SHOOT_TO_BUMP = new Pose2d( - new Translation2d(3.60064959526062, 2.3620588779449463), - new Rotation2d(1.57873264137917)); + public static Pose2d RED_LEFT_INTAKE_TO_BUMP = + new Pose2d( + new Translation2d(10.908942222595215, 2.2220301628112793), + new Rotation2d(1.5707963267948966)); + public static Pose2d RED_RIGHT_INTAKE_TO_BUMP = + new Pose2d( + new Translation2d(10.921140670776367, 5.914291858673096), + new Rotation2d(-1.5707963267948966)); + public static Pose2d BLUE_LEFT_INTAKE_TO_BUMP = + new Pose2d( + new Translation2d(5.6342058181762695, 5.8613409996032715), + new Rotation2d(-1.5649821399611368)); + public static Pose2d BLUE_RIGHT_INTAKE_TO_BUMP = + new Pose2d( + new Translation2d(5.624283313751221, 2.281606435775757), + new Rotation2d(1.57873264137917)); + + public static Pose2d RED_LEFT_SHOOT_TO_BUMP = + new Pose2d( + new Translation2d(12.9309492111206055, 2.3620588779449463), + new Rotation2d(1.5707963267948966)); + public static Pose2d RED_RIGHT_SHOOT_TO_BUMP = + new Pose2d( + new Translation2d(12.951465606689453, 5.671574592590332), + new Rotation2d(-1.5707963267948966)); + public static Pose2d BLUE_LEFT_SHOOT_TO_BUMP = + new Pose2d( + new Translation2d(3.60064959526062, 5.671574592590332), + new Rotation2d(-1.5649821399611368)); + public static Pose2d BLUE_RIGHT_SHOOT_TO_BUMP = + new Pose2d( + new Translation2d(3.60064959526062, 2.3620588779449463), + new Rotation2d(1.57873264137917)); public static Pose2d BLUE_PASSING_L = new Pose2d(2.111, 6.003, new Rotation2d()); public static Pose2d BLUE_PASSING_R = new Pose2d(2.111, 2.007, new Rotation2d()); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 46cc0ae..d59352e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,15 +4,12 @@ package frc.robot; -import choreo.auto.AutoChooser; // * KEEP FOR WIN COMMAND TESTING // import edu.wpi.first.math.geometry.Pose2d; // import edu.wpi.first.math.geometry.Rotation2d; // import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commandGroups.ShootBasicHood; // * KEEP FOR WIN COMMAND TESTING @@ -23,11 +20,9 @@ import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.IntakeVisionDetection; -import frc.robot.subsystems.LEDSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.VisionSubsystem; import frc.robot.util.CustomController; -import frc.robot.util.MiscUtils; import frc.robot.util.VisionUtils; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; @@ -36,40 +31,49 @@ 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 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 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; + 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; @@ -106,18 +110,19 @@ private void configureBindings() { DoubleSupplier rotationFunction = () -> -joystick.getRightX(); DoubleSupplier speedFunction = () -> 1d; - SwerveJoystickCommand swerveJoystickDefaultCommand = new SwerveJoystickCommand( - frontBackFunction, - leftRightFunction, - rotationFunction, - speedFunction, - () -> true, - joystick.leftTrigger()::getAsBoolean, - () -> false, - redside, - drivetrain, - () -> false, - () -> false); + SwerveJoystickCommand swerveJoystickDefaultCommand = + new SwerveJoystickCommand( + frontBackFunction, + leftRightFunction, + rotationFunction, + speedFunction, + () -> true, + joystick.leftTrigger()::getAsBoolean, + () -> false, + redside, + drivetrain, + () -> false, + () -> false); joystick.x().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric)); drivetrain.setDefaultCommand(swerveJoystickDefaultCommand); @@ -179,10 +184,24 @@ private void configureBindings() { // 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_ANGLE, shooterSubsystem, - intakeSubsystem, hopperSubsystem)); - joystick.rightTrigger().whileTrue(new ShootBasicHood(58.2, Constants.Shooter.Hood.MAX_HOOD_ANGLE, shooterSubsystem, - intakeSubsystem, hopperSubsystem)); + joystick + .rightBumper() + .whileTrue( + new ShootBasicHood( + 44.2, + Constants.Shooter.Hood.MIN_HOOD_ANGLE, + shooterSubsystem, + intakeSubsystem, + hopperSubsystem)); + joystick + .rightTrigger() + .whileTrue( + new ShootBasicHood( + 58.2, + Constants.Shooter.Hood.MAX_HOOD_ANGLE, + 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 index b5ff587..500ad32 100644 --- a/src/main/java/frc/robot/commandGroups/ShootBasicHood.java +++ b/src/main/java/frc/robot/commandGroups/ShootBasicHood.java @@ -1,7 +1,5 @@ package frc.robot.commandGroups; -import java.util.function.DoubleSupplier; - import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import frc.robot.subsystems.HopperSubsystem; @@ -9,10 +7,15 @@ 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())); - } + 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/commands/SwerveCommands/SwerveJoystickCommand.java b/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java index 09fce43..5eeedd7 100644 --- a/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java +++ b/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java @@ -3,8 +3,6 @@ import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; import dev.doglog.DogLog; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -131,8 +129,7 @@ public void execute() { ? fieldCentricDrive.withVelocityX(x).withVelocityY(y).withRotationalRate(turn) : robotCentricDrive.withVelocityX(x).withVelocityY(y).withRotationalRate(turn); - boolean isPointed = - true; + boolean isPointed = true; boolean intentionallyStationary = (xSpdFunction.getAsDouble() < 0.1) && (ySpdFunction.getAsDouble() < 0.1) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 13ca681..4ff11ec 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,7 +19,6 @@ 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,163 +27,176 @@ public class ShooterSubsystem extends SubsystemBase { - private final LoggedTalonFX warmup1, warmup2, warmup3, shooter, hood; - private final CANcoder hoodEncoder; - - /* - * 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 final VelocityVoltage m_velocityVoltageRequest = new VelocityVoltage(0.0); - private final PositionVoltage m_positionVoltageRequest = new PositionVoltage(0.0); - private double target_roller_speed = 0.0, target_hood_angle = 0.0; - - 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("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() - .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 - MotorOutputConfigs rollersOutputConfigs = new MotorOutputConfigs() - .withInverted(InvertedValue.Clockwise_Positive) - .withNeutralMode(NeutralModeValue.Coast); - - // Set the Slot0, CurrentLimits, and MotorOutput parameters of `rollersConfig` - // to the configs you just created. - TalonFXConfiguration rollersConfig = new TalonFXConfiguration() - .withSlot0(rollersSlot0Configs) - .withCurrentLimits(rollersClConfigs) - .withMotorOutput(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() - .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() - .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 - // Set the Slot0, CurrentLimits, MotorOutput, and Feedback parameters to the - // device configs above - TalonFXConfiguration hoodConfig = new TalonFXConfiguration() - .withSlot0(hoodSlot0Configs) - .withCurrentLimits(hoodClConfigs) - .withMotorOutput(hoodOutputConfigs) - .withFeedback(hoodFeedbackConfigs); - - // 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); - - } - - public void setHoodAngle(double degrees) { - target_hood_angle = degrees; - hood.setControl(m_positionVoltageRequest - .withPosition(MathUtil.clamp(degrees / 360.0, Constants.Shooter.Hood.MIN_HOOD_ANGLE, - Constants.Shooter.Hood.MAX_HOOD_ANGLE))); - } - - public void setShooterSpeed(double velocityRps) { - target_roller_speed = velocityRps; - shooter.setControl(m_velocityVoltageRequest - .withVelocity(velocityRps * Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROT)); - } - - public void stopShooter() { - target_roller_speed = 0.0; - shooter.stopMotor(); - } - - public boolean isShooterAtSpeed() { - double currentVelocity = shooter.getCachedVelocityRps() - / Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROT; - return Math.abs(currentVelocity - target_roller_speed) <= 1.0; - } - - public Command shootWithHood(double targetShooterSpeed, double targetHoodAngle) { - return runEnd( - () -> { - setShooterSpeed(targetShooterSpeed); - setHoodAngle(targetHoodAngle); - }, - this::stopShooter); - } - + private final LoggedTalonFX warmup1, warmup2, warmup3, shooter, hood; + private final CANcoder hoodEncoder; + + /* + * 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 final VelocityVoltage m_velocityVoltageRequest = new VelocityVoltage(0.0); + private final PositionVoltage m_positionVoltageRequest = new PositionVoltage(0.0); + private double target_roller_speed = 0.0, target_hood_angle = 0.0; + + 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("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() + .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 + MotorOutputConfigs rollersOutputConfigs = + new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Coast); + + // Set the Slot0, CurrentLimits, and MotorOutput parameters of `rollersConfig` + // to the configs you just created. + TalonFXConfiguration rollersConfig = + new TalonFXConfiguration() + .withSlot0(rollersSlot0Configs) + .withCurrentLimits(rollersClConfigs) + .withMotorOutput(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() + .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() + .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 + // Set the Slot0, CurrentLimits, MotorOutput, and Feedback parameters to the + // device configs above + TalonFXConfiguration hoodConfig = + new TalonFXConfiguration() + .withSlot0(hoodSlot0Configs) + .withCurrentLimits(hoodClConfigs) + .withMotorOutput(hoodOutputConfigs) + .withFeedback(hoodFeedbackConfigs); + + // 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); + } + + public void setHoodAngle(double degrees) { + target_hood_angle = degrees; + hood.setControl( + m_positionVoltageRequest.withPosition( + MathUtil.clamp( + degrees / 360.0, + Constants.Shooter.Hood.MIN_HOOD_ANGLE, + Constants.Shooter.Hood.MAX_HOOD_ANGLE))); + } + + public void setShooterSpeed(double velocityRps) { + target_roller_speed = velocityRps; + shooter.setControl( + m_velocityVoltageRequest.withVelocity( + velocityRps * Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROT)); + } + + public void stopShooter() { + target_roller_speed = 0.0; + shooter.stopMotor(); + } + + public boolean isShooterAtSpeed() { + double currentVelocity = + shooter.getCachedVelocityRps() / Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROT; + return Math.abs(currentVelocity - target_roller_speed) <= 1.0; + } + + public Command shootWithHood(double targetShooterSpeed, double targetHoodAngle) { + return runEnd( + () -> { + setShooterSpeed(targetShooterSpeed); + setHoodAngle(targetHoodAngle); + }, + this::stopShooter); + } } From 53b1851b18bc6dfb4961f2cdf442e96b8107eeaf Mon Sep 17 00:00:00 2001 From: ItsGetWreckt Date: Sun, 16 Aug 2026 15:15:11 -0700 Subject: [PATCH 6/8] Fix complaining --- .../robot/subsystems/ShooterSubsystem.java | 333 +++++++++--------- 1 file changed, 161 insertions(+), 172 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 4ff11ec..8297e90 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -27,176 +27,165 @@ public class ShooterSubsystem extends SubsystemBase { - private final LoggedTalonFX warmup1, warmup2, warmup3, shooter, hood; - private final CANcoder hoodEncoder; - - /* - * 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 final VelocityVoltage m_velocityVoltageRequest = new VelocityVoltage(0.0); - private final PositionVoltage m_positionVoltageRequest = new PositionVoltage(0.0); - private double target_roller_speed = 0.0, target_hood_angle = 0.0; - - 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("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() - .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 - MotorOutputConfigs rollersOutputConfigs = - new MotorOutputConfigs() - .withInverted(InvertedValue.Clockwise_Positive) - .withNeutralMode(NeutralModeValue.Coast); - - // Set the Slot0, CurrentLimits, and MotorOutput parameters of `rollersConfig` - // to the configs you just created. - TalonFXConfiguration rollersConfig = - new TalonFXConfiguration() - .withSlot0(rollersSlot0Configs) - .withCurrentLimits(rollersClConfigs) - .withMotorOutput(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() - .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() - .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 - // Set the Slot0, CurrentLimits, MotorOutput, and Feedback parameters to the - // device configs above - TalonFXConfiguration hoodConfig = - new TalonFXConfiguration() - .withSlot0(hoodSlot0Configs) - .withCurrentLimits(hoodClConfigs) - .withMotorOutput(hoodOutputConfigs) - .withFeedback(hoodFeedbackConfigs); - - // 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); - } - - public void setHoodAngle(double degrees) { - target_hood_angle = degrees; - hood.setControl( - m_positionVoltageRequest.withPosition( - MathUtil.clamp( - degrees / 360.0, - Constants.Shooter.Hood.MIN_HOOD_ANGLE, - Constants.Shooter.Hood.MAX_HOOD_ANGLE))); - } - - public void setShooterSpeed(double velocityRps) { - target_roller_speed = velocityRps; - shooter.setControl( - m_velocityVoltageRequest.withVelocity( - velocityRps * Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROT)); - } - - public void stopShooter() { - target_roller_speed = 0.0; - shooter.stopMotor(); - } - - public boolean isShooterAtSpeed() { - double currentVelocity = - shooter.getCachedVelocityRps() / Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROT; - return Math.abs(currentVelocity - target_roller_speed) <= 1.0; - } - - public Command shootWithHood(double targetShooterSpeed, double targetHoodAngle) { - return runEnd( - () -> { - setShooterSpeed(targetShooterSpeed); - setHoodAngle(targetHoodAngle); - }, - this::stopShooter); - } + private final LoggedTalonFX warmup1, warmup2, warmup3, shooter, hood; + private final CANcoder hoodEncoder; + + /* + * 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 final VelocityVoltage m_velocityVoltageRequest = new VelocityVoltage(0.0); + private final PositionVoltage m_positionVoltageRequest = new PositionVoltage(0.0); + private double target_roller_speed = 0.0, target_hood_angle = 0.0; + + 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("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() + .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 + MotorOutputConfigs rollersOutputConfigs = new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Coast); + + // Set the Slot0, CurrentLimits, and MotorOutput parameters of `rollersConfig` + // to the configs you just created. + TalonFXConfiguration rollersConfig = new TalonFXConfiguration() + .withSlot0(rollersSlot0Configs) + .withCurrentLimits(rollersClConfigs) + .withMotorOutput(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() + .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() + .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 + // Set the Slot0, CurrentLimits, MotorOutput, and Feedback parameters to the + // device configs above + TalonFXConfiguration hoodConfig = new TalonFXConfiguration() + .withSlot0(hoodSlot0Configs) + .withCurrentLimits(hoodClConfigs) + .withMotorOutput(hoodOutputConfigs) + .withFeedback(hoodFeedbackConfigs); + + // 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); + } + + public void setHoodAngle(double degrees) { + target_hood_angle = degrees; + hood.setControl( + m_positionVoltageRequest.withPosition( + MathUtil.clamp( + target_hood_angle / 360.0, + Constants.Shooter.Hood.MIN_HOOD_ANGLE, + Constants.Shooter.Hood.MAX_HOOD_ANGLE))); + } + + public void setShooterSpeed(double velocityRps) { + target_roller_speed = velocityRps; + shooter.setControl( + m_velocityVoltageRequest.withVelocity( + velocityRps * Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROT)); + } + + public void stopShooter() { + target_roller_speed = 0.0; + shooter.stopMotor(); + } + + public boolean isShooterAtSpeed() { + double currentVelocity = shooter.getCachedVelocityRps() + / Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROT; + return Math.abs(currentVelocity - target_roller_speed) <= 1.0; + } + + public Command shootWithHood(double targetShooterSpeed, double targetHoodAngle) { + return runEnd( + () -> { + setShooterSpeed(targetShooterSpeed); + setHoodAngle(targetHoodAngle); + }, + this::stopShooter); + } } From 076312ccd70912ceda9a7ca6a30d7d4ebfe6b1be Mon Sep 17 00:00:00 2001 From: ItsGetWreckt Date: Mon, 24 Aug 2026 18:46:15 -0700 Subject: [PATCH 7/8] fix spotless --- .../robot/subsystems/ShooterSubsystem.java | 333 +++++++++--------- 1 file changed, 172 insertions(+), 161 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 8297e90..97f6e7f 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -27,165 +27,176 @@ public class ShooterSubsystem extends SubsystemBase { - private final LoggedTalonFX warmup1, warmup2, warmup3, shooter, hood; - private final CANcoder hoodEncoder; - - /* - * 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 final VelocityVoltage m_velocityVoltageRequest = new VelocityVoltage(0.0); - private final PositionVoltage m_positionVoltageRequest = new PositionVoltage(0.0); - private double target_roller_speed = 0.0, target_hood_angle = 0.0; - - 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("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() - .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 - MotorOutputConfigs rollersOutputConfigs = new MotorOutputConfigs() - .withInverted(InvertedValue.Clockwise_Positive) - .withNeutralMode(NeutralModeValue.Coast); - - // Set the Slot0, CurrentLimits, and MotorOutput parameters of `rollersConfig` - // to the configs you just created. - TalonFXConfiguration rollersConfig = new TalonFXConfiguration() - .withSlot0(rollersSlot0Configs) - .withCurrentLimits(rollersClConfigs) - .withMotorOutput(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() - .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() - .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 - // Set the Slot0, CurrentLimits, MotorOutput, and Feedback parameters to the - // device configs above - TalonFXConfiguration hoodConfig = new TalonFXConfiguration() - .withSlot0(hoodSlot0Configs) - .withCurrentLimits(hoodClConfigs) - .withMotorOutput(hoodOutputConfigs) - .withFeedback(hoodFeedbackConfigs); - - // 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); - } - - public void setHoodAngle(double degrees) { - target_hood_angle = degrees; - hood.setControl( - m_positionVoltageRequest.withPosition( - MathUtil.clamp( - target_hood_angle / 360.0, - Constants.Shooter.Hood.MIN_HOOD_ANGLE, - Constants.Shooter.Hood.MAX_HOOD_ANGLE))); - } - - public void setShooterSpeed(double velocityRps) { - target_roller_speed = velocityRps; - shooter.setControl( - m_velocityVoltageRequest.withVelocity( - velocityRps * Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROT)); - } - - public void stopShooter() { - target_roller_speed = 0.0; - shooter.stopMotor(); - } - - public boolean isShooterAtSpeed() { - double currentVelocity = shooter.getCachedVelocityRps() - / Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROT; - return Math.abs(currentVelocity - target_roller_speed) <= 1.0; - } - - public Command shootWithHood(double targetShooterSpeed, double targetHoodAngle) { - return runEnd( - () -> { - setShooterSpeed(targetShooterSpeed); - setHoodAngle(targetHoodAngle); - }, - this::stopShooter); - } + private final LoggedTalonFX warmup1, warmup2, warmup3, shooter, hood; + private final CANcoder hoodEncoder; + + /* + * 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 final VelocityVoltage m_velocityVoltageRequest = new VelocityVoltage(0.0); + private final PositionVoltage m_positionVoltageRequest = new PositionVoltage(0.0); + private double target_roller_speed = 0.0, target_hood_angle = 0.0; + + 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("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() + .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 + MotorOutputConfigs rollersOutputConfigs = + new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Coast); + + // Set the Slot0, CurrentLimits, and MotorOutput parameters of `rollersConfig` + // to the configs you just created. + TalonFXConfiguration rollersConfig = + new TalonFXConfiguration() + .withSlot0(rollersSlot0Configs) + .withCurrentLimits(rollersClConfigs) + .withMotorOutput(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() + .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() + .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 + // Set the Slot0, CurrentLimits, MotorOutput, and Feedback parameters to the + // device configs above + TalonFXConfiguration hoodConfig = + new TalonFXConfiguration() + .withSlot0(hoodSlot0Configs) + .withCurrentLimits(hoodClConfigs) + .withMotorOutput(hoodOutputConfigs) + .withFeedback(hoodFeedbackConfigs); + + // 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); + } + + public void setHoodAngle(double degrees) { + target_hood_angle = degrees; + hood.setControl( + m_positionVoltageRequest.withPosition( + MathUtil.clamp( + target_hood_angle / 360.0, + Constants.Shooter.Hood.MIN_HOOD_ANGLE, + Constants.Shooter.Hood.MAX_HOOD_ANGLE))); + } + + public void setShooterSpeed(double velocityRps) { + target_roller_speed = velocityRps; + shooter.setControl( + m_velocityVoltageRequest.withVelocity( + velocityRps * Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROT)); + } + + public void stopShooter() { + target_roller_speed = 0.0; + shooter.stopMotor(); + } + + public boolean isShooterAtSpeed() { + double currentVelocity = + shooter.getCachedVelocityRps() / Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROT; + return Math.abs(currentVelocity - target_roller_speed) <= 1.0; + } + + public Command shootWithHood(double targetShooterSpeed, double targetHoodAngle) { + return runEnd( + () -> { + setShooterSpeed(targetShooterSpeed); + setHoodAngle(targetHoodAngle); + }, + this::stopShooter); + } } From de38c3c400acf44642a9d9a9234bb2db6a94050d Mon Sep 17 00:00:00 2001 From: ItsGetWreckt Date: Mon, 14 Sep 2026 18:55:29 -0700 Subject: [PATCH 8/8] ? --- src/main/java/frc/robot/RobotContainer.java | 221 ++++++++++---------- 1 file changed, 115 insertions(+), 106 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d59352e..b0b194a 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; @@ -28,80 +28,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 void doTelemetry() { - // logger.telemeterize(drivetrain.getCurrentState()); + public final HopperSubsystem hopperSubsystem = Constants.hopperOnRobot ? new HopperSubsystem() : null; + public final IntakeSubsystem intakeSubsystem = Constants.intakeOnRobot ? new IntakeSubsystem() : null; - // String commandName = "nah"; + public final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(); - // if (drivetrain.getCurrentCommand() != null) { - // commandName = drivetrain.getCurrentCommand().getName(); - // } - // DogLog.log("Robot/SwerveDriveCommand", commandName); - // } + // private final AutoRoutines autoRoutines; + // private final AutoChooser autoChooser; - // public CommandSwerveDrivetrain getDrivetrain() { - // return drivetrain; - // } + 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()); + + // String commandName = "nah"; + + // if (drivetrain.getCurrentCommand() != null) { + // commandName = drivetrain.getCurrentCommand().getName(); + // } + // DogLog.log("Robot/SwerveDriveCommand", commandName); + // } + + // public CommandSwerveDrivetrain getDrivetrain() { + // return drivetrain; + // } private void configureBindings() { // Swerve @@ -184,54 +175,72 @@ private void configureBindings() { // 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_ANGLE, + // shooterSubsystem, + // intakeSubsystem, + // hopperSubsystem)); + // joystick + // .rightTrigger() + // .whileTrue( + // new ShootBasicHood( + // 58.2, + // Constants.Shooter.Hood.MAX_HOOD_ANGLE, + // shooterSubsystem, + // intakeSubsystem, + // hopperSubsystem)); + joystick .rightBumper() .whileTrue( - new ShootBasicHood( - 44.2, - Constants.Shooter.Hood.MIN_HOOD_ANGLE, - shooterSubsystem, - intakeSubsystem, - hopperSubsystem)); + shooterSubsystem + .shootWithHood(44.2, Constants.Shooter.Hood.MIN_HOOD_ANGLE) + .alongWith( + Commands.waitUntil(shooterSubsystem::isShooterAtSpeed) + .andThen(hopperSubsystem.runHopperUntilInterruptedCommand()))); joystick .rightTrigger() .whileTrue( - new ShootBasicHood( - 58.2, - Constants.Shooter.Hood.MAX_HOOD_ANGLE, - shooterSubsystem, - intakeSubsystem, - hopperSubsystem)); + shooterSubsystem + .shootWithHood(58.2, Constants.Shooter.Hood.MAX_HOOD_ANGLE) + .alongWith( + Commands.waitUntil(shooterSubsystem::isSho + oterAtSpeed) + .andThen(hopperSubsystem.runHopperUntilInterruptedCommand()))); } - 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(); + // } }