From b6b68fd5ef4c5aff16e7be27b37b19cbb9329aad Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Thu, 4 Jun 2026 10:42:26 -0400 Subject: [PATCH 1/8] Refactor constants naming and move sim gains to IO classes - Apply camelCase convention to all shared constants files (Constants, DriveConstants, FeederConstants, LauncherConstants, IntakeConstants) - Apply UPPER_SNAKE_CASE to private static final fields in IO implementation classes - Move sim-only DCMotor/gain constants out of shared constants into their respective sim IO classes (TurretIOSimSpark, FlywheelIOSimWPI, KickerIOSimSpark, SpindexerIOSimSpark, RollerIOSimSpark/TalonFX) - Merge TurretIOSpark and TurretIOSimSpark gains into common kP/kD fields in TurretConstants Co-Authored-By: Claude Sonnet 4.6 --- .gitignore | 1 + .vscode/settings.json | 2 +- src/main/java/frc/robot/Constants.java | 36 +++--- src/main/java/frc/robot/Robot.java | 39 +++--- .../frc/robot/subsystems/drive/Drive.java | 32 ++--- .../subsystems/drive/DriveConstants.java | 122 +++++++++--------- .../frc/robot/subsystems/drive/Module.java | 41 +++--- .../drive/PhoenixOdometryThread.java | 18 +-- .../frc/robot/subsystems/feeder/Feeder.java | 12 +- .../subsystems/feeder/FeederConstants.java | 15 +-- .../subsystems/feeder/KickerIOSimSpark.java | 16 ++- .../subsystems/feeder/KickerIOSpark.java | 8 +- .../feeder/SpindexerIOSimSpark.java | 18 +-- .../subsystems/feeder/SpindexerIOSpark.java | 8 +- .../frc/robot/subsystems/hopper/Hopper.java | 8 +- .../frc/robot/subsystems/intake/Intake.java | 8 +- .../subsystems/intake/RollerIOSimSpark.java | 20 +-- .../subsystems/intake/RollerIOSimTalonFX.java | 20 +-- .../subsystems/intake/RollerIOSpark.java | 12 +- .../subsystems/intake/RollerIOTalonFX.java | 25 ++-- .../subsystems/launcher/FlywheelIOSimWPI.java | 8 +- .../launcher/FlywheelIOTalonFX.java | 9 +- .../subsystems/launcher/HoodIOSimSpark.java | 10 +- .../subsystems/launcher/HoodIOSimWPI.java | 4 +- .../subsystems/launcher/HoodIOSpark.java | 8 +- .../robot/subsystems/launcher/Launcher.java | 26 ++-- .../launcher/LauncherConstants.java | 14 +- .../subsystems/launcher/TurretIOSimSpark.java | 14 +- .../subsystems/launcher/TurretIOSpark.java | 8 +- .../frc/robot/subsystems/vision/Vision.java | 16 +-- 30 files changed, 290 insertions(+), 288 deletions(-) diff --git a/.gitignore b/.gitignore index bf12c9ff..bb807f0f 100644 --- a/.gitignore +++ b/.gitignore @@ -157,6 +157,7 @@ gradle-app.setting .project .settings/ bin/ +.vscode/ # IntelliJ *.iml diff --git a/.vscode/settings.json b/.vscode/settings.json index b9029060..2b608d44 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -71,5 +71,5 @@ "[java]": { "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle" }, - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx32G -Xms100m -Xlog:disable" + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx64G -Xms100m -Xlog:disable" } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2aac87ec..d62fc3c2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -41,38 +41,38 @@ public static enum Mode { public static final class FeatureFlags { /** Enable to print loop timing when total exceeds 20ms. */ - public static final boolean PROFILING_ENABLED = false; + public static final boolean profilingEnabled = false; /** Set to false to disable the hopper subsystem entirely. */ - public static final boolean kHopperEnabled = false; + public static final boolean hopperEnabled = false; } public final class RobotConstants { - public static final double kNominalVoltage = 12.0; + public static final double nominalVoltage = 12.0; } public static final class MotorConstants { public static final class NEOConstants { - public static final AngularVelocity kFreeSpeed = RPM.of(5676); - public static final int kDefaultSupplyCurrentLimit = 60; - public static final int kDefaultStatorCurrentLimit = 100; + public static final AngularVelocity freeSpeed = RPM.of(5676); + public static final int defaultSupplyCurrentLimit = 60; + public static final int defaultStatorCurrentLimit = 100; } public static final class NEO550Constants { - public static final AngularVelocity kFreeSpeed = RPM.of(11000); - public static final int kDefaultSupplyCurrentLimit = 10; + public static final AngularVelocity freeSpeed = RPM.of(11000); + public static final int defaultSupplyCurrentLimit = 10; } public static final class NEOVortexConstants { - public static final AngularVelocity kFreeSpeed = RPM.of(6784); - public static final int kDefaultSupplyCurrentLimit = 60; - public static final int kDefaultStatorCurrentLimit = 100; + public static final AngularVelocity freeSpeed = RPM.of(6784); + public static final int defaultSupplyCurrentLimit = 60; + public static final int defaultStatorCurrentLimit = 100; } public static final class KrakenX60Constants { - public static final AngularVelocity kFreeSpeed = RPM.of(6000); - public static final int kDefaultSupplyCurrentLimit = 60; - public static final int kDefaultStatorCurrentLimit = 100; + public static final AngularVelocity freeSpeed = RPM.of(6000); + public static final int defaultSupplyCurrentLimit = 60; + public static final int defaultStatorCurrentLimit = 100; } } @@ -85,16 +85,14 @@ public static final class DIOPorts { public static final int turretAbsEncoder = 4; } - public static final class OIPorts { - public static final int defaultDriver = 0; - public static final int defaultOperator = 1; - } - public static final class CANBusPorts { public static final class CAN2 { public static final CANBus bus = CANBus.roboRIO(); + // Power distribution + public static final int pd = 1; + // Drivetrain public static final int gyro = 0; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3a232e6f..b93cf444 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -36,6 +36,7 @@ import frc.lib.LoggedCompressor; import frc.lib.LoggedPowerDistribution; import frc.lib.ZorroController.Axis; +import frc.robot.Constants.CANBusPorts.CAN2; import frc.robot.Constants.DIOPorts; import frc.robot.Constants.FeatureFlags; import frc.robot.auto.B_LeftTrenchAuto; @@ -126,7 +127,7 @@ public class Robot extends LoggedRobot { public static final AutoSelector autoSelector = new AutoSelector(DIOPorts.autonomousModeSelector, allianceSelector::getAllianceColor); public final LoggedPowerDistribution powerDistribution = - new LoggedPowerDistribution(1, ModuleType.kRev, "PDH"); + new LoggedPowerDistribution(CAN2.pd, ModuleType.kRev, "PD"); private final java.util.Set activeCommands = new java.util.LinkedHashSet<>(); @@ -176,10 +177,10 @@ public Robot() { drive = new Drive( new GyroIOBoron(), - new ModuleIOTalonFX(DriveConstants.FrontLeft), - new ModuleIOTalonFX(DriveConstants.FrontRight), - new ModuleIOTalonFX(DriveConstants.BackLeft), - new ModuleIOTalonFX(DriveConstants.BackRight)); + new ModuleIOTalonFX(DriveConstants.frontLeft), + new ModuleIOTalonFX(DriveConstants.frontRight), + new ModuleIOTalonFX(DriveConstants.backLeft), + new ModuleIOTalonFX(DriveConstants.backRight)); vision = new Vision( drive::addVisionMeasurement, @@ -195,7 +196,7 @@ public Robot() { new TurretIOSpark(), new FlywheelIOTalonFX(), new HoodIOSpark()); - if (FeatureFlags.kHopperEnabled) hopper = new Hopper(new HopperIOReal()); + if (FeatureFlags.hopperEnabled) hopper = new Hopper(new HopperIOReal()); intake = new Intake( new RollerIOSpark(RollerConstants.upperRollerConfig), @@ -217,10 +218,10 @@ public Robot() { drive = new Drive( new GyroIO() {}, - new ModuleIOSimWPI(DriveConstants.FrontLeft), - new ModuleIOSimWPI(DriveConstants.FrontRight), - new ModuleIOSimWPI(DriveConstants.BackLeft), - new ModuleIOSimWPI(DriveConstants.BackRight)); + new ModuleIOSimWPI(DriveConstants.frontLeft), + new ModuleIOSimWPI(DriveConstants.frontRight), + new ModuleIOSimWPI(DriveConstants.backLeft), + new ModuleIOSimWPI(DriveConstants.backRight)); vision = new Vision( drive::addVisionMeasurement, @@ -241,7 +242,7 @@ public Robot() { new FlywheelIOSimTalonFX(), new HoodIOSimSpark()); feeder = new Feeder(new SpindexerIOSimSpark(), new KickerIOSimSpark()); - if (FeatureFlags.kHopperEnabled) hopper = new Hopper(new HopperIOSim()); + if (FeatureFlags.hopperEnabled) hopper = new Hopper(new HopperIOSim()); var intakeArmIOSim = new IntakeArmIOSim(); intake = new Intake( @@ -283,7 +284,7 @@ public Robot() { new TurretIO() {}, new FlywheelIO() {}, new HoodIO() {}); - if (FeatureFlags.kHopperEnabled) hopper = new Hopper(new HopperIO() {}); + if (FeatureFlags.hopperEnabled) hopper = new Hopper(new HopperIO() {}); intake = new Intake(new RollerIO() {}, new RollerIO() {}, new IntakeArmIO() {}); feeder = new Feeder(new SpindexerIO() {}, new KickerIO() {}); break; @@ -302,7 +303,7 @@ public Robot() { // Wire the hopper/intake interlocks. Done here (after both subsystems exist) to avoid a // circular dependency between the two subsystems. - if (FeatureFlags.kHopperEnabled) { + if (FeatureFlags.hopperEnabled) { intake.setDeployInterlock( hopper::isDeployed, () -> hopper.getDeployCommand().withTimeout(IntakeConstants.kInterlockSettleSeconds)); @@ -342,7 +343,7 @@ public Robot() { /** This function is called periodically during all modes. */ @Override public void robotPeriodic() { - long loopStart = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long loopStart = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; // Runs the Scheduler. This is responsible for polling buttons, adding // newly-scheduled commands, running already-scheduled commands, removing @@ -350,7 +351,7 @@ public void robotPeriodic() { // This must be called from the robot's periodic block in order for anything in // the Command-based framework to work. CommandScheduler.getInstance().run(); - long t1 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t1 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; logCANBus("CAN2", Constants.CANBusPorts.CAN2.bus); logCANBus("CANHD", Constants.CANBusPorts.CANHD.bus); @@ -361,16 +362,16 @@ public void robotPeriodic() { Logger.recordOutput("USB/FreeSpaceMB", getUSBStorageFreeSpace() / 1024 / 1024); GameState.logValues(); - long t2 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t2 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; // Publish kernel log events to NetworkTables (only runs on real robot) if (RobotBase.isReal()) { KernelLogMonitor.getInstance().publishToLogger(); } - long t3 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t3 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; // Profiling output - if (FeatureFlags.PROFILING_ENABLED) { + if (FeatureFlags.profilingEnabled) { long schedulerMs = (t1 - loopStart) / 1_000_000; long gameStateMs = (t2 - t1) / 1_000_000; long kernelMonitorMs = (t3 - t2) / 1_000_000; @@ -548,7 +549,7 @@ public boolean getFieldRelativeInput() { // Toggle hopper: deploy if stowed, stow if deployed (retracting intake first if needed). // runOnce has no subsystem requirements so it always executes; the scheduled command // requires hopper and will interrupt whatever is currently running on that subsystem. - if (FeatureFlags.kHopperEnabled) + if (FeatureFlags.hopperEnabled) zorroDriver .DIn() .onTrue( diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 61e1e1ea..3969aec7 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -57,9 +57,9 @@ public class Drive extends SubsystemBase { static final double ODOMETRY_FREQUENCY = - new CANBus(DrivetrainConstants.CANBusName).isNetworkFD() ? 250.0 : 100.0; + new CANBus(drivetrainConstants.CANBusName).isNetworkFD() ? 250.0 : 100.0; - static final Lock odometryLock = new ReentrantLock(); + protected static final Lock ODOMETRY_LOCK = new ReentrantLock(); private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Module[] modules = new Module[4]; // FL, FR, BL, BR @@ -107,10 +107,10 @@ public Drive( ModuleIO blModuleIO, ModuleIO brModuleIO) { this.gyroIO = gyroIO; - modules[0] = new Module(flModuleIO, 0); - modules[1] = new Module(frModuleIO, 1); - modules[2] = new Module(blModuleIO, 2); - modules[3] = new Module(brModuleIO, 3); + modules[0] = new Module(flModuleIO, "FrontLeft"); + modules[1] = new Module(frModuleIO, "FrontRight"); + modules[2] = new Module(blModuleIO, "BackLeft"); + modules[3] = new Module(brModuleIO, "BackRight"); // Usage reporting for swerve template HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDriveSwerve_AdvantageKit); @@ -156,19 +156,19 @@ public Drive( @Override public void periodic() { - long startNanos = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long startNanos = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; - odometryLock.lock(); // Prevents odometry updates while reading data - long t1 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + ODOMETRY_LOCK.lock(); // Prevents odometry updates while reading data + long t1 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; gyroIO.updateInputs(gyroInputs); - long t2 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t2 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; Logger.processInputs("Drive/Gyro", gyroInputs); - long t3 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t3 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; for (var module : modules) { module.periodic(); } - long t4 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - odometryLock.unlock(); + long t4 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + ODOMETRY_LOCK.unlock(); // Stop moving when disabled if (DriverStation.isDisabled()) { @@ -182,7 +182,7 @@ public void periodic() { Logger.recordOutput("SwerveStates/Setpoints", emptyModuleStates); Logger.recordOutput("SwerveStates/SetpointsOptimized", emptyModuleStates); } - long t5 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t5 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; // Update odometry double[] sampleTimestamps = @@ -216,7 +216,7 @@ public void periodic() { chassisSpeeds = kinematics.toChassisSpeeds(getModuleStates()); } - long t6 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t6 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; // Update gyro alert boolean gyroDisconnected = !gyroInputs.connected && Constants.currentMode != Mode.SIM; @@ -224,7 +224,7 @@ public void periodic() { Logger.recordOutput("Faults/Drive/GyroDisconnected", gyroDisconnected); // Profiling output - if (FeatureFlags.PROFILING_ENABLED) { + if (FeatureFlags.profilingEnabled) { long totalMs = (t6 - startNanos) / 1_000_000; if (totalMs > 5) { System.out.println( diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 8b39a9d5..da07b269 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -76,16 +76,18 @@ public class DriveConstants { public static final LinearVelocity drivetrainSpeedLimit = MetersPerSecond.of( 0.9 - * (wheelRadius.in(Meters) * 2.0 * Math.PI) - * KrakenX60Constants.kFreeSpeed.in(RotationsPerSecond) + * (wheelRadiusMeters * 2.0 * Math.PI) + * KrakenX60Constants.freeSpeed.in(RotationsPerSecond) / driveMotorReduction); // Chassis movement limits private static final LinearVelocity driverSpeedLimit = MetersPerSecond.of(5); public static final LinearVelocity maxChassisVelocity = MetersPerSecond.of( - Math.min(drivetrainSpeedLimit.in(MetersPerSecond), driverSpeedLimit.in(MetersPerSecond))); - public static final LinearAcceleration maxChassisAcceleration = MetersPerSecondPerSecond.of(3.0); + Math.min( + drivetrainSpeedLimit.in(MetersPerSecond), driverSpeedLimit.in(MetersPerSecond))); + public static final LinearAcceleration maxChassisAcceleration = + MetersPerSecondPerSecond.of(3.0); public static final AngularVelocity maxChassisAngularVelocity = RadiansPerSecond.of(maxChassisVelocity.in(MetersPerSecond) / driveBaseRadius.in(Meters)); @@ -102,8 +104,8 @@ public class DriveConstants { // Turn motor configuration public static final boolean turnInverted = false; public static final double turnMotorReduction = (32.0 / 15.0) * (60.0 / 10.0); // SDS MK4 - // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - private static final double kCoupleRatio = (50.0 / 14.0); // SDS MK4 L2 + // Every 1 rotation of the azimuth results in coupleRatio drive motor turns + private static final double coupleRatio = (50.0 / 14.0); // SDS MK4 L2 public static final DCMotor turnGearbox = DCMotor.getKrakenX60(1); // Absolute turn encoder configuration @@ -111,18 +113,18 @@ public class DriveConstants { // PathPlanner configuration public static final Mass robotMass = Pounds.of(150); - public static final MomentOfInertia robotMOI = KilogramSquareMeters.of(6); - public static final double wheelCOF = 1.2; + public static final MomentOfInertia robotMoi = KilogramSquareMeters.of(6); + public static final double wheelCof = 1.2; public static final RobotConfig ppConfig = new RobotConfig( robotMass.in(Kilograms), - robotMOI.in(KilogramSquareMeters), + robotMoi.in(KilogramSquareMeters), new ModuleConfig( - wheelRadius.in(Meters), + wheelRadiusMeters, drivetrainSpeedLimit.in(MetersPerSecond), - wheelCOF, + wheelCof, driveGearbox.withReduction(driveMotorReduction), - KrakenX60Constants.kDefaultSupplyCurrentLimit, + KrakenX60Constants.defaultSupplyCurrentLimit, 1), moduleTranslations); @@ -144,45 +146,45 @@ public class DriveConstants { // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType kSteerClosedLoopOutput = + private static final ClosedLoopOutputType steerClosedLoopOutput = ClosedLoopOutputType.TorqueCurrentFOC; // The closed-loop output type to use for the drive motors; // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType kDriveClosedLoopOutput = + private static final ClosedLoopOutputType driveClosedLoopOutput = ClosedLoopOutputType.TorqueCurrentFOC; // The type of motor used for the drive motor - private static final DriveMotorArrangement kDriveMotorType = + private static final DriveMotorArrangement driveMotorType = DriveMotorArrangement.TalonFX_Integrated; - // The type of motor used for the drive motor - private static final SteerMotorArrangement kSteerMotorType = + // The type of motor used for the steer motor + private static final SteerMotorArrangement steerMotorType = SteerMotorArrangement.TalonFX_Integrated; - // The remote sensor feedback type to use for the steer motors; - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + // The remote sensor feedback type to use for the steer motors + private static final SteerFeedbackType steerFeedbackType = SteerFeedbackType.FusedCANcoder; // TorqueCurrent peak at which the wheels start to slip; used for slip detection in // TorqueCurrentFOC control mode. This needs to be tuned to your individual robot. - static final int kSlipCurrent = 120; + static final int slipCurrent = 120; // Hardware stator current limit for drive motors - static final int kDriveStatorCurrentLimit = KrakenX60Constants.kDefaultStatorCurrentLimit; + static final int driveStatorCurrentLimit = KrakenX60Constants.defaultStatorCurrentLimit; // Stator current limit for azimuth (steer) motors; lower than drive to reduce brownout risk // since steering requires minimal torque compared to driving. - static final int kSteerStatorCurrentLimit = 60; + static final int steerStatorCurrentLimit = 60; private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration() .withTorqueCurrent( new TorqueCurrentConfigs() - .withPeakForwardTorqueCurrent(kSlipCurrent) - .withPeakReverseTorqueCurrent(-kSlipCurrent)) + .withPeakForwardTorqueCurrent(slipCurrent) + .withPeakReverseTorqueCurrent(-slipCurrent)) .withCurrentLimits( new CurrentLimitsConfigs() - .withStatorCurrentLimit(kDriveStatorCurrentLimit) + .withStatorCurrentLimit(driveStatorCurrentLimit) .withStatorCurrentLimitEnable(true) - .withSupplyCurrentLimit(KrakenX60Constants.kDefaultSupplyCurrentLimit) + .withSupplyCurrentLimit(KrakenX60Constants.defaultSupplyCurrentLimit) .withSupplyCurrentLimitEnable(true)); // Azimuth does not require much torque; keep stator limit low to reduce brownout risk @@ -191,99 +193,99 @@ public class DriveConstants { new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() - .withStatorCurrentLimit(kSteerStatorCurrentLimit) + .withStatorCurrentLimit(steerStatorCurrentLimit) .withStatorCurrentLimitEnable(true) - .withSupplyCurrentLimit(KrakenX60Constants.kDefaultSupplyCurrentLimit) + .withSupplyCurrentLimit(KrakenX60Constants.defaultSupplyCurrentLimit) .withSupplyCurrentLimitEnable(true)); - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = false; + private static final boolean invertLeftSide = false; + private static final boolean invertRightSide = false; // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004); - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025); + private static final MomentOfInertia steerInertia = KilogramSquareMeters.of(0.004); + private static final MomentOfInertia driveInertia = KilogramSquareMeters.of(0.025); // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); - private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + private static final Voltage steerFrictionVoltage = Volts.of(0.2); + private static final Voltage driveFrictionVoltage = Volts.of(0.2); - public static final SwerveDrivetrainConstants DrivetrainConstants = + public static final SwerveDrivetrainConstants drivetrainConstants = new SwerveDrivetrainConstants().withCANBusName(CANHD.bus.getName()); private static final SwerveModuleConstantsFactory< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - ConstantCreator = + constantCreator = new SwerveModuleConstantsFactory< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() .withDriveMotorGearRatio(driveMotorReduction) .withSteerMotorGearRatio(turnMotorReduction) - .withCouplingGearRatio(kCoupleRatio) + .withCouplingGearRatio(coupleRatio) .withWheelRadius(wheelRadius) .withSteerMotorGains(steerGains) .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(Amps.of(kSlipCurrent)) + .withSteerMotorClosedLoopOutput(steerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(driveClosedLoopOutput) + .withSlipCurrent(Amps.of(slipCurrent)) .withSpeedAt12Volts(drivetrainSpeedLimit) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorType(driveMotorType) + .withSteerMotorType(steerMotorType) + .withFeedbackSource(steerFeedbackType) .withDriveMotorInitialConfigs(driveInitialConfigs) .withSteerMotorInitialConfigs(turnInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); + .withSteerInertia(steerInertia) + .withDriveInertia(driveInertia) + .withSteerFrictionVoltage(steerFrictionVoltage) + .withDriveFrictionVoltage(driveFrictionVoltage); public static final SwerveModuleConstants< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - FrontLeft = - ConstantCreator.createModuleConstants( + frontLeft = + constantCreator.createModuleConstants( CANHD.frontLeftTurn, CANHD.frontLeftDrive, CANHD.frontLeftTurnAbsEncoder, Rotations.of(0), wheelBase.div(2.0), trackWidth.div(2.0), - kInvertLeftSide, + invertLeftSide, turnInverted, turnEncoderInverted); public static final SwerveModuleConstants< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - FrontRight = - ConstantCreator.createModuleConstants( + frontRight = + constantCreator.createModuleConstants( CANHD.frontRightTurn, CANHD.frontRightDrive, CANHD.frontRightTurnAbsEncoder, Rotations.of(0), wheelBase.div(2.0), trackWidth.div(-2.0), - kInvertRightSide, + invertRightSide, turnInverted, turnEncoderInverted); public static final SwerveModuleConstants< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - BackLeft = - ConstantCreator.createModuleConstants( + backLeft = + constantCreator.createModuleConstants( CANHD.backLeftTurn, CANHD.backLeftDrive, CANHD.backLeftTurnAbsEncoder, Rotations.of(0), wheelBase.div(-2.0), trackWidth.div(2.0), - kInvertLeftSide, + invertLeftSide, turnInverted, turnEncoderInverted); public static final SwerveModuleConstants< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - BackRight = - ConstantCreator.createModuleConstants( + backRight = + constantCreator.createModuleConstants( CANHD.backRightTurn, CANHD.backRightDrive, CANHD.backRightTurnAbsEncoder, Rotations.of(0), wheelBase.div(-2.0), trackWidth.div(-2.0), - kInvertRightSide, + invertRightSide, turnInverted, turnEncoderInverted); @@ -293,7 +295,7 @@ public class DriveConstants { */ // public static CommandSwerveDrivetrain createDrivetrain() { // return new CommandSwerveDrivetrain( - // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); + // drivetrainConstants, frontLeft, frontRight, backLeft, backRight); // } /** Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. */ diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index dc6c1eab..8749734e 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -27,40 +27,37 @@ public class Module { private final ModuleIO io; private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); - private final int index; + private final String name; private final Alert driveDisconnectedAlert; private final Alert turnDisconnectedAlert; private SwerveModulePosition[] odometryPositions = new SwerveModulePosition[] {}; - public Module(ModuleIO io, int index) { + public Module(ModuleIO io, String name) { this.io = io; - this.index = index; + this.name = name; driveDisconnectedAlert = - new Alert( - "Disconnected drive motor on module " + Integer.toString(index) + ".", - AlertType.kError); + new Alert("Disconnected drive motor on module " + name + ".", AlertType.kError); turnDisconnectedAlert = - new Alert( - "Disconnected turn motor on module " + Integer.toString(index) + ".", AlertType.kError); + new Alert("Disconnected turn motor on module " + name + ".", AlertType.kError); // Set turn zero from preferences Rotation2d turnZeroFromCancoder = inputs.turnZero; - Preferences.initDouble(zeroRotationKey + index, turnZeroFromCancoder.getRadians()); + Preferences.initDouble(zeroRotationKey + name, turnZeroFromCancoder.getRadians()); Rotation2d turnZeroFromPreferences = new Rotation2d( - Preferences.getDouble(zeroRotationKey + index, turnZeroFromCancoder.getRadians())); + Preferences.getDouble(zeroRotationKey + name, turnZeroFromCancoder.getRadians())); io.setTurnZero(turnZeroFromPreferences); Logger.recordOutput( - "Drive/Module" + index + "/TurnZeroRad", turnZeroFromPreferences.getRadians()); + "Drive/Module" + name + "/TurnZeroRad", turnZeroFromPreferences.getRadians()); } public void periodic() { - long t0 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t0 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; io.updateInputs(inputs); - long t1 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - Logger.processInputs("Drive/Module" + Integer.toString(index), inputs); - long t2 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t1 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + Logger.processInputs("Drive/Module" + name, inputs); + long t2 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; // Calculate positions for odometry int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together @@ -74,17 +71,17 @@ public void periodic() { // Update alerts driveDisconnectedAlert.set(!inputs.driveConnected); turnDisconnectedAlert.set(!inputs.turnConnected); - Logger.recordOutput("Faults/Module" + index + "/DriveDisconnected", !inputs.driveConnected); - Logger.recordOutput("Faults/Module" + index + "/TurnDisconnected", !inputs.turnConnected); - long t3 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + Logger.recordOutput("Faults/Module" + name + "/DriveDisconnected", !inputs.driveConnected); + Logger.recordOutput("Faults/Module" + name + "/TurnDisconnected", !inputs.turnConnected); + long t3 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; // Profiling output - if (FeatureFlags.PROFILING_ENABLED) { + if (FeatureFlags.profilingEnabled) { long totalMs = (t3 - t0) / 1_000_000; if (totalMs > 2) { System.out.println( "[Module" - + index + + name + "] updateInputs=" + (t1 - t0) / 1_000_000 + "ms log=" @@ -173,7 +170,7 @@ public double getSimCurrentDrawAmps() { public void setTurnZero() { Rotation2d newTurnZero = inputs.turnZero.minus(inputs.turnPosition); io.setTurnZero(newTurnZero); - Preferences.setDouble(zeroRotationKey + index, newTurnZero.getRadians()); - Logger.recordOutput("Drive/Module" + index + "/TurnZeroRad", newTurnZero.getRadians()); + Preferences.setDouble(zeroRotationKey + name, newTurnZero.getRadians()); + Logger.recordOutput("Drive/Module" + name + "/TurnZeroRad", newTurnZero.getRadians()); } } diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 92640fc9..9664fe3f 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -39,7 +39,7 @@ public class PhoenixOdometryThread extends Thread { private final List> genericQueues = new ArrayList<>(); private final List> timestampQueues = new ArrayList<>(); - private static boolean isCANFD = new CANBus(DrivetrainConstants.CANBusName).isNetworkFD(); + private static boolean isCANFD = new CANBus(drivetrainConstants.CANBusName).isNetworkFD(); private static PhoenixOdometryThread instance = null; public static PhoenixOdometryThread getInstance() { @@ -65,7 +65,7 @@ public void start() { public Queue registerSignal(StatusSignal signal) { Queue queue = new ArrayBlockingQueue<>(20); signalsLock.lock(); - Drive.odometryLock.lock(); + Drive.ODOMETRY_LOCK.lock(); try { BaseStatusSignal[] newSignals = new BaseStatusSignal[phoenixSignals.length + 1]; System.arraycopy(phoenixSignals, 0, newSignals, 0, phoenixSignals.length); @@ -74,7 +74,7 @@ public Queue registerSignal(StatusSignal signal) { phoenixQueues.add(queue); } finally { signalsLock.unlock(); - Drive.odometryLock.unlock(); + Drive.ODOMETRY_LOCK.unlock(); } return queue; } @@ -83,13 +83,13 @@ public Queue registerSignal(StatusSignal signal) { public Queue registerSignal(DoubleSupplier signal) { Queue queue = new ArrayBlockingQueue<>(20); signalsLock.lock(); - Drive.odometryLock.lock(); + Drive.ODOMETRY_LOCK.lock(); try { genericSignals.add(signal); genericQueues.add(queue); } finally { signalsLock.unlock(); - Drive.odometryLock.unlock(); + Drive.ODOMETRY_LOCK.unlock(); } return queue; } @@ -97,11 +97,11 @@ public Queue registerSignal(DoubleSupplier signal) { /** Returns a new queue that returns timestamp values for each sample. */ public Queue makeTimestampQueue() { Queue queue = new ArrayBlockingQueue<>(20); - Drive.odometryLock.lock(); + Drive.ODOMETRY_LOCK.lock(); try { timestampQueues.add(queue); } finally { - Drive.odometryLock.unlock(); + Drive.ODOMETRY_LOCK.unlock(); } return queue; } @@ -128,7 +128,7 @@ public void run() { } // Save new data to queues - Drive.odometryLock.lock(); + Drive.ODOMETRY_LOCK.lock(); try { // Sample timestamp is current FPGA time minus average CAN latency // Default timestamps from Phoenix are NOT compatible with @@ -153,7 +153,7 @@ public void run() { timestampQueues.get(i).offer(timestamp); } } finally { - Drive.odometryLock.unlock(); + Drive.ODOMETRY_LOCK.unlock(); } } } diff --git a/src/main/java/frc/robot/subsystems/feeder/Feeder.java b/src/main/java/frc/robot/subsystems/feeder/Feeder.java index 25c0199e..c66d1e75 100644 --- a/src/main/java/frc/robot/subsystems/feeder/Feeder.java +++ b/src/main/java/frc/robot/subsystems/feeder/Feeder.java @@ -31,16 +31,16 @@ public Feeder(SpindexerIO spindexerIO, KickerIO kickerIO) { @Override public void periodic() { - long t0 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t0 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; spindexerIO.updateInputs(spindexerInputs); - long t1 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t1 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; kickerIO.updateInputs(kickerInputs); - long t2 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t2 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; Logger.processInputs("Spindexer", spindexerInputs); - long t3 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t3 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; Logger.processInputs("Kicker", kickerInputs); - long t4 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t4 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; spindexerDisconnectedAlert.set(!spindexerInputs.connected); kickerDisconnectedAlert.set(!kickerInputs.connected); @@ -48,7 +48,7 @@ public void periodic() { Logger.recordOutput("Faults/Feeder/KickerDisconnected", !kickerInputs.connected); // Profiling output - if (Constants.FeatureFlags.PROFILING_ENABLED) { + if (Constants.FeatureFlags.profilingEnabled) { long totalMs = (t4 - t0) / 1_000_000; if (totalMs > 2) { System.out.println( diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java index a39b2aca..f2e73466 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java @@ -5,7 +5,6 @@ import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecond; -import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearVelocity; import frc.robot.Constants.MotorConstants.NEOVortexConstants; @@ -19,7 +18,7 @@ public static final class SpindexerConstants { public static final double motorReduction = 1.0; public static final LinearVelocity maxTangentialVelocity = MetersPerSecond.of( - NEOVortexConstants.kFreeSpeed.in(RadiansPerSecond) + NEOVortexConstants.freeSpeed.in(RadiansPerSecond) * radius.in(Meters) / motorReduction); @@ -28,9 +27,8 @@ public static final class SpindexerConstants { public static final double encoderVelocityFactor = encoderPositionFactor / 60.0; // Meters/sec // Simulation - public static final double kPSim = 0.005; - public static final double kDSim = 0.005; - public static final DCMotor gearbox = DCMotor.getNeoVortex(1); + public static final double kP = 0.005; + public static final double kD = 0.005; } public static final class KickerConstants { @@ -41,7 +39,7 @@ public static final class KickerConstants { public static final double motorReduction = 5.0; public static final LinearVelocity maxTangentialVelocity = MetersPerSecond.of( - NEOVortexConstants.kFreeSpeed.in(RadiansPerSecond) + NEOVortexConstants.freeSpeed.in(RadiansPerSecond) * radius.in(Meters) / motorReduction); @@ -50,8 +48,7 @@ public static final class KickerConstants { public static final double encoderVelocityFactor = encoderPositionFactor / 60.0; // Meters/sec // Simulation - public static final double kPSim = 0.0025; - public static final double kDSim = 0.0025; - public static final DCMotor gearbox = DCMotor.getNeoVortex(1); + public static final double kP = 0.0025; + public static final double kD = 0.0025; } } diff --git a/src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java b/src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java index bb2f8941..9778fdc4 100644 --- a/src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java @@ -14,6 +14,7 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkFlexConfig; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Voltage; @@ -26,6 +27,7 @@ public class KickerIOSimSpark implements KickerIO { private static final double KICKER_MOI_KG_M2 = 0.00052; + private static final DCMotor GEARBOX = DCMotor.getNeoVortex(1); private final DCMotorSim kickerSim; @@ -41,22 +43,22 @@ public KickerIOSimSpark() { config .inverted(false) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.kDefaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.kNominalVoltage); + .smartCurrentLimit(NEOVortexConstants.defaultSupplyCurrentLimit) + .voltageCompensation(RobotConstants.nominalVoltage); config .encoder .positionConversionFactor(encoderPositionFactor) .velocityConversionFactor(encoderVelocityFactor); - config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kPSim, 0.0, 0.0); + config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kP, 0.0, kD); flex.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - flexSim = new SparkFlexSim(flex, gearbox); + flexSim = new SparkFlexSim(flex, GEARBOX); kickerSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(gearbox, KICKER_MOI_KG_M2, motorReduction), gearbox); + LinearSystemId.createDCMotorSystem(GEARBOX, KICKER_MOI_KG_M2, motorReduction), GEARBOX); } @Override @@ -76,13 +78,13 @@ public void updateInputs(KickerIOInputs inputs) { @Override public void setOpenLoop(Voltage volts) { - flexSim.setAppliedOutput(volts.in(Volts) / RobotConstants.kNominalVoltage); + flexSim.setAppliedOutput(volts.in(Volts) / RobotConstants.nominalVoltage); } @Override public void setVelocity(LinearVelocity tangentialVelocity) { double feedforwardVolts = - RobotConstants.kNominalVoltage + RobotConstants.nominalVoltage * tangentialVelocity.in(MetersPerSecond) / maxTangentialVelocity.in(MetersPerSecond); controller.setSetpoint( diff --git a/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java b/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java index a1b7e430..01db3412 100644 --- a/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java +++ b/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java @@ -40,8 +40,8 @@ public KickerIOSpark() { config .inverted(false) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.kDefaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.kNominalVoltage); + .smartCurrentLimit(NEOVortexConstants.defaultSupplyCurrentLimit) + .voltageCompensation(RobotConstants.nominalVoltage); config .encoder @@ -50,7 +50,7 @@ public KickerIOSpark() { .uvwAverageDepth(2) .uvwMeasurementPeriod(8); - config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kPSim, 0.0, kDSim); + config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kP, 0.0, kD); tryUntilOk( flex, @@ -79,7 +79,7 @@ public void setOpenLoop(Voltage volts) { @Override public void setVelocity(LinearVelocity tangentialVelocity) { double feedforwardVolts = - RobotConstants.kNominalVoltage + RobotConstants.nominalVoltage * tangentialVelocity.in(MetersPerSecond) / maxTangentialVelocity.in(MetersPerSecond); controller.setSetpoint( diff --git a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java index dfd30bad..65159fcc 100644 --- a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java @@ -14,6 +14,7 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkFlexConfig; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Voltage; @@ -26,6 +27,7 @@ public class SpindexerIOSimSpark implements SpindexerIO { private static final double SPINDEXER_MOI_KG_M2 = 0.00207; + private static final DCMotor GEARBOX = DCMotor.getNeoVortex(1); private final DCMotorSim spindexerSim; @@ -41,23 +43,23 @@ public SpindexerIOSimSpark() { config .inverted(false) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.kDefaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.kNominalVoltage); + .smartCurrentLimit(NEOVortexConstants.defaultSupplyCurrentLimit) + .voltageCompensation(RobotConstants.nominalVoltage); config .encoder .positionConversionFactor(encoderPositionFactor) .velocityConversionFactor(encoderVelocityFactor); - config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kPSim, 0.0, 0.0); + config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kP, 0.0, kD); flex.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - flexSim = new SparkFlexSim(flex, gearbox); + flexSim = new SparkFlexSim(flex, GEARBOX); spindexerSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(gearbox, SPINDEXER_MOI_KG_M2, motorReduction), - gearbox); + LinearSystemId.createDCMotorSystem(GEARBOX, SPINDEXER_MOI_KG_M2, motorReduction), + GEARBOX); } @Override @@ -78,13 +80,13 @@ public void updateInputs(SpindexerIOInputs inputs) { @Override public void setOpenLoop(Voltage volts) { - flexSim.setAppliedOutput(volts.in(Volts) / RobotConstants.kNominalVoltage); + flexSim.setAppliedOutput(volts.in(Volts) / RobotConstants.nominalVoltage); } @Override public void setVelocity(LinearVelocity tangentialVelocity) { double feedforwardVolts = - RobotConstants.kNominalVoltage + RobotConstants.nominalVoltage * tangentialVelocity.in(MetersPerSecond) / maxTangentialVelocity.in(MetersPerSecond); controller.setSetpoint( diff --git a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java index 5b6403f8..fffae89a 100644 --- a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java +++ b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java @@ -40,8 +40,8 @@ public SpindexerIOSpark() { config .inverted(false) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.kDefaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.kNominalVoltage); + .smartCurrentLimit(NEOVortexConstants.defaultSupplyCurrentLimit) + .voltageCompensation(RobotConstants.nominalVoltage); config .encoder @@ -50,7 +50,7 @@ public SpindexerIOSpark() { .uvwAverageDepth(2) .uvwMeasurementPeriod(8); - config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kPSim, 0.0, kDSim); + config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kP, 0.0, kD); tryUntilOk( flex, @@ -79,7 +79,7 @@ public void setOpenLoop(Voltage volts) { @Override public void setVelocity(LinearVelocity tangentialVelocity) { double feedforwardVolts = - RobotConstants.kNominalVoltage + RobotConstants.nominalVoltage * tangentialVelocity.in(MetersPerSecond) / maxTangentialVelocity.in(MetersPerSecond); controller.setSetpoint( diff --git a/src/main/java/frc/robot/subsystems/hopper/Hopper.java b/src/main/java/frc/robot/subsystems/hopper/Hopper.java index 09eafec1..331976ac 100644 --- a/src/main/java/frc/robot/subsystems/hopper/Hopper.java +++ b/src/main/java/frc/robot/subsystems/hopper/Hopper.java @@ -24,14 +24,14 @@ public Hopper(HopperIO hopperIO) { @Override public void periodic() { - long t0 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t0 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; hopperIO.updateInputs(hopperInputs); - long t1 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t1 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; Logger.processInputs("Hopper", hopperInputs); - long t2 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t2 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; // Profiling output - if (Constants.FeatureFlags.PROFILING_ENABLED) { + if (Constants.FeatureFlags.profilingEnabled) { long totalMs = (t2 - t0) / 1_000_000; if (totalMs > 2) { System.out.println( diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index e0393bee..fdabb831 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -42,16 +42,16 @@ public Intake(RollerIO upperRollerIO, RollerIO lowerRollerIO, IntakeArmIO intake @Override public void periodic() { - long t0 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t0 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; upperRollerIO.updateInputs(upperRollerInputs); lowerRollerIO.updateInputs(lowerRollerInputs); intakeArmIO.updateInputs(intakeArmInputs); - long t1 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t1 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; Logger.processInputs("UpperRoller", upperRollerInputs); Logger.processInputs("LowerRoller", lowerRollerInputs); Logger.processInputs("IntakeArm", intakeArmInputs); - long t2 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t2 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; upperRollerDisconnectedAlert.set(!upperRollerInputs.connected); lowerRollerDisconnectedAlert.set(!lowerRollerInputs.connected); @@ -59,7 +59,7 @@ public void periodic() { Logger.recordOutput("Faults/Intake/LowerRollerDisconnected", !lowerRollerInputs.connected); // Profiling output - if (Constants.FeatureFlags.PROFILING_ENABLED) { + if (Constants.FeatureFlags.profilingEnabled) { long totalMs = (t2 - t0) / 1_000_000; if (totalMs > 2) { System.out.println( diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java index ef4e0449..60b52bc2 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java @@ -29,12 +29,12 @@ public class RollerIOSimSpark implements RollerIO { private static final double KICKER_MOI_KG_M2 = 0.00052; private static final double KP = 0.11; private static final double KD = 0.0; - private static final LinearVelocity maxTangentialVelocity = + private static final LinearVelocity MAX_TANGENTIAL_VELOCITY = MetersPerSecond.of( - NEOVortexConstants.kFreeSpeed.in(RadiansPerSecond) + NEOVortexConstants.freeSpeed.in(RadiansPerSecond) * rollerRadius.in(Meters) / motorReduction); - private static final DCMotor gearbox = DCMotor.getNeoVortex(numMotors); + private static final DCMotor GEARBOX = DCMotor.getNeoVortex(numMotors); private final DCMotorSim rollerSim; @@ -50,8 +50,8 @@ public RollerIOSimSpark(RollerConfig rollerConfig) { config .inverted(rollerConfig.inverted) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.kDefaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.kNominalVoltage); + .smartCurrentLimit(NEOVortexConstants.defaultSupplyCurrentLimit) + .voltageCompensation(RobotConstants.nominalVoltage); config .encoder @@ -61,11 +61,11 @@ public RollerIOSimSpark(RollerConfig rollerConfig) { config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(KP, 0.0, KD); flex.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - flexSim = new SparkFlexSim(flex, gearbox); + flexSim = new SparkFlexSim(flex, GEARBOX); rollerSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(gearbox, KICKER_MOI_KG_M2, motorReduction), gearbox); + LinearSystemId.createDCMotorSystem(GEARBOX, KICKER_MOI_KG_M2, motorReduction), GEARBOX); } @Override @@ -85,15 +85,15 @@ public void updateInputs(RollerIOInputs inputs) { @Override public void setOpenLoop(Voltage volts) { - flexSim.setAppliedOutput(volts.in(Volts) / RobotConstants.kNominalVoltage); + flexSim.setAppliedOutput(volts.in(Volts) / RobotConstants.nominalVoltage); } @Override public void setVelocity(LinearVelocity tangentialVelocity) { double feedforwardVolts = - RobotConstants.kNominalVoltage + RobotConstants.nominalVoltage * tangentialVelocity.in(MetersPerSecond) - / maxTangentialVelocity.in(MetersPerSecond); + / MAX_TANGENTIAL_VELOCITY.in(MetersPerSecond); controller.setSetpoint( tangentialVelocity.in(MetersPerSecond) / rollerRadius.in(Meters), ControlType.kVelocity, diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java index 87096d02..0d917db9 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java @@ -29,13 +29,13 @@ import frc.robot.subsystems.intake.IntakeConstants.RollerConfig; public class RollerIOSimTalonFX implements RollerIO { - private static final double kP = 0.11; - private static final double kD = 0.0; - private static final Slot0Configs velocityVoltageGains = - new Slot0Configs().withKP(kP).withKI(0.0).withKD(kD).withKS(0.1).withKV(0.12); - private static final Slot1Configs velocityTorqueCurrentGains = - new Slot1Configs().withKP(kP).withKI(0.0).withKD(kD).withKS(2.5); - private static final DCMotor gearbox = DCMotor.getKrakenX60(numMotors); + private static final double KP = 0.11; + private static final double KD = 0.0; + private static final Slot0Configs VELOCITY_VOLTAGE_GAINS = + new Slot0Configs().withKP(KP).withKI(0.0).withKD(KD).withKS(0.1).withKV(0.12); + private static final Slot1Configs VELOCITY_TORQUE_CURRENT_GAINS = + new Slot1Configs().withKP(KP).withKI(0.0).withKD(KD).withKS(2.5); + private static final DCMotor GEARBOX = DCMotor.getKrakenX60(numMotors); private final DCMotorSim rollerSim; @@ -63,13 +63,13 @@ public RollerIOSimTalonFX(RollerConfig rollerConfig) { ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; config.MotorOutput.withNeutralMode(NeutralModeValue.Brake); - config.Slot0 = velocityVoltageGains; - config.Slot1 = velocityTorqueCurrentGains; + config.Slot0 = VELOCITY_VOLTAGE_GAINS; + config.Slot1 = VELOCITY_TORQUE_CURRENT_GAINS; tryUntilOk(5, () -> motor.getConfigurator().apply(config, 0.25)); rollerSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(gearbox, 0.0005, motorReduction), gearbox); + LinearSystemId.createDCMotorSystem(GEARBOX, 0.0005, motorReduction), GEARBOX); velocity = motor.getVelocity(); acceleration = motor.getAcceleration(); diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java index 48357e13..257b3093 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java @@ -26,9 +26,9 @@ public class RollerIOSpark implements RollerIO { private static final double KP = 0.001; private static final double KD = 0.0; - private static final LinearVelocity maxTangentialVelocity = + private static final LinearVelocity MAX_TANGENTIAL_VELOCITY = MetersPerSecond.of( - NEOVortexConstants.kFreeSpeed.in(RadiansPerSecond) + NEOVortexConstants.freeSpeed.in(RadiansPerSecond) * rollerRadius.in(Meters) / motorReduction); @@ -46,8 +46,8 @@ public RollerIOSpark(RollerConfig rollerConfig) { config .inverted(rollerConfig.inverted) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.kDefaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.kNominalVoltage); + .smartCurrentLimit(NEOVortexConstants.defaultSupplyCurrentLimit) + .voltageCompensation(RobotConstants.nominalVoltage); config .encoder @@ -84,9 +84,9 @@ public void setOpenLoop(Voltage volts) { @Override public void setVelocity(LinearVelocity tangentialVelocity) { double feedforwardVolts = - RobotConstants.kNominalVoltage + RobotConstants.nominalVoltage * tangentialVelocity.in(MetersPerSecond) - / maxTangentialVelocity.in(MetersPerSecond); + / MAX_TANGENTIAL_VELOCITY.in(MetersPerSecond); controller.setSetpoint( tangentialVelocity.in(MetersPerSecond) / rollerRadius.in(Meters), ControlType.kVelocity, diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java index 05cc7657..cd3484cf 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java @@ -25,12 +25,12 @@ import frc.robot.subsystems.intake.IntakeConstants.RollerConfig; public class RollerIOTalonFX implements RollerIO { - private static final double kP = 0.11; - private static final double kD = 0.0; - private static final Slot0Configs velocityVoltageGains = - new Slot0Configs().withKP(kP).withKI(0.0).withKD(kD).withKS(0.1).withKV(0.12); - private static final Slot1Configs velocityTorqueCurrentGains = - new Slot1Configs().withKP(kP).withKI(0.0).withKD(kD).withKS(2.5); + private static final double KP = 0.11; + private static final double KD = 0.0; + private static final Slot0Configs VELOCITY_VOLTAGE_GAINS = + new Slot0Configs().withKP(KP).withKI(0.0).withKD(KD).withKS(0.1).withKV(0.12); + private static final Slot1Configs VELOCITY_TORQUE_CURRENT_GAINS = + new Slot1Configs().withKP(KP).withKI(0.0).withKD(KD).withKS(2.5); private final TalonFX motor; private final TalonFXConfiguration config; @@ -51,19 +51,20 @@ public class RollerIOTalonFX implements RollerIO { public RollerIOTalonFX(RollerConfig rollerConfig) { motor = new TalonFX(rollerConfig.port, rollerConfig.bus); config = new TalonFXConfiguration(); - config.TorqueCurrent.PeakForwardTorqueCurrent = KrakenX60Constants.kDefaultStatorCurrentLimit; - config.TorqueCurrent.PeakReverseTorqueCurrent = -KrakenX60Constants.kDefaultStatorCurrentLimit; - config.CurrentLimits.StatorCurrentLimit = KrakenX60Constants.kDefaultStatorCurrentLimit; + config.TorqueCurrent.PeakForwardTorqueCurrent = KrakenX60Constants.defaultStatorCurrentLimit; + config.TorqueCurrent.PeakReverseTorqueCurrent = + -KrakenX60Constants.defaultStatorCurrentLimit; + config.CurrentLimits.StatorCurrentLimit = KrakenX60Constants.defaultStatorCurrentLimit; config.CurrentLimits.StatorCurrentLimitEnable = true; - config.CurrentLimits.SupplyCurrentLimit = KrakenX60Constants.kDefaultSupplyCurrentLimit; + config.CurrentLimits.SupplyCurrentLimit = KrakenX60Constants.defaultSupplyCurrentLimit; config.CurrentLimits.SupplyCurrentLimitEnable = true; config.MotorOutput.Inverted = rollerConfig.inverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; config.MotorOutput.withNeutralMode(NeutralModeValue.Brake); - config.Slot0 = velocityVoltageGains; - config.Slot1 = velocityTorqueCurrentGains; + config.Slot0 = VELOCITY_VOLTAGE_GAINS; + config.Slot1 = VELOCITY_TORQUE_CURRENT_GAINS; tryUntilOk(5, () -> motor.getConfigurator().apply(config, 0.25)); // -1 tryUntilOkay velocity = motor.getVelocity(); diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimWPI.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimWPI.java index 26a8c409..1d811bd3 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimWPI.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimWPI.java @@ -13,10 +13,12 @@ import frc.robot.Robot; public class FlywheelIOSimWPI implements FlywheelIO { + private static final double KP_SIM = 0.1; + private final DCMotorSim flywheelSim; private boolean closedLoop = false; - private PIDController velocityController = new PIDController(kPSim, 0.0, 0.0); + private PIDController velocityController = new PIDController(KP_SIM, 0.0, 0.0); private double appliedVolts = 0.0; private double feedforwardVolts = 0.0; @@ -39,7 +41,7 @@ public void updateInputs(FlywheelIOInputs inputs) { // Update simulation state flywheelSim.setInputVoltage( MathUtil.clamp( - appliedVolts, -RobotConstants.kNominalVoltage, RobotConstants.kNominalVoltage)); + appliedVolts, -RobotConstants.nominalVoltage, RobotConstants.nominalVoltage)); flywheelSim.update(Robot.defaultPeriodSecs); // Update turn inputs @@ -60,7 +62,7 @@ public void setOpenLoop(Voltage volts) { public void setVelocity(LinearVelocity tangentialVelocity) { closedLoop = true; this.feedforwardVolts = - RobotConstants.kNominalVoltage + RobotConstants.nominalVoltage * tangentialVelocity.in(MetersPerSecond) / maxAngularVelocity.in(RadiansPerSecond); velocityController.setSetpoint(tangentialVelocity.in(MetersPerSecond)); diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java index 9ea59578..a366e1a8 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java @@ -56,11 +56,12 @@ public FlywheelIOTalonFX() { .withNeutralMode(NeutralModeValue.Brake); config.Slot0 = velocityVoltageGains; config.Slot1 = velocityTorqueCurrentGains; - config.TorqueCurrent.PeakForwardTorqueCurrent = KrakenX60Constants.kDefaultStatorCurrentLimit; - config.TorqueCurrent.PeakReverseTorqueCurrent = -KrakenX60Constants.kDefaultStatorCurrentLimit; - config.CurrentLimits.StatorCurrentLimit = KrakenX60Constants.kDefaultStatorCurrentLimit; + config.TorqueCurrent.PeakForwardTorqueCurrent = KrakenX60Constants.defaultStatorCurrentLimit; + config.TorqueCurrent.PeakReverseTorqueCurrent = + -KrakenX60Constants.defaultStatorCurrentLimit; + config.CurrentLimits.StatorCurrentLimit = KrakenX60Constants.defaultStatorCurrentLimit; config.CurrentLimits.StatorCurrentLimitEnable = true; - config.CurrentLimits.SupplyCurrentLimit = KrakenX60Constants.kDefaultSupplyCurrentLimit; + config.CurrentLimits.SupplyCurrentLimit = KrakenX60Constants.defaultSupplyCurrentLimit; config.CurrentLimits.SupplyCurrentLimitEnable = true; tryUntilOk(5, () -> flywheelLeaderTalon.getConfigurator().apply(config, 0.25)); tryUntilOk(5, () -> flywheelFollowerTalon.getConfigurator().apply(config, 0.25)); diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSimSpark.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSimSpark.java index daa1885f..7465f4e1 100644 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIOSimSpark.java @@ -45,8 +45,8 @@ public HoodIOSimSpark() { hoodConfig .inverted(false) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEO550Constants.kDefaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.kNominalVoltage); + .smartCurrentLimit(NEO550Constants.defaultSupplyCurrentLimit) + .voltageCompensation(RobotConstants.nominalVoltage); hoodConfig .encoder @@ -110,14 +110,14 @@ public void updateInputs(HoodIOInputs inputs) { @Override public void setOpenLoop(Voltage volts) { - maxSim.setAppliedOutput(volts.in(Volts) / RobotConstants.kNominalVoltage); + maxSim.setAppliedOutput(volts.in(Volts) / RobotConstants.nominalVoltage); } @Override public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { double setpoint = MathUtil.clamp(rotation.getRadians(), minPosRad, maxPosRad); double feedforwardVolts = - RobotConstants.kNominalVoltage + RobotConstants.nominalVoltage * angularVelocity.in(RadiansPerSecond) / maxAngularVelocity.in(RadiansPerSecond); controller.setSetpoint( @@ -128,7 +128,7 @@ public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { public void setVelocity(AngularVelocity angularVelocity) { double setpoint = angularVelocity.in(RadiansPerSecond); double feedforwardVolts = - RobotConstants.kNominalVoltage + RobotConstants.nominalVoltage * angularVelocity.in(RadiansPerSecond) / maxAngularVelocity.in(RadiansPerSecond); controller.setSetpoint( diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSimWPI.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSimWPI.java index 80af5024..e9d91531 100644 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIOSimWPI.java +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIOSimWPI.java @@ -44,7 +44,7 @@ public void updateInputs(HoodIOInputs inputs) { // Update simulation state hoodSim.setInputVoltage( MathUtil.clamp( - appliedVolts, -RobotConstants.kNominalVoltage, RobotConstants.kNominalVoltage)); + appliedVolts, -RobotConstants.nominalVoltage, RobotConstants.nominalVoltage)); hoodSim.update(Robot.defaultPeriodSecs); // Update turn inputs @@ -70,7 +70,7 @@ public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { closedLoop = true; double setpoint = MathUtil.clamp(rotation.getRadians(), minPosRad, maxPosRad); this.feedforwardVolts = - RobotConstants.kNominalVoltage + RobotConstants.nominalVoltage * angularVelocity.in(RadiansPerSecond) / maxAngularVelocity.in(RadiansPerSecond); positionController.setSetpoint(setpoint); diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java index 0336c204..d7ae5184 100644 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java @@ -47,8 +47,8 @@ public HoodIOSpark() { hoodConfig .inverted(true) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEO550Constants.kDefaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.kNominalVoltage); + .smartCurrentLimit(NEO550Constants.defaultSupplyCurrentLimit) + .voltageCompensation(RobotConstants.nominalVoltage); hoodConfig .encoder @@ -101,7 +101,7 @@ public void setOpenLoop(Voltage volts) { public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { double setpoint = MathUtil.clamp(rotation.getRadians(), minPosRad, maxPosRad); double feedforwardVolts = - RobotConstants.kNominalVoltage + RobotConstants.nominalVoltage * angularVelocity.in(RadiansPerSecond) / maxAngularVelocity.in(RadiansPerSecond); hoodController.setSetpoint( @@ -112,7 +112,7 @@ public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { public void setVelocity(AngularVelocity angularVelocity) { double setpoint = angularVelocity.in(RadiansPerSecond); double feedforwardVolts = - RobotConstants.kNominalVoltage + RobotConstants.nominalVoltage * angularVelocity.in(RadiansPerSecond) / maxAngularVelocity.in(RadiansPerSecond); hoodController.setSetpoint( diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index cc4e621c..a7cb7e8d 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -93,17 +93,17 @@ public Launcher( @Override public void periodic() { - long t0 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t0 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; turretIO.updateInputs(turretInputs); flywheelIO.updateInputs(flywheelInputs); hoodIO.updateInputs(hoodInputs); - long t1 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t1 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; Logger.processInputs("Turret", turretInputs); Logger.processInputs("Flywheel", flywheelInputs); Logger.processInputs("Hood", hoodInputs); - long t2 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t2 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; turretDisconnectedAlert.set(!turretInputs.motorControllerConnected); flywheelDisconnectedAlert.set(!flywheelInputs.connected); @@ -118,7 +118,7 @@ public void periodic() { logCachedAimData(); hasCachedAimData = false; } - long t3 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t3 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; // Update and plot ball trajectories if (logFuelTrajectories) { @@ -142,10 +142,10 @@ public void periodic() { ballisticLogTimer = 0.0; } } - long t4 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t4 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; // Profiling output - if (Constants.FeatureFlags.PROFILING_ENABLED) { + if (Constants.FeatureFlags.profilingEnabled) { long totalMs = (t4 - t0) / 1_000_000; if (totalMs > 3) { System.out.println( @@ -176,7 +176,7 @@ public double getSimCurrentDrawAmps() { } public void aim(Translation3d target) { - long aimStart = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long aimStart = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; // Get vector from static target to turret turretBasePose = new Pose3d(chassisPoseSupplier.get()).plus(chassisToTurretBase); @@ -191,7 +191,7 @@ public void aim(Translation3d target) { var v0_nominal = getV0Nominal(vectorTurretBaseToTarget, dynamicImpactAngle, nominalKey); var flywheelSetpoint = MetersPerSecond.of(flywheelSetpointfromBallistics(v0_nominal.getNorm())); flywheelIO.setVelocity(flywheelSetpoint); - long t1 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t1 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; // Get translation velocities (m/s) of the turret caused by motion of the chassis var robotRelative = chassisSpeedsSupplier.get(); @@ -199,7 +199,7 @@ public void aim(Translation3d target) { ChassisSpeeds.fromRobotRelativeSpeeds( robotRelative, turretBasePose.toPose2d().getRotation()); var v_base = getTurretBaseSpeeds(turretBasePose.toPose2d().getRotation(), fieldRelative); - long t2 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t2 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; // Get actual initial shot speed double initialSpeedMetersPerSec = @@ -207,7 +207,7 @@ public void aim(Translation3d target) { // Replan shot using actual initial shot speed speed var v0_total = getV0Replanned(vectorTurretBaseToTarget, initialSpeedMetersPerSec, replannedKey); - long t3 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t3 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; // Point turret to align velocity vectors var v0_flywheel = v0_total.minus(v_base); @@ -229,7 +229,7 @@ public void aim(Translation3d target) { RadiansPerSecond.of(robotRelative.omegaRadiansPerSecond).unaryMinus().times(2.0)); hoodSetpoint = new Rotation2d(v0_horizontal, v0_flywheel.getZ()).minus(ballToHoodOffset); hoodIO.setPosition(hoodSetpoint, RadiansPerSecond.of(0)); - long t4 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t4 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; // Get actual hood & turret position Rotation2d hoodPosition = hoodInputs.position.plus(ballToHoodOffset); @@ -252,7 +252,7 @@ public void aim(Translation3d target) { cachedActualD = vectorTurretBaseToTarget; cachedActualV = v0_actual; hasCachedAimData = true; - long t5 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t5 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; // Spawn simulated fuel fuelSpawnTimer += Robot.defaultPeriodSecs; @@ -268,7 +268,7 @@ public void aim(Translation3d target) { } // Profiling output for aim() - if (Constants.FeatureFlags.PROFILING_ENABLED) { + if (Constants.FeatureFlags.profilingEnabled) { long totalUs = (t5 - aimStart) / 1_000; if (totalUs > 500) { System.out.println( diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index 36c7b08f..42a0ea21 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -85,21 +85,18 @@ public static final class TurretConstants { public static final double marginRad = Units.degreesToRadians(5); // Position controller - public static final double kPReal = 0.5; + public static final double kP = 0.5; + public static final double kD = 0.05; public static final Angle kAllowableError = Degrees.of(0.25); // Motor controller public static final double motorReduction = 9.0 * 72.0 / 12.0; public static final AngularVelocity maxAngularVelocity = - NEO550Constants.kFreeSpeed.div(motorReduction); + NEO550Constants.freeSpeed.div(motorReduction); public static final double encoderPositionFactor = (2 * Math.PI) / motorReduction; // Radians public static final double encoderVelocityFactor = (2 * Math.PI) / (60.0 * motorReduction); // Rad/sec - // Simulation - public static final DCMotor gearbox = DCMotor.getNeo550(1); - public static final double kPSim = 0.5; - public static final double kDSim = 0.05; } public static final class FlywheelConstants { @@ -118,14 +115,13 @@ public static final class FlywheelScaling { // Motor controller public static final double motorReduction = 1.0; public static final AngularVelocity maxAngularVelocity = - KrakenX60Constants.kFreeSpeed.div(motorReduction); + KrakenX60Constants.freeSpeed.div(motorReduction); public static final Slot0Configs velocityVoltageGains = new Slot0Configs().withKP(0.11).withKI(0.0).withKD(0.0).withKS(0.1).withKV(0.12); public static final Slot1Configs velocityTorqueCurrentGains = new Slot1Configs().withKP(12).withKI(0.0).withKD(0.0).withKS(2.5); // Simulation - public static final double kPSim = 0.1; public static final DCMotor gearbox = DCMotor.getKrakenX60(2); } @@ -148,7 +144,7 @@ public static final class HoodConstants { // Motor controller public static final double motorReduction = 5.0 * 256.0 / 16.0; public static final AngularVelocity maxAngularVelocity = - NEO550Constants.kFreeSpeed.div(motorReduction); + NEO550Constants.freeSpeed.div(motorReduction); public static final double encoderPositionFactor = 2 * Math.PI / motorReduction; // Radians public static final double encoderVelocityFactor = (2 * Math.PI) / (60.0 * motorReduction); // Rad/sec diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java index a0101ab7..67683d11 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java @@ -16,6 +16,7 @@ import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Voltage; @@ -28,6 +29,7 @@ public class TurretIOSimSpark implements TurretIO { private static final double TURRET_MOI_KG_M2 = 0.237; + private static final DCMotor GEARBOX = DCMotor.getNeo550(1); private final DCMotorSim turnSim; @@ -47,8 +49,8 @@ public TurretIOSimSpark() { turnConfig .inverted(false) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEO550Constants.kDefaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.kNominalVoltage); + .smartCurrentLimit(NEO550Constants.defaultSupplyCurrentLimit) + .voltageCompensation(RobotConstants.nominalVoltage); turnConfig .encoder @@ -65,17 +67,17 @@ public TurretIOSimSpark() { turnConfig .closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) - .pid(kPSim, 0.0, kDSim) + .pid(kP, 0.0, kD) .allowedClosedLoopError(kAllowableError.in(Radians), ClosedLoopSlot.kSlot0); turnConfig.signals.appliedOutputPeriodMs(20).busVoltagePeriodMs(20).outputCurrentPeriodMs(20); turnSpark.configure(turnConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - turnSparkSim = new SparkMaxSim(turnSpark, gearbox); + turnSparkSim = new SparkMaxSim(turnSpark, GEARBOX); turnSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(gearbox, TURRET_MOI_KG_M2, motorReduction), gearbox); + LinearSystemId.createDCMotorSystem(GEARBOX, TURRET_MOI_KG_M2, motorReduction), GEARBOX); turnSim.setState(2.0 * Math.PI - mechanismOffset.getRadians(), 0); turnSparkSim.setPosition(turnSim.getAngularPositionRad()); @@ -122,7 +124,7 @@ public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { oversaturation = setpoint - clampedSetpoint; oversaturationLessMargin = setpoint - clampedSetpointWithMargin; double feedforwardVolts = - RobotConstants.kNominalVoltage + RobotConstants.nominalVoltage * angularVelocity.in(RadiansPerSecond) / maxAngularVelocity.in(RadiansPerSecond); controller.setSetpoint( diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java index 7b14f37d..d7e85267 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java @@ -61,8 +61,8 @@ public TurretIOSpark() { turnConfig .inverted(true) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEO550Constants.kDefaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.kNominalVoltage); + .smartCurrentLimit(NEO550Constants.defaultSupplyCurrentLimit) + .voltageCompensation(RobotConstants.nominalVoltage); turnConfig .encoder @@ -79,7 +79,7 @@ public TurretIOSpark() { turnConfig .closedLoop .feedbackSensor(FeedbackSensor.kPrimaryEncoder) - .pid(kPReal, 0.0, 0.0) + .pid(kP, 0.0, kD) .allowedClosedLoopError(kAllowableError.in(Radians), ClosedLoopSlot.kSlot0); turnConfig.signals.appliedOutputPeriodMs(20).busVoltagePeriodMs(20).outputCurrentPeriodMs(20); @@ -137,7 +137,7 @@ public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { oversaturation = setpoint - clampedSetpoint; oversaturationLessMargin = setpoint - clampedSetpointWithMargin; double feedforwardVolts = - RobotConstants.kNominalVoltage + RobotConstants.nominalVoltage * angularVelocity.in(RadiansPerSecond) / maxAngularVelocity.in(RadiansPerSecond); controller.setSetpoint( diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index c967e830..3fe74f0d 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -22,7 +22,7 @@ import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; +import frc.robot.Constants.FeatureFlags; import frc.robot.subsystems.vision.VisionFilter.FusedObservation; import frc.robot.subsystems.vision.VisionFilter.Test; import frc.robot.subsystems.vision.VisionFilter.TestedObservation; @@ -123,14 +123,14 @@ public Rotation2d getTargetX(int cameraIndex) { @Override public void periodic() { - long visionStart = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long visionStart = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; loopCounter++; // Copy cached inputs from background thread (should be fast - volatile reads) for (int i = 0; i < io.length; i++) { visionInputs[i].getSnapshot().copyTo(inputs[i]); } - long t1 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t1 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; // Log inputs via AdvantageKit (throttled - serialization is expensive) // Note: Throttling reduces CPU load but loses data granularity for replay @@ -139,7 +139,7 @@ public void periodic() { Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]); } } - long t2 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t2 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; // Initialize logging values allTagPoses.clear(); @@ -217,7 +217,7 @@ public void periodic() { allRobotPosesRejected.addAll(robotPosesRejected); } - long t3 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t3 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; // Process observations in batches every processingIntervalLoops // This allows cameras to accumulate observations before fusion decides what agrees @@ -254,7 +254,7 @@ public void periodic() { // Clear buffer after processing observationBuffer.clear(); } - long t4 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t4 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; // Log summary data (throttled along with processInputs) if (loopCounter % kLoggingDivisor == 0) { @@ -271,10 +271,10 @@ public void periodic() { "Vision/Summary/RobotPosesRejected", allRobotPosesRejected.toArray(Pose3d[]::new)); } } - long t5 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t5 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; // Profiling output - if (Constants.FeatureFlags.PROFILING_ENABLED) { + if (FeatureFlags.profilingEnabled) { long totalMs = (t5 - visionStart) / 1_000_000; if (totalMs > 5) { System.out.println( From 333501cb2e9b1de6df31b5740c2c6730a2f1a9b9 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Thu, 4 Jun 2026 11:11:39 -0400 Subject: [PATCH 2/8] Switch shared constants classes to UPPER_SNAKE_CASE naming All public/private static final fields in Constants, DriveConstants, LauncherConstants, FeederConstants, IntakeConstants, and VisionConstants now use UPPER_SNAKE_CASE per Java convention. k-prefix gain constants (kP, kD, kAllowableError, etc.) are left unchanged per FRC convention. Co-Authored-By: Claude Sonnet 4.6 --- src/main/java/frc/robot/Constants.java | 28 +- src/main/java/frc/robot/Robot.java | 36 +-- .../frc/robot/commands/DriveCommands.java | 8 +- .../java/frc/robot/commands/PathCommands.java | 2 +- .../frc/robot/subsystems/drive/Drive.java | 32 +-- .../subsystems/drive/DriveConstants.java | 244 +++++++++--------- .../frc/robot/subsystems/drive/Module.java | 24 +- .../drive/PhoenixOdometryThread.java | 2 +- .../frc/robot/subsystems/feeder/Feeder.java | 12 +- .../subsystems/feeder/FeederConstants.java | 26 +- .../subsystems/feeder/KickerIOSimSpark.java | 17 +- .../subsystems/feeder/KickerIOSpark.java | 12 +- .../feeder/SpindexerIOSimSpark.java | 16 +- .../subsystems/feeder/SpindexerIOSpark.java | 12 +- .../frc/robot/subsystems/hopper/Hopper.java | 8 +- .../frc/robot/subsystems/intake/Intake.java | 8 +- .../subsystems/intake/IntakeConstants.java | 11 +- .../subsystems/intake/RollerIOSimSpark.java | 19 +- .../subsystems/intake/RollerIOSimTalonFX.java | 10 +- .../subsystems/intake/RollerIOSpark.java | 14 +- .../subsystems/intake/RollerIOTalonFX.java | 14 +- .../launcher/FlywheelIOSimTalonFX.java | 18 +- .../subsystems/launcher/FlywheelIOSimWPI.java | 11 +- .../launcher/FlywheelIOTalonFX.java | 20 +- .../subsystems/launcher/HoodIOSimSpark.java | 41 +-- .../subsystems/launcher/HoodIOSimWPI.java | 17 +- .../subsystems/launcher/HoodIOSpark.java | 24 +- .../robot/subsystems/launcher/Launcher.java | 91 +++---- .../launcher/LauncherConstants.java | 133 +++++----- .../subsystems/launcher/TurretIOSimSpark.java | 31 +-- .../subsystems/launcher/TurretIOSpark.java | 26 +- .../frc/robot/subsystems/vision/Vision.java | 14 +- .../subsystems/vision/VisionConstants.java | 2 +- 33 files changed, 496 insertions(+), 487 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d62fc3c2..bb79dcb2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -41,38 +41,38 @@ public static enum Mode { public static final class FeatureFlags { /** Enable to print loop timing when total exceeds 20ms. */ - public static final boolean profilingEnabled = false; + public static final boolean PROFILING_ENABLED = false; /** Set to false to disable the hopper subsystem entirely. */ - public static final boolean hopperEnabled = false; + public static final boolean HOPPER_ENABLED = false; } public final class RobotConstants { - public static final double nominalVoltage = 12.0; + public static final double NOMINAL_VOLTAGE = 12.0; } public static final class MotorConstants { public static final class NEOConstants { - public static final AngularVelocity freeSpeed = RPM.of(5676); - public static final int defaultSupplyCurrentLimit = 60; - public static final int defaultStatorCurrentLimit = 100; + public static final AngularVelocity FREE_SPEED = RPM.of(5676); + public static final int DEFAULT_SUPPLY_CURRENT_LIMIT = 60; + public static final int DEFAULT_STATOR_CURRENT_LIMIT = 100; } public static final class NEO550Constants { - public static final AngularVelocity freeSpeed = RPM.of(11000); - public static final int defaultSupplyCurrentLimit = 10; + public static final AngularVelocity FREE_SPEED = RPM.of(11000); + public static final int DEFAULT_SUPPLY_CURRENT_LIMIT = 10; } public static final class NEOVortexConstants { - public static final AngularVelocity freeSpeed = RPM.of(6784); - public static final int defaultSupplyCurrentLimit = 60; - public static final int defaultStatorCurrentLimit = 100; + public static final AngularVelocity FREE_SPEED = RPM.of(6784); + public static final int DEFAULT_SUPPLY_CURRENT_LIMIT = 60; + public static final int DEFAULT_STATOR_CURRENT_LIMIT = 100; } public static final class KrakenX60Constants { - public static final AngularVelocity freeSpeed = RPM.of(6000); - public static final int defaultSupplyCurrentLimit = 60; - public static final int defaultStatorCurrentLimit = 100; + public static final AngularVelocity FREE_SPEED = RPM.of(6000); + public static final int DEFAULT_SUPPLY_CURRENT_LIMIT = 60; + public static final int DEFAULT_STATOR_CURRENT_LIMIT = 100; } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b93cf444..7a764b30 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -177,10 +177,10 @@ public Robot() { drive = new Drive( new GyroIOBoron(), - new ModuleIOTalonFX(DriveConstants.frontLeft), - new ModuleIOTalonFX(DriveConstants.frontRight), - new ModuleIOTalonFX(DriveConstants.backLeft), - new ModuleIOTalonFX(DriveConstants.backRight)); + new ModuleIOTalonFX(DriveConstants.FRONT_LEFT), + new ModuleIOTalonFX(DriveConstants.FRONT_RIGHT), + new ModuleIOTalonFX(DriveConstants.BACK_LEFT), + new ModuleIOTalonFX(DriveConstants.BACK_RIGHT)); vision = new Vision( drive::addVisionMeasurement, @@ -196,7 +196,7 @@ public Robot() { new TurretIOSpark(), new FlywheelIOTalonFX(), new HoodIOSpark()); - if (FeatureFlags.hopperEnabled) hopper = new Hopper(new HopperIOReal()); + if (FeatureFlags.HOPPER_ENABLED) hopper = new Hopper(new HopperIOReal()); intake = new Intake( new RollerIOSpark(RollerConstants.upperRollerConfig), @@ -218,10 +218,10 @@ public Robot() { drive = new Drive( new GyroIO() {}, - new ModuleIOSimWPI(DriveConstants.frontLeft), - new ModuleIOSimWPI(DriveConstants.frontRight), - new ModuleIOSimWPI(DriveConstants.backLeft), - new ModuleIOSimWPI(DriveConstants.backRight)); + new ModuleIOSimWPI(DriveConstants.FRONT_LEFT), + new ModuleIOSimWPI(DriveConstants.FRONT_RIGHT), + new ModuleIOSimWPI(DriveConstants.BACK_LEFT), + new ModuleIOSimWPI(DriveConstants.BACK_RIGHT)); vision = new Vision( drive::addVisionMeasurement, @@ -242,7 +242,7 @@ public Robot() { new FlywheelIOSimTalonFX(), new HoodIOSimSpark()); feeder = new Feeder(new SpindexerIOSimSpark(), new KickerIOSimSpark()); - if (FeatureFlags.hopperEnabled) hopper = new Hopper(new HopperIOSim()); + if (FeatureFlags.HOPPER_ENABLED) hopper = new Hopper(new HopperIOSim()); var intakeArmIOSim = new IntakeArmIOSim(); intake = new Intake( @@ -284,7 +284,7 @@ public Robot() { new TurretIO() {}, new FlywheelIO() {}, new HoodIO() {}); - if (FeatureFlags.hopperEnabled) hopper = new Hopper(new HopperIO() {}); + if (FeatureFlags.HOPPER_ENABLED) hopper = new Hopper(new HopperIO() {}); intake = new Intake(new RollerIO() {}, new RollerIO() {}, new IntakeArmIO() {}); feeder = new Feeder(new SpindexerIO() {}, new KickerIO() {}); break; @@ -303,7 +303,7 @@ public Robot() { // Wire the hopper/intake interlocks. Done here (after both subsystems exist) to avoid a // circular dependency between the two subsystems. - if (FeatureFlags.hopperEnabled) { + if (FeatureFlags.HOPPER_ENABLED) { intake.setDeployInterlock( hopper::isDeployed, () -> hopper.getDeployCommand().withTimeout(IntakeConstants.kInterlockSettleSeconds)); @@ -343,7 +343,7 @@ public Robot() { /** This function is called periodically during all modes. */ @Override public void robotPeriodic() { - long loopStart = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long loopStart = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Runs the Scheduler. This is responsible for polling buttons, adding // newly-scheduled commands, running already-scheduled commands, removing @@ -351,7 +351,7 @@ public void robotPeriodic() { // This must be called from the robot's periodic block in order for anything in // the Command-based framework to work. CommandScheduler.getInstance().run(); - long t1 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t1 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; logCANBus("CAN2", Constants.CANBusPorts.CAN2.bus); logCANBus("CANHD", Constants.CANBusPorts.CANHD.bus); @@ -362,16 +362,16 @@ public void robotPeriodic() { Logger.recordOutput("USB/FreeSpaceMB", getUSBStorageFreeSpace() / 1024 / 1024); GameState.logValues(); - long t2 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t2 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Publish kernel log events to NetworkTables (only runs on real robot) if (RobotBase.isReal()) { KernelLogMonitor.getInstance().publishToLogger(); } - long t3 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t3 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Profiling output - if (FeatureFlags.profilingEnabled) { + if (FeatureFlags.PROFILING_ENABLED) { long schedulerMs = (t1 - loopStart) / 1_000_000; long gameStateMs = (t2 - t1) / 1_000_000; long kernelMonitorMs = (t3 - t2) / 1_000_000; @@ -549,7 +549,7 @@ public boolean getFieldRelativeInput() { // Toggle hopper: deploy if stowed, stow if deployed (retracting intake first if needed). // runOnce has no subsystem requirements so it always executes; the scheduled command // requires hopper and will interrupt whatever is currently running on that subsystem. - if (FeatureFlags.hopperEnabled) + if (FeatureFlags.HOPPER_ENABLED) zorroDriver .DIn() .onTrue( diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 67edd999..f708faea 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -486,8 +486,8 @@ public static Command wheelRadiusCharacterization(Drive drive) { return; } - double wheelRadius = - (state.gyroDelta * driveBaseRadius.in(Meters)) / wheelDelta; + double WHEEL_RADIUS = + (state.gyroDelta * DRIVE_BASE_RADIUS.in(Meters)) / wheelDelta; System.out.println( "\tWheel Delta: " + formatter.format(wheelDelta) + " radians"); @@ -495,9 +495,9 @@ public static Command wheelRadiusCharacterization(Drive drive) { "\tGyro Delta: " + formatter.format(state.gyroDelta) + " radians"); System.out.println( "\tWheel Radius: " - + formatter.format(wheelRadius) + + formatter.format(WHEEL_RADIUS) + " meters, " - + formatter.format(Units.metersToInches(wheelRadius)) + + formatter.format(Units.metersToInches(WHEEL_RADIUS)) + " inches"); }))); } diff --git a/src/main/java/frc/robot/commands/PathCommands.java b/src/main/java/frc/robot/commands/PathCommands.java index 6c7f9306..8e79a8de 100644 --- a/src/main/java/frc/robot/commands/PathCommands.java +++ b/src/main/java/frc/robot/commands/PathCommands.java @@ -157,7 +157,7 @@ private static PathPlannerPath createPath(List waypoints, Rotation2d e var path = new PathPlannerPath( waypoints, - pathFollowingConstraints, + PATH_FOLLOWING_CONSTRAINTS, // The ideal starting state, this is only relevant for pre-planned paths, // so can be null for on-the-fly paths. null, diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 3969aec7..376aab37 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -57,7 +57,7 @@ public class Drive extends SubsystemBase { static final double ODOMETRY_FREQUENCY = - new CANBus(drivetrainConstants.CANBusName).isNetworkFD() ? 250.0 : 100.0; + new CANBus(DRIVETRAIN_CONSTANTS.CANBusName).isNetworkFD() ? 250.0 : 100.0; protected static final Lock ODOMETRY_LOCK = new ReentrantLock(); private final GyroIO gyroIO; @@ -67,7 +67,7 @@ public class Drive extends SubsystemBase { private final Alert gyroDisconnectedAlert = new Alert("Disconnected gyro, using kinematics as fallback.", AlertType.kError); - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(moduleTranslations); + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(MODULE_TRANSLATIONS); private Rotation2d rawGyroRotation = Rotation2d.kZero; private SwerveModulePosition[] lastModulePositions = // For delta tracking new SwerveModulePosition[] { @@ -126,7 +126,7 @@ public Drive( this::runVelocity, new PPHolonomicDriveController( new PIDConstants(5.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)), - ppConfig, + PP_CONFIG, () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, this); Pathfinding.setPathfinder(new LocalADStarAK()); @@ -156,18 +156,18 @@ public Drive( @Override public void periodic() { - long startNanos = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long startNanos = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; ODOMETRY_LOCK.lock(); // Prevents odometry updates while reading data - long t1 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t1 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; gyroIO.updateInputs(gyroInputs); - long t2 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t2 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; Logger.processInputs("Drive/Gyro", gyroInputs); - long t3 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t3 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; for (var module : modules) { module.periodic(); } - long t4 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t4 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; ODOMETRY_LOCK.unlock(); // Stop moving when disabled @@ -182,7 +182,7 @@ public void periodic() { Logger.recordOutput("SwerveStates/Setpoints", emptyModuleStates); Logger.recordOutput("SwerveStates/SetpointsOptimized", emptyModuleStates); } - long t5 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t5 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Update odometry double[] sampleTimestamps = @@ -216,7 +216,7 @@ public void periodic() { chassisSpeeds = kinematics.toChassisSpeeds(getModuleStates()); } - long t6 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t6 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Update gyro alert boolean gyroDisconnected = !gyroInputs.connected && Constants.currentMode != Mode.SIM; @@ -224,7 +224,7 @@ public void periodic() { Logger.recordOutput("Faults/Drive/GyroDisconnected", gyroDisconnected); // Profiling output - if (FeatureFlags.profilingEnabled) { + if (FeatureFlags.PROFILING_ENABLED) { long totalMs = (t6 - startNanos) / 1_000_000; if (totalMs > 5) { System.out.println( @@ -262,7 +262,7 @@ public void runVelocity(ChassisSpeeds speeds) { Logger.recordOutput("SwerveStates/Setpoints", states); // 2: Desaturate (apply wheel limits FIRST) - SwerveDriveKinematics.desaturateWheelSpeeds(states, drivetrainSpeedLimit.in(MetersPerSecond)); + SwerveDriveKinematics.desaturateWheelSpeeds(states, DRIVETRAIN_SPEED_LIMIT.in(MetersPerSecond)); // 3: Reconstruct the ACTUAL chassis speeds after limiting ChassisSpeeds limitedSpeeds = kinematics.toChassisSpeeds(states); @@ -276,7 +276,7 @@ public void runVelocity(ChassisSpeeds speeds) { // (Optional but usually unnecessary) // desaturate again for safety SwerveDriveKinematics.desaturateWheelSpeeds( - finalStates, drivetrainSpeedLimit.in(MetersPerSecond)); + finalStates, DRIVETRAIN_SPEED_LIMIT.in(MetersPerSecond)); // 6: Send to modules for (int i = 0; i < 4; i++) { @@ -322,7 +322,7 @@ public void stop() { public void stopWithX() { Rotation2d[] headings = new Rotation2d[4]; for (int i = 0; i < 4; i++) { - headings[i] = moduleTranslations[i].getAngle(); + headings[i] = MODULE_TRANSLATIONS[i].getAngle(); } kinematics.resetHeadings(headings); stop(); @@ -423,12 +423,12 @@ public void addVisionMeasurement( /** Returns the maximum linear speed in meters per sec. */ public double getMaxLinearSpeedMetersPerSec() { - return maxChassisVelocity.in(MetersPerSecond); + return MAX_CHASSIS_VELOCITY.in(MetersPerSecond); } /** Returns the maximum angular speed in radians per sec. */ public double getMaxAngularSpeedRadPerSec() { - return maxChassisAngularVelocity.in(RadiansPerSecond); + return MAX_CHASSIS_ANGULAR_VELOCITY.in(RadiansPerSecond); } /** Returns the total motor current draw across all modules for battery simulation. */ diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index da07b269..001fc339 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -52,85 +52,85 @@ public class DriveConstants { - public static final String zeroRotationKey = "ZeroRotation"; + public static final String ZERO_ROTATION_KEY = "ZeroRotation"; // Robot physical dimensions - public static final Distance wheelBase = Inches.of(22.5); - public static final Distance trackWidth = Inches.of(19.5); - public static final Translation2d[] moduleTranslations = + public static final Distance WHEEL_BASE = Inches.of(22.5); + public static final Distance TRACK_WIDTH = Inches.of(19.5); + public static final Translation2d[] MODULE_TRANSLATIONS = new Translation2d[] { - new Translation2d(wheelBase.div(2.0), trackWidth.div(2.0)), - new Translation2d(wheelBase.div(2.0), trackWidth.div(-2.0)), - new Translation2d(wheelBase.div(-2.0), trackWidth.div(2.0)), - new Translation2d(wheelBase.div(-2.0), trackWidth.div(-2.0)) + new Translation2d(WHEEL_BASE.div(2.0), TRACK_WIDTH.div(2.0)), + new Translation2d(WHEEL_BASE.div(2.0), TRACK_WIDTH.div(-2.0)), + new Translation2d(WHEEL_BASE.div(-2.0), TRACK_WIDTH.div(2.0)), + new Translation2d(WHEEL_BASE.div(-2.0), TRACK_WIDTH.div(-2.0)) }; - public static final Distance driveBaseRadius = - Meters.of(Translation2d.kZero.getDistance(moduleTranslations[0])); + public static final Distance DRIVE_BASE_RADIUS = + Meters.of(Translation2d.kZero.getDistance(MODULE_TRANSLATIONS[0])); // Drive motor configuration - public static final Distance wheelRadius = Inches.of(2); - public static final double wheelRadiusMeters = wheelRadius.in(Meters); - public static final double driveMotorReduction = + public static final Distance WHEEL_RADIUS = Inches.of(2); + public static final double WHEEL_RADIUS_METERS = WHEEL_RADIUS.in(Meters); + public static final double DRIVE_MOTOR_REDUCTION = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); // SDS MK4 L2 - public static final DCMotor driveGearbox = DCMotor.getKrakenX60(1); - public static final LinearVelocity drivetrainSpeedLimit = + public static final DCMotor DRIVE_GEARBOX = DCMotor.getKrakenX60(1); + public static final LinearVelocity DRIVETRAIN_SPEED_LIMIT = MetersPerSecond.of( 0.9 - * (wheelRadiusMeters * 2.0 * Math.PI) - * KrakenX60Constants.freeSpeed.in(RotationsPerSecond) - / driveMotorReduction); + * (WHEEL_RADIUS_METERS * 2.0 * Math.PI) + * KrakenX60Constants.FREE_SPEED.in(RotationsPerSecond) + / DRIVE_MOTOR_REDUCTION); // Chassis movement limits - private static final LinearVelocity driverSpeedLimit = MetersPerSecond.of(5); - public static final LinearVelocity maxChassisVelocity = + private static final LinearVelocity DRIVER_SPEED_LIMIT = MetersPerSecond.of(5); + public static final LinearVelocity MAX_CHASSIS_VELOCITY = MetersPerSecond.of( Math.min( - drivetrainSpeedLimit.in(MetersPerSecond), driverSpeedLimit.in(MetersPerSecond))); - public static final LinearAcceleration maxChassisAcceleration = + DRIVETRAIN_SPEED_LIMIT.in(MetersPerSecond), DRIVER_SPEED_LIMIT.in(MetersPerSecond))); + public static final LinearAcceleration MAX_CHASSIS_ACCELERATION = MetersPerSecondPerSecond.of(3.0); - public static final AngularVelocity maxChassisAngularVelocity = - RadiansPerSecond.of(maxChassisVelocity.in(MetersPerSecond) / driveBaseRadius.in(Meters)); - public static final AngularAcceleration maxChassisAngularAcceleration = + public static final AngularVelocity MAX_CHASSIS_ANGULAR_VELOCITY = + RadiansPerSecond.of(MAX_CHASSIS_VELOCITY.in(MetersPerSecond) / DRIVE_BASE_RADIUS.in(Meters)); + public static final AngularAcceleration MAX_CHASSIS_ANGULAR_ACCELERATION = RadiansPerSecondPerSecond.of(30); - public static final PathConstraints pathFollowingConstraints = + public static final PathConstraints PATH_FOLLOWING_CONSTRAINTS = new PathConstraints( - maxChassisVelocity.in(MetersPerSecond), - maxChassisAcceleration.in(MetersPerSecondPerSecond), - maxChassisAngularVelocity.in(RadiansPerSecond), - maxChassisAngularAcceleration.in(RadiansPerSecondPerSecond)); + MAX_CHASSIS_VELOCITY.in(MetersPerSecond), + MAX_CHASSIS_ACCELERATION.in(MetersPerSecondPerSecond), + MAX_CHASSIS_ANGULAR_VELOCITY.in(RadiansPerSecond), + MAX_CHASSIS_ANGULAR_ACCELERATION.in(RadiansPerSecondPerSecond)); // Turn motor configuration - public static final boolean turnInverted = false; - public static final double turnMotorReduction = (32.0 / 15.0) * (60.0 / 10.0); // SDS MK4 - // Every 1 rotation of the azimuth results in coupleRatio drive motor turns - private static final double coupleRatio = (50.0 / 14.0); // SDS MK4 L2 - public static final DCMotor turnGearbox = DCMotor.getKrakenX60(1); + public static final boolean TURN_INVERTED = false; + public static final double TURN_MOTOR_REDUCTION = (32.0 / 15.0) * (60.0 / 10.0); // SDS MK4 + // Every 1 rotation of the azimuth results in COUPLE_RATIO drive motor turns + private static final double COUPLE_RATIO = (50.0 / 14.0); // SDS MK4 L2 + public static final DCMotor TURN_GEARBOX = DCMotor.getKrakenX60(1); // Absolute turn encoder configuration - public static final boolean turnEncoderInverted = false; + public static final boolean TURN_ENCODER_INVERTED = false; // PathPlanner configuration - public static final Mass robotMass = Pounds.of(150); - public static final MomentOfInertia robotMoi = KilogramSquareMeters.of(6); - public static final double wheelCof = 1.2; - public static final RobotConfig ppConfig = + public static final Mass ROBOT_MASS = Pounds.of(150); + public static final MomentOfInertia ROBOT_MOI = KilogramSquareMeters.of(6); + public static final double WHEEL_COF = 1.2; + public static final RobotConfig PP_CONFIG = new RobotConfig( - robotMass.in(Kilograms), - robotMoi.in(KilogramSquareMeters), + ROBOT_MASS.in(Kilograms), + ROBOT_MOI.in(KilogramSquareMeters), new ModuleConfig( - wheelRadiusMeters, - drivetrainSpeedLimit.in(MetersPerSecond), - wheelCof, - driveGearbox.withReduction(driveMotorReduction), - KrakenX60Constants.defaultSupplyCurrentLimit, + WHEEL_RADIUS_METERS, + DRIVETRAIN_SPEED_LIMIT.in(MetersPerSecond), + WHEEL_COF, + DRIVE_GEARBOX.withReduction(DRIVE_MOTOR_REDUCTION), + KrakenX60Constants.DEFAULT_SUPPLY_CURRENT_LIMIT, 1), - moduleTranslations); + MODULE_TRANSLATIONS); // The steer motor uses any SwerveModule.SteerRequestType control request with the // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - private static final Slot0Configs steerGains = + private static final Slot0Configs STEER_GAINS = new Slot0Configs() .withKP(300) .withKI(0) @@ -141,153 +141,153 @@ public class DriveConstants { .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static final Slot0Configs driveGains = + private static final Slot0Configs DRIVE_GAINS = new Slot0Configs().withKP(10).withKI(0).withKD(0).withKS(0).withKV(0.124); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType steerClosedLoopOutput = + private static final ClosedLoopOutputType STEER_CLOSED_LOOP_OUTPUT = ClosedLoopOutputType.TorqueCurrentFOC; // The closed-loop output type to use for the drive motors; // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType driveClosedLoopOutput = + private static final ClosedLoopOutputType DRIVE_CLOSED_LOOP_OUTPUT = ClosedLoopOutputType.TorqueCurrentFOC; // The type of motor used for the drive motor - private static final DriveMotorArrangement driveMotorType = + private static final DriveMotorArrangement DRIVE_MOTOR_TYPE = DriveMotorArrangement.TalonFX_Integrated; // The type of motor used for the steer motor - private static final SteerMotorArrangement steerMotorType = + private static final SteerMotorArrangement STEER_MOTOR_TYPE = SteerMotorArrangement.TalonFX_Integrated; // The remote sensor feedback type to use for the steer motors - private static final SteerFeedbackType steerFeedbackType = SteerFeedbackType.FusedCANcoder; + private static final SteerFeedbackType STEER_FEEDBACK_TYPE = SteerFeedbackType.FusedCANcoder; // TorqueCurrent peak at which the wheels start to slip; used for slip detection in // TorqueCurrentFOC control mode. This needs to be tuned to your individual robot. - static final int slipCurrent = 120; + static final int SLIP_CURRENT = 120; // Hardware stator current limit for drive motors - static final int driveStatorCurrentLimit = KrakenX60Constants.defaultStatorCurrentLimit; + static final int DRIVE_STATOR_CURRENT_LIMIT = KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; // Stator current limit for azimuth (steer) motors; lower than drive to reduce brownout risk // since steering requires minimal torque compared to driving. - static final int steerStatorCurrentLimit = 60; + static final int STEER_STATOR_CURRENT_LIMIT = 60; - private static final TalonFXConfiguration driveInitialConfigs = + private static final TalonFXConfiguration DRIVE_INITIAL_CONFIGS = new TalonFXConfiguration() .withTorqueCurrent( new TorqueCurrentConfigs() - .withPeakForwardTorqueCurrent(slipCurrent) - .withPeakReverseTorqueCurrent(-slipCurrent)) + .withPeakForwardTorqueCurrent(SLIP_CURRENT) + .withPeakReverseTorqueCurrent(-SLIP_CURRENT)) .withCurrentLimits( new CurrentLimitsConfigs() - .withStatorCurrentLimit(driveStatorCurrentLimit) + .withStatorCurrentLimit(DRIVE_STATOR_CURRENT_LIMIT) .withStatorCurrentLimitEnable(true) - .withSupplyCurrentLimit(KrakenX60Constants.defaultSupplyCurrentLimit) + .withSupplyCurrentLimit(KrakenX60Constants.DEFAULT_SUPPLY_CURRENT_LIMIT) .withSupplyCurrentLimitEnable(true)); // Azimuth does not require much torque; keep stator limit low to reduce brownout risk // since steering requires minimal torque compared to driving. - private static final TalonFXConfiguration turnInitialConfigs = + private static final TalonFXConfiguration TURN_INITIAL_CONFIGS = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() - .withStatorCurrentLimit(steerStatorCurrentLimit) + .withStatorCurrentLimit(STEER_STATOR_CURRENT_LIMIT) .withStatorCurrentLimitEnable(true) - .withSupplyCurrentLimit(KrakenX60Constants.defaultSupplyCurrentLimit) + .withSupplyCurrentLimit(KrakenX60Constants.DEFAULT_SUPPLY_CURRENT_LIMIT) .withSupplyCurrentLimitEnable(true)); - private static final boolean invertLeftSide = false; - private static final boolean invertRightSide = false; + private static final boolean INVERT_LEFT_SIDE = false; + private static final boolean INVERT_RIGHT_SIDE = false; // These are only used for simulation - private static final MomentOfInertia steerInertia = KilogramSquareMeters.of(0.004); - private static final MomentOfInertia driveInertia = KilogramSquareMeters.of(0.025); + private static final MomentOfInertia STEER_INERTIA = KilogramSquareMeters.of(0.004); + private static final MomentOfInertia DRIVE_INERTIA = KilogramSquareMeters.of(0.025); // Simulated voltage necessary to overcome friction - private static final Voltage steerFrictionVoltage = Volts.of(0.2); - private static final Voltage driveFrictionVoltage = Volts.of(0.2); + private static final Voltage STEER_FRICTION_VOLTAGE = Volts.of(0.2); + private static final Voltage DRIVE_FRICTION_VOLTAGE = Volts.of(0.2); - public static final SwerveDrivetrainConstants drivetrainConstants = + public static final SwerveDrivetrainConstants DRIVETRAIN_CONSTANTS = new SwerveDrivetrainConstants().withCANBusName(CANHD.bus.getName()); private static final SwerveModuleConstantsFactory< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - constantCreator = + CONSTANT_CREATOR = new SwerveModuleConstantsFactory< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() - .withDriveMotorGearRatio(driveMotorReduction) - .withSteerMotorGearRatio(turnMotorReduction) - .withCouplingGearRatio(coupleRatio) - .withWheelRadius(wheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(steerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(driveClosedLoopOutput) - .withSlipCurrent(Amps.of(slipCurrent)) - .withSpeedAt12Volts(drivetrainSpeedLimit) - .withDriveMotorType(driveMotorType) - .withSteerMotorType(steerMotorType) - .withFeedbackSource(steerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(turnInitialConfigs) - .withSteerInertia(steerInertia) - .withDriveInertia(driveInertia) - .withSteerFrictionVoltage(steerFrictionVoltage) - .withDriveFrictionVoltage(driveFrictionVoltage); + .withDriveMotorGearRatio(DRIVE_MOTOR_REDUCTION) + .withSteerMotorGearRatio(TURN_MOTOR_REDUCTION) + .withCouplingGearRatio(COUPLE_RATIO) + .withWheelRadius(WHEEL_RADIUS) + .withSteerMotorGains(STEER_GAINS) + .withDriveMotorGains(DRIVE_GAINS) + .withSteerMotorClosedLoopOutput(STEER_CLOSED_LOOP_OUTPUT) + .withDriveMotorClosedLoopOutput(DRIVE_CLOSED_LOOP_OUTPUT) + .withSlipCurrent(Amps.of(SLIP_CURRENT)) + .withSpeedAt12Volts(DRIVETRAIN_SPEED_LIMIT) + .withDriveMotorType(DRIVE_MOTOR_TYPE) + .withSteerMotorType(STEER_MOTOR_TYPE) + .withFeedbackSource(STEER_FEEDBACK_TYPE) + .withDriveMotorInitialConfigs(DRIVE_INITIAL_CONFIGS) + .withSteerMotorInitialConfigs(TURN_INITIAL_CONFIGS) + .withSteerInertia(STEER_INERTIA) + .withDriveInertia(DRIVE_INERTIA) + .withSteerFrictionVoltage(STEER_FRICTION_VOLTAGE) + .withDriveFrictionVoltage(DRIVE_FRICTION_VOLTAGE); public static final SwerveModuleConstants< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - frontLeft = - constantCreator.createModuleConstants( + FRONT_LEFT = + CONSTANT_CREATOR.createModuleConstants( CANHD.frontLeftTurn, CANHD.frontLeftDrive, CANHD.frontLeftTurnAbsEncoder, Rotations.of(0), - wheelBase.div(2.0), - trackWidth.div(2.0), - invertLeftSide, - turnInverted, - turnEncoderInverted); + WHEEL_BASE.div(2.0), + TRACK_WIDTH.div(2.0), + INVERT_LEFT_SIDE, + TURN_INVERTED, + TURN_ENCODER_INVERTED); public static final SwerveModuleConstants< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - frontRight = - constantCreator.createModuleConstants( + FRONT_RIGHT = + CONSTANT_CREATOR.createModuleConstants( CANHD.frontRightTurn, CANHD.frontRightDrive, CANHD.frontRightTurnAbsEncoder, Rotations.of(0), - wheelBase.div(2.0), - trackWidth.div(-2.0), - invertRightSide, - turnInverted, - turnEncoderInverted); + WHEEL_BASE.div(2.0), + TRACK_WIDTH.div(-2.0), + INVERT_RIGHT_SIDE, + TURN_INVERTED, + TURN_ENCODER_INVERTED); public static final SwerveModuleConstants< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - backLeft = - constantCreator.createModuleConstants( + BACK_LEFT = + CONSTANT_CREATOR.createModuleConstants( CANHD.backLeftTurn, CANHD.backLeftDrive, CANHD.backLeftTurnAbsEncoder, Rotations.of(0), - wheelBase.div(-2.0), - trackWidth.div(2.0), - invertLeftSide, - turnInverted, - turnEncoderInverted); + WHEEL_BASE.div(-2.0), + TRACK_WIDTH.div(2.0), + INVERT_LEFT_SIDE, + TURN_INVERTED, + TURN_ENCODER_INVERTED); public static final SwerveModuleConstants< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - backRight = - constantCreator.createModuleConstants( + BACK_RIGHT = + CONSTANT_CREATOR.createModuleConstants( CANHD.backRightTurn, CANHD.backRightDrive, CANHD.backRightTurnAbsEncoder, Rotations.of(0), - wheelBase.div(-2.0), - trackWidth.div(-2.0), - invertRightSide, - turnInverted, - turnEncoderInverted); + WHEEL_BASE.div(-2.0), + TRACK_WIDTH.div(-2.0), + INVERT_RIGHT_SIDE, + TURN_INVERTED, + TURN_ENCODER_INVERTED); /** * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot @@ -295,7 +295,7 @@ public class DriveConstants { */ // public static CommandSwerveDrivetrain createDrivetrain() { // return new CommandSwerveDrivetrain( - // drivetrainConstants, frontLeft, frontRight, backLeft, backRight); + // DRIVETRAIN_CONSTANTS, FRONT_LEFT, FRONT_RIGHT, BACK_LEFT, BACK_RIGHT); // } /** Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. */ diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index 8749734e..e1b24426 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -43,27 +43,27 @@ public Module(ModuleIO io, String name) { // Set turn zero from preferences Rotation2d turnZeroFromCancoder = inputs.turnZero; - Preferences.initDouble(zeroRotationKey + name, turnZeroFromCancoder.getRadians()); + Preferences.initDouble(ZERO_ROTATION_KEY + name, turnZeroFromCancoder.getRadians()); Rotation2d turnZeroFromPreferences = new Rotation2d( - Preferences.getDouble(zeroRotationKey + name, turnZeroFromCancoder.getRadians())); + Preferences.getDouble(ZERO_ROTATION_KEY + name, turnZeroFromCancoder.getRadians())); io.setTurnZero(turnZeroFromPreferences); Logger.recordOutput( "Drive/Module" + name + "/TurnZeroRad", turnZeroFromPreferences.getRadians()); } public void periodic() { - long t0 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t0 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; io.updateInputs(inputs); - long t1 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t1 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; Logger.processInputs("Drive/Module" + name, inputs); - long t2 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t2 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Calculate positions for odometry int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together odometryPositions = new SwerveModulePosition[sampleCount]; for (int i = 0; i < sampleCount; i++) { - double positionMeters = inputs.odometryDrivePositionsRad[i] * wheelRadiusMeters; + double positionMeters = inputs.odometryDrivePositionsRad[i] * WHEEL_RADIUS_METERS; Rotation2d angle = inputs.odometryTurnPositions[i]; odometryPositions[i] = new SwerveModulePosition(positionMeters, angle); } @@ -73,10 +73,10 @@ public void periodic() { turnDisconnectedAlert.set(!inputs.turnConnected); Logger.recordOutput("Faults/Module" + name + "/DriveDisconnected", !inputs.driveConnected); Logger.recordOutput("Faults/Module" + name + "/TurnDisconnected", !inputs.turnConnected); - long t3 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t3 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Profiling output - if (FeatureFlags.profilingEnabled) { + if (FeatureFlags.PROFILING_ENABLED) { long totalMs = (t3 - t0) / 1_000_000; if (totalMs > 2) { System.out.println( @@ -100,7 +100,7 @@ public void runSetpoint(SwerveModuleState state) { state.cosineScale(inputs.turnPosition); // Apply setpoints - io.setDriveVelocity(state.speedMetersPerSecond / wheelRadiusMeters); + io.setDriveVelocity(state.speedMetersPerSecond / WHEEL_RADIUS_METERS); io.setTurnPosition(state.angle); } @@ -123,12 +123,12 @@ public Rotation2d getAngle() { /** Returns the current drive position of the module in meters. */ public double getPositionMeters() { - return inputs.drivePositionRad * wheelRadiusMeters; + return inputs.drivePositionRad * WHEEL_RADIUS_METERS; } /** Returns the current drive velocity of the module in meters per second. */ public double getVelocityMetersPerSec() { - return inputs.driveVelocityRadPerSec * wheelRadiusMeters; + return inputs.driveVelocityRadPerSec * WHEEL_RADIUS_METERS; } /** Returns the module position (turn angle and drive position). */ @@ -170,7 +170,7 @@ public double getSimCurrentDrawAmps() { public void setTurnZero() { Rotation2d newTurnZero = inputs.turnZero.minus(inputs.turnPosition); io.setTurnZero(newTurnZero); - Preferences.setDouble(zeroRotationKey + name, newTurnZero.getRadians()); + Preferences.setDouble(ZERO_ROTATION_KEY + name, newTurnZero.getRadians()); Logger.recordOutput("Drive/Module" + name + "/TurnZeroRad", newTurnZero.getRadians()); } } diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 9664fe3f..3937e812 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -39,7 +39,7 @@ public class PhoenixOdometryThread extends Thread { private final List> genericQueues = new ArrayList<>(); private final List> timestampQueues = new ArrayList<>(); - private static boolean isCANFD = new CANBus(drivetrainConstants.CANBusName).isNetworkFD(); + private static boolean isCANFD = new CANBus(DRIVETRAIN_CONSTANTS.CANBusName).isNetworkFD(); private static PhoenixOdometryThread instance = null; public static PhoenixOdometryThread getInstance() { diff --git a/src/main/java/frc/robot/subsystems/feeder/Feeder.java b/src/main/java/frc/robot/subsystems/feeder/Feeder.java index c66d1e75..25c0199e 100644 --- a/src/main/java/frc/robot/subsystems/feeder/Feeder.java +++ b/src/main/java/frc/robot/subsystems/feeder/Feeder.java @@ -31,16 +31,16 @@ public Feeder(SpindexerIO spindexerIO, KickerIO kickerIO) { @Override public void periodic() { - long t0 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t0 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; spindexerIO.updateInputs(spindexerInputs); - long t1 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t1 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; kickerIO.updateInputs(kickerInputs); - long t2 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t2 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; Logger.processInputs("Spindexer", spindexerInputs); - long t3 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t3 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; Logger.processInputs("Kicker", kickerInputs); - long t4 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t4 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; spindexerDisconnectedAlert.set(!spindexerInputs.connected); kickerDisconnectedAlert.set(!kickerInputs.connected); @@ -48,7 +48,7 @@ public void periodic() { Logger.recordOutput("Faults/Feeder/KickerDisconnected", !kickerInputs.connected); // Profiling output - if (Constants.FeatureFlags.profilingEnabled) { + if (Constants.FeatureFlags.PROFILING_ENABLED) { long totalMs = (t4 - t0) / 1_000_000; if (totalMs > 2) { System.out.println( diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java index f2e73466..728be50b 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java @@ -15,16 +15,17 @@ public static final class SpindexerConstants { public static final Distance radius = Inches.of(3.0); // Motor - public static final double motorReduction = 1.0; - public static final LinearVelocity maxTangentialVelocity = + public static final double MOTOR_REDUCTION = 1.0; + public static final LinearVelocity MAX_TANGENTIAL_VELOCITY = MetersPerSecond.of( - NEOVortexConstants.freeSpeed.in(RadiansPerSecond) + NEOVortexConstants.FREE_SPEED.in(RadiansPerSecond) * radius.in(Meters) - / motorReduction); + / MOTOR_REDUCTION); // Encoder - public static final double encoderPositionFactor = 2.0 * Math.PI / motorReduction; // Meters - public static final double encoderVelocityFactor = encoderPositionFactor / 60.0; // Meters/sec + public static final double ENCODER_POSITION_FACTOR = 2.0 * Math.PI / MOTOR_REDUCTION; // Meters + public static final double ENCODER_VELOCITY_FACTOR = + ENCODER_POSITION_FACTOR / 60.0; // Meters/sec // Simulation public static final double kP = 0.005; @@ -36,16 +37,17 @@ public static final class KickerConstants { public static final Distance radius = Inches.of(1.5); // Motor - public static final double motorReduction = 5.0; - public static final LinearVelocity maxTangentialVelocity = + public static final double MOTOR_REDUCTION = 5.0; + public static final LinearVelocity MAX_TANGENTIAL_VELOCITY = MetersPerSecond.of( - NEOVortexConstants.freeSpeed.in(RadiansPerSecond) + NEOVortexConstants.FREE_SPEED.in(RadiansPerSecond) * radius.in(Meters) - / motorReduction); + / MOTOR_REDUCTION); // Encoder - public static final double encoderPositionFactor = 2.0 * Math.PI / motorReduction; // Meters - public static final double encoderVelocityFactor = encoderPositionFactor / 60.0; // Meters/sec + public static final double ENCODER_POSITION_FACTOR = 2.0 * Math.PI / MOTOR_REDUCTION; // Meters + public static final double ENCODER_VELOCITY_FACTOR = + ENCODER_POSITION_FACTOR / 60.0; // Meters/sec // Simulation public static final double kP = 0.0025; diff --git a/src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java b/src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java index 9778fdc4..e9434eb3 100644 --- a/src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java @@ -43,13 +43,13 @@ public KickerIOSimSpark() { config .inverted(false) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.defaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.nominalVoltage); + .smartCurrentLimit(NEOVortexConstants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); config .encoder - .positionConversionFactor(encoderPositionFactor) - .velocityConversionFactor(encoderVelocityFactor); + .positionConversionFactor(ENCODER_POSITION_FACTOR) + .velocityConversionFactor(ENCODER_VELOCITY_FACTOR); config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kP, 0.0, kD); @@ -58,7 +58,8 @@ public KickerIOSimSpark() { kickerSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(GEARBOX, KICKER_MOI_KG_M2, motorReduction), GEARBOX); + LinearSystemId.createDCMotorSystem(GEARBOX, KICKER_MOI_KG_M2, MOTOR_REDUCTION), + GEARBOX); } @Override @@ -78,15 +79,15 @@ public void updateInputs(KickerIOInputs inputs) { @Override public void setOpenLoop(Voltage volts) { - flexSim.setAppliedOutput(volts.in(Volts) / RobotConstants.nominalVoltage); + flexSim.setAppliedOutput(volts.in(Volts) / RobotConstants.NOMINAL_VOLTAGE); } @Override public void setVelocity(LinearVelocity tangentialVelocity) { double feedforwardVolts = - RobotConstants.nominalVoltage + RobotConstants.NOMINAL_VOLTAGE * tangentialVelocity.in(MetersPerSecond) - / maxTangentialVelocity.in(MetersPerSecond); + / MAX_TANGENTIAL_VELOCITY.in(MetersPerSecond); controller.setSetpoint( tangentialVelocity.in(MetersPerSecond) / radius.in(Meters), ControlType.kVelocity, diff --git a/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java b/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java index 01db3412..f88b7074 100644 --- a/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java +++ b/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java @@ -40,13 +40,13 @@ public KickerIOSpark() { config .inverted(false) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.defaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.nominalVoltage); + .smartCurrentLimit(NEOVortexConstants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); config .encoder - .positionConversionFactor(encoderPositionFactor) - .velocityConversionFactor(encoderVelocityFactor) + .positionConversionFactor(ENCODER_POSITION_FACTOR) + .velocityConversionFactor(ENCODER_VELOCITY_FACTOR) .uvwAverageDepth(2) .uvwMeasurementPeriod(8); @@ -79,9 +79,9 @@ public void setOpenLoop(Voltage volts) { @Override public void setVelocity(LinearVelocity tangentialVelocity) { double feedforwardVolts = - RobotConstants.nominalVoltage + RobotConstants.NOMINAL_VOLTAGE * tangentialVelocity.in(MetersPerSecond) - / maxTangentialVelocity.in(MetersPerSecond); + / MAX_TANGENTIAL_VELOCITY.in(MetersPerSecond); controller.setSetpoint( tangentialVelocity.in(MetersPerSecond) / radius.in(Meters), ControlType.kVelocity, diff --git a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java index 65159fcc..bb3ef445 100644 --- a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java @@ -43,13 +43,13 @@ public SpindexerIOSimSpark() { config .inverted(false) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.defaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.nominalVoltage); + .smartCurrentLimit(NEOVortexConstants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); config .encoder - .positionConversionFactor(encoderPositionFactor) - .velocityConversionFactor(encoderVelocityFactor); + .positionConversionFactor(ENCODER_POSITION_FACTOR) + .velocityConversionFactor(ENCODER_VELOCITY_FACTOR); config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kP, 0.0, kD); @@ -58,7 +58,7 @@ public SpindexerIOSimSpark() { spindexerSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(GEARBOX, SPINDEXER_MOI_KG_M2, motorReduction), + LinearSystemId.createDCMotorSystem(GEARBOX, SPINDEXER_MOI_KG_M2, MOTOR_REDUCTION), GEARBOX); } @@ -80,15 +80,15 @@ public void updateInputs(SpindexerIOInputs inputs) { @Override public void setOpenLoop(Voltage volts) { - flexSim.setAppliedOutput(volts.in(Volts) / RobotConstants.nominalVoltage); + flexSim.setAppliedOutput(volts.in(Volts) / RobotConstants.NOMINAL_VOLTAGE); } @Override public void setVelocity(LinearVelocity tangentialVelocity) { double feedforwardVolts = - RobotConstants.nominalVoltage + RobotConstants.NOMINAL_VOLTAGE * tangentialVelocity.in(MetersPerSecond) - / maxTangentialVelocity.in(MetersPerSecond); + / MAX_TANGENTIAL_VELOCITY.in(MetersPerSecond); controller.setSetpoint( tangentialVelocity.in(MetersPerSecond) / radius.in(Meters), ControlType.kVelocity, diff --git a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java index fffae89a..1a166059 100644 --- a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java +++ b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java @@ -40,13 +40,13 @@ public SpindexerIOSpark() { config .inverted(false) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.defaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.nominalVoltage); + .smartCurrentLimit(NEOVortexConstants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); config .encoder - .positionConversionFactor(encoderPositionFactor) - .velocityConversionFactor(encoderVelocityFactor) + .positionConversionFactor(ENCODER_POSITION_FACTOR) + .velocityConversionFactor(ENCODER_VELOCITY_FACTOR) .uvwAverageDepth(2) .uvwMeasurementPeriod(8); @@ -79,9 +79,9 @@ public void setOpenLoop(Voltage volts) { @Override public void setVelocity(LinearVelocity tangentialVelocity) { double feedforwardVolts = - RobotConstants.nominalVoltage + RobotConstants.NOMINAL_VOLTAGE * tangentialVelocity.in(MetersPerSecond) - / maxTangentialVelocity.in(MetersPerSecond); + / MAX_TANGENTIAL_VELOCITY.in(MetersPerSecond); controller.setSetpoint( tangentialVelocity.in(MetersPerSecond) / radius.in(Meters), ControlType.kVelocity, diff --git a/src/main/java/frc/robot/subsystems/hopper/Hopper.java b/src/main/java/frc/robot/subsystems/hopper/Hopper.java index 331976ac..09eafec1 100644 --- a/src/main/java/frc/robot/subsystems/hopper/Hopper.java +++ b/src/main/java/frc/robot/subsystems/hopper/Hopper.java @@ -24,14 +24,14 @@ public Hopper(HopperIO hopperIO) { @Override public void periodic() { - long t0 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t0 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; hopperIO.updateInputs(hopperInputs); - long t1 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t1 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; Logger.processInputs("Hopper", hopperInputs); - long t2 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t2 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Profiling output - if (Constants.FeatureFlags.profilingEnabled) { + if (Constants.FeatureFlags.PROFILING_ENABLED) { long totalMs = (t2 - t0) / 1_000_000; if (totalMs > 2) { System.out.println( diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index fdabb831..e0393bee 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -42,16 +42,16 @@ public Intake(RollerIO upperRollerIO, RollerIO lowerRollerIO, IntakeArmIO intake @Override public void periodic() { - long t0 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t0 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; upperRollerIO.updateInputs(upperRollerInputs); lowerRollerIO.updateInputs(lowerRollerInputs); intakeArmIO.updateInputs(intakeArmInputs); - long t1 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t1 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; Logger.processInputs("UpperRoller", upperRollerInputs); Logger.processInputs("LowerRoller", lowerRollerInputs); Logger.processInputs("IntakeArm", intakeArmInputs); - long t2 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t2 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; upperRollerDisconnectedAlert.set(!upperRollerInputs.connected); lowerRollerDisconnectedAlert.set(!lowerRollerInputs.connected); @@ -59,7 +59,7 @@ public void periodic() { Logger.recordOutput("Faults/Intake/LowerRollerDisconnected", !lowerRollerInputs.connected); // Profiling output - if (Constants.FeatureFlags.profilingEnabled) { + if (Constants.FeatureFlags.PROFILING_ENABLED) { long totalMs = (t2 - t0) / 1_000_000; if (totalMs > 2) { System.out.println( diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 8b89b3ed..c707bbbc 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -14,14 +14,15 @@ public static class RollerConstants { public static final Distance rollerRadius = Inches.of(0.85); // motor controller - public static final double motorReduction = 1.0; + public static final double MOTOR_REDUCTION = 1.0; public static final int numMotors = 1; - public static final double maxAcceleration = 4000.0; - public static final double maxJerk = 40000.0; + public static final double MAX_ACCELERATION = 4000.0; + public static final double MAX_JERK = 40000.0; // roller constants - public static final double encoderPositionFactor = 2.0 * Math.PI / motorReduction; // Meters - public static final double encoderVelocityFactor = encoderPositionFactor / 60.0; // Meters/sec + public static final double ENCODER_POSITION_FACTOR = 2.0 * Math.PI / MOTOR_REDUCTION; // Meters + public static final double ENCODER_VELOCITY_FACTOR = + ENCODER_POSITION_FACTOR / 60.0; // Meters/sec // configs public static final RollerConfig upperRollerConfig = diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java index 60b52bc2..d148055e 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java @@ -31,9 +31,9 @@ public class RollerIOSimSpark implements RollerIO { private static final double KD = 0.0; private static final LinearVelocity MAX_TANGENTIAL_VELOCITY = MetersPerSecond.of( - NEOVortexConstants.freeSpeed.in(RadiansPerSecond) + NEOVortexConstants.FREE_SPEED.in(RadiansPerSecond) * rollerRadius.in(Meters) - / motorReduction); + / MOTOR_REDUCTION); private static final DCMotor GEARBOX = DCMotor.getNeoVortex(numMotors); private final DCMotorSim rollerSim; @@ -50,13 +50,13 @@ public RollerIOSimSpark(RollerConfig rollerConfig) { config .inverted(rollerConfig.inverted) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.defaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.nominalVoltage); + .smartCurrentLimit(NEOVortexConstants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); config .encoder - .positionConversionFactor(encoderPositionFactor) - .velocityConversionFactor(encoderVelocityFactor); + .positionConversionFactor(ENCODER_POSITION_FACTOR) + .velocityConversionFactor(ENCODER_VELOCITY_FACTOR); config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(KP, 0.0, KD); @@ -65,7 +65,8 @@ public RollerIOSimSpark(RollerConfig rollerConfig) { rollerSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(GEARBOX, KICKER_MOI_KG_M2, motorReduction), GEARBOX); + LinearSystemId.createDCMotorSystem(GEARBOX, KICKER_MOI_KG_M2, MOTOR_REDUCTION), + GEARBOX); } @Override @@ -85,13 +86,13 @@ public void updateInputs(RollerIOInputs inputs) { @Override public void setOpenLoop(Voltage volts) { - flexSim.setAppliedOutput(volts.in(Volts) / RobotConstants.nominalVoltage); + flexSim.setAppliedOutput(volts.in(Volts) / RobotConstants.NOMINAL_VOLTAGE); } @Override public void setVelocity(LinearVelocity tangentialVelocity) { double feedforwardVolts = - RobotConstants.nominalVoltage + RobotConstants.NOMINAL_VOLTAGE * tangentialVelocity.in(MetersPerSecond) / MAX_TANGENTIAL_VELOCITY.in(MetersPerSecond); controller.setSetpoint( diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java index 0d917db9..af2b2a89 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java @@ -69,7 +69,7 @@ public RollerIOSimTalonFX(RollerConfig rollerConfig) { rollerSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(GEARBOX, 0.0005, motorReduction), GEARBOX); + LinearSystemId.createDCMotorSystem(GEARBOX, 0.0005, MOTOR_REDUCTION), GEARBOX); velocity = motor.getVelocity(); acceleration = motor.getAcceleration(); @@ -95,13 +95,13 @@ public void updateInputs(RollerIOInputs inputs) { motorSim.setSupplyVoltage(RoboRioSim.getVInVoltage()); rollerSim.setInput(motorSim.getMotorVoltage()); rollerSim.update(Robot.defaultPeriodSecs); - motorSim.setRawRotorPosition(rollerSim.getAngularPositionRotations() * motorReduction); - motorSim.setRotorVelocity(rollerSim.getAngularVelocity().times(motorReduction)); + motorSim.setRawRotorPosition(rollerSim.getAngularPositionRotations() * MOTOR_REDUCTION); + motorSim.setRotorVelocity(rollerSim.getAngularVelocity().times(MOTOR_REDUCTION)); inputs.appliedVolts = appliedVolts.getValueAsDouble(); inputs.currentAmps = supplyCurrent.getValueAsDouble(); inputs.velocityMetersPerSec = - velocity.getValue().in(RadiansPerSecond) * rollerRadius.in(Meters) / motorReduction; + velocity.getValue().in(RadiansPerSecond) * rollerRadius.in(Meters) / MOTOR_REDUCTION; } @Override @@ -117,7 +117,7 @@ public void setOpenLoop(Voltage volts) { public void setVelocity(LinearVelocity tangentialVelocity) { AngularVelocity angularVelocity = RadiansPerSecond.of( - tangentialVelocity.in(MetersPerSecond) * motorReduction / rollerRadius.in(Meters)); + tangentialVelocity.in(MetersPerSecond) * MOTOR_REDUCTION / rollerRadius.in(Meters)); motor.setControl(velocityTorqueCurrentRequest.withVelocity(angularVelocity)); } } diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java index 257b3093..bda11365 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java @@ -28,9 +28,9 @@ public class RollerIOSpark implements RollerIO { private static final double KD = 0.0; private static final LinearVelocity MAX_TANGENTIAL_VELOCITY = MetersPerSecond.of( - NEOVortexConstants.freeSpeed.in(RadiansPerSecond) + NEOVortexConstants.FREE_SPEED.in(RadiansPerSecond) * rollerRadius.in(Meters) - / motorReduction); + / MOTOR_REDUCTION); private final SparkFlex flex; private final RelativeEncoder encoder; @@ -46,13 +46,13 @@ public RollerIOSpark(RollerConfig rollerConfig) { config .inverted(rollerConfig.inverted) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.defaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.nominalVoltage); + .smartCurrentLimit(NEOVortexConstants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); config .encoder - .positionConversionFactor(encoderPositionFactor) - .velocityConversionFactor(encoderVelocityFactor) + .positionConversionFactor(ENCODER_POSITION_FACTOR) + .velocityConversionFactor(ENCODER_VELOCITY_FACTOR) .uvwAverageDepth(2) .uvwMeasurementPeriod(8); @@ -84,7 +84,7 @@ public void setOpenLoop(Voltage volts) { @Override public void setVelocity(LinearVelocity tangentialVelocity) { double feedforwardVolts = - RobotConstants.nominalVoltage + RobotConstants.NOMINAL_VOLTAGE * tangentialVelocity.in(MetersPerSecond) / MAX_TANGENTIAL_VELOCITY.in(MetersPerSecond); controller.setSetpoint( diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java index cd3484cf..328804cc 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java @@ -42,7 +42,7 @@ public class RollerIOTalonFX implements RollerIO { private final NeutralOut brake = new NeutralOut(); // private final TrapezoidProfile profile = - // new TrapezoidProfile(new TrapezoidProfile.Constraints(maxAcceleration, maxJerk)); + // new TrapezoidProfile(new TrapezoidProfile.Constraints(MAX_ACCELERATION, MAX_JERK)); // Inputs from intake motor private final StatusSignal velocity; private final StatusSignal appliedVolts; @@ -51,12 +51,12 @@ public class RollerIOTalonFX implements RollerIO { public RollerIOTalonFX(RollerConfig rollerConfig) { motor = new TalonFX(rollerConfig.port, rollerConfig.bus); config = new TalonFXConfiguration(); - config.TorqueCurrent.PeakForwardTorqueCurrent = KrakenX60Constants.defaultStatorCurrentLimit; + config.TorqueCurrent.PeakForwardTorqueCurrent = KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; config.TorqueCurrent.PeakReverseTorqueCurrent = - -KrakenX60Constants.defaultStatorCurrentLimit; - config.CurrentLimits.StatorCurrentLimit = KrakenX60Constants.defaultStatorCurrentLimit; + -KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; + config.CurrentLimits.StatorCurrentLimit = KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; config.CurrentLimits.StatorCurrentLimitEnable = true; - config.CurrentLimits.SupplyCurrentLimit = KrakenX60Constants.defaultSupplyCurrentLimit; + config.CurrentLimits.SupplyCurrentLimit = KrakenX60Constants.DEFAULT_SUPPLY_CURRENT_LIMIT; config.CurrentLimits.SupplyCurrentLimitEnable = true; config.MotorOutput.Inverted = rollerConfig.inverted @@ -86,7 +86,7 @@ public void updateInputs(RollerIOInputs inputs) { inputs.appliedVolts = appliedVolts.getValueAsDouble(); inputs.currentAmps = supplyCurrent.getValueAsDouble(); inputs.velocityMetersPerSec = - velocity.getValue().in(RadiansPerSecond) * rollerRadius.in(Meters) / motorReduction; + velocity.getValue().in(RadiansPerSecond) * rollerRadius.in(Meters) / MOTOR_REDUCTION; } @Override @@ -102,7 +102,7 @@ public void setOpenLoop(Voltage volts) { public void setVelocity(LinearVelocity tangentialVelocity) { AngularVelocity angularVelocity = RadiansPerSecond.of( - tangentialVelocity.in(MetersPerSecond) * motorReduction / rollerRadius.in(Meters)); + tangentialVelocity.in(MetersPerSecond) * MOTOR_REDUCTION / rollerRadius.in(Meters)); // TrapezoidProfile.State goal = // new TrapezoidProfile.State(angularVelocity.in(RotationsPerSecond), 0); diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java index e3b38c58..5cceafd6 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java @@ -56,8 +56,8 @@ public FlywheelIOSimTalonFX() { config = new TalonFXConfiguration(); config.MotorOutput.withInverted(InvertedValue.CounterClockwise_Positive) .withNeutralMode(NeutralModeValue.Brake); - config.Slot0 = velocityVoltageGains; - config.Slot1 = velocityTorqueCurrentGains; + config.Slot0 = VELOCITY_VOLTAGE_GAINS; + config.Slot1 = VELOCITY_TORQUE_CURRENT_GAINS; tryUntilOk(5, () -> flywheelLeaderTalon.getConfigurator().apply(config, 0.25)); tryUntilOk(5, () -> flywheelFollowerTalon.getConfigurator().apply(config, 0.25)); @@ -67,8 +67,8 @@ public FlywheelIOSimTalonFX() { flywheelSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(gearbox, FLYWHEEL_MOI_KG_M2, motorReduction), - gearbox); + LinearSystemId.createDCMotorSystem(GEARBOX, FLYWHEEL_MOI_KG_M2, MOTOR_REDUCTION), + GEARBOX); flywheelVelocity = flywheelLeaderTalon.getVelocity(); flywheelAppliedVolts = flywheelLeaderTalon.getMotorVoltage(); @@ -95,14 +95,14 @@ public void updateInputs(FlywheelIOInputs inputs) { flywheelSim.setInput(flywheelMotorSim.getMotorVoltage()); flywheelSim.update(Robot.defaultPeriodSecs); flywheelMotorSim.setRawRotorPosition( - flywheelSim.getAngularPositionRotations() * motorReduction); - flywheelMotorSim.setRotorVelocity(flywheelSim.getAngularVelocity().times(motorReduction)); + flywheelSim.getAngularPositionRotations() * MOTOR_REDUCTION); + flywheelMotorSim.setRotorVelocity(flywheelSim.getAngularVelocity().times(MOTOR_REDUCTION)); inputs.appliedVolts = flywheelAppliedVolts.getValueAsDouble(); inputs.currentAmps = flywheelCurrent.getValueAsDouble(); inputs.velocityMetersPerSec = - (flywheelVelocity.getValue().in(RadiansPerSecond) * wheelRadius.in(Meters)) - / motorReduction; + (flywheelVelocity.getValue().in(RadiansPerSecond) * WHEEL_RADIUS.in(Meters)) + / MOTOR_REDUCTION; } @Override @@ -114,7 +114,7 @@ public void setOpenLoop(Voltage volts) { public void setVelocity(LinearVelocity tangentialVelocity) { var angularVelocity = RadiansPerSecond.of( - tangentialVelocity.in(MetersPerSecond) * motorReduction / wheelRadius.in(Meters)); + tangentialVelocity.in(MetersPerSecond) * MOTOR_REDUCTION / WHEEL_RADIUS.in(Meters)); flywheelLeaderTalon.setControl(velocityTorqueCurrentRequest.withVelocity(angularVelocity)); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimWPI.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimWPI.java index 1d811bd3..f5fcd129 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimWPI.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimWPI.java @@ -24,7 +24,8 @@ public class FlywheelIOSimWPI implements FlywheelIO { public FlywheelIOSimWPI() { flywheelSim = - new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox); + new DCMotorSim( + LinearSystemId.createDCMotorSystem(GEARBOX, 0.004, MOTOR_REDUCTION), GEARBOX); } @Override @@ -41,13 +42,13 @@ public void updateInputs(FlywheelIOInputs inputs) { // Update simulation state flywheelSim.setInputVoltage( MathUtil.clamp( - appliedVolts, -RobotConstants.nominalVoltage, RobotConstants.nominalVoltage)); + appliedVolts, -RobotConstants.NOMINAL_VOLTAGE, RobotConstants.NOMINAL_VOLTAGE)); flywheelSim.update(Robot.defaultPeriodSecs); // Update turn inputs inputs.connected = true; inputs.velocityMetersPerSec = - flywheelSim.getAngularVelocityRadPerSec() * wheelRadius.in(Meters); + flywheelSim.getAngularVelocityRadPerSec() * WHEEL_RADIUS.in(Meters); inputs.appliedVolts = appliedVolts; inputs.currentAmps = Math.abs(flywheelSim.getCurrentDrawAmps()); } @@ -62,9 +63,9 @@ public void setOpenLoop(Voltage volts) { public void setVelocity(LinearVelocity tangentialVelocity) { closedLoop = true; this.feedforwardVolts = - RobotConstants.nominalVoltage + RobotConstants.NOMINAL_VOLTAGE * tangentialVelocity.in(MetersPerSecond) - / maxAngularVelocity.in(RadiansPerSecond); + / MAX_ANGULAR_VELOCITY.in(RadiansPerSecond); velocityController.setSetpoint(tangentialVelocity.in(MetersPerSecond)); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java index a366e1a8..ebeb3742 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java @@ -39,7 +39,7 @@ public class FlywheelIOTalonFX implements FlywheelIO { private final NeutralOut brake = new NeutralOut(); private final TrapezoidProfile profile = - new TrapezoidProfile(new TrapezoidProfile.Constraints(maxAcceleration, maxJerk)); + new TrapezoidProfile(new TrapezoidProfile.Constraints(MAX_ACCELERATION, MAX_JERK)); // Inputs from flywheel motor private final StatusSignal flywheelVelocity; @@ -54,14 +54,14 @@ public FlywheelIOTalonFX() { config = new TalonFXConfiguration(); config.MotorOutput.withNeutralMode(NeutralModeValue.Brake) .withNeutralMode(NeutralModeValue.Brake); - config.Slot0 = velocityVoltageGains; - config.Slot1 = velocityTorqueCurrentGains; - config.TorqueCurrent.PeakForwardTorqueCurrent = KrakenX60Constants.defaultStatorCurrentLimit; + config.Slot0 = VELOCITY_VOLTAGE_GAINS; + config.Slot1 = VELOCITY_TORQUE_CURRENT_GAINS; + config.TorqueCurrent.PeakForwardTorqueCurrent = KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; config.TorqueCurrent.PeakReverseTorqueCurrent = - -KrakenX60Constants.defaultStatorCurrentLimit; - config.CurrentLimits.StatorCurrentLimit = KrakenX60Constants.defaultStatorCurrentLimit; + -KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; + config.CurrentLimits.StatorCurrentLimit = KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; config.CurrentLimits.StatorCurrentLimitEnable = true; - config.CurrentLimits.SupplyCurrentLimit = KrakenX60Constants.defaultSupplyCurrentLimit; + config.CurrentLimits.SupplyCurrentLimit = KrakenX60Constants.DEFAULT_SUPPLY_CURRENT_LIMIT; config.CurrentLimits.SupplyCurrentLimitEnable = true; tryUntilOk(5, () -> flywheelLeaderTalon.getConfigurator().apply(config, 0.25)); tryUntilOk(5, () -> flywheelFollowerTalon.getConfigurator().apply(config, 0.25)); @@ -107,8 +107,8 @@ public void updateInputs(FlywheelIOInputs inputs) { inputs.appliedVolts = flywheelAppliedVolts.getValueAsDouble(); inputs.currentAmps = flywheelCurrent.getValueAsDouble(); inputs.velocityMetersPerSec = - (flywheelVelocity.getValue().in(RadiansPerSecond) * wheelRadius.in(Meters)) - / motorReduction; + (flywheelVelocity.getValue().in(RadiansPerSecond) * WHEEL_RADIUS.in(Meters)) + / MOTOR_REDUCTION; // Populate follower telemetry via inputs struct (AdvantageKit best practice: // IO layers should be pure - only populate inputs, logging happens via @AutoLog) @@ -129,7 +129,7 @@ public void setOpenLoop(Voltage volts) { public void setVelocity(LinearVelocity tangentialVelocity) { AngularVelocity angularVelocity = RadiansPerSecond.of( - tangentialVelocity.in(MetersPerSecond) * motorReduction / wheelRadius.in(Meters)); + tangentialVelocity.in(MetersPerSecond) * MOTOR_REDUCTION / WHEEL_RADIUS.in(Meters)); TrapezoidProfile.State goal = new TrapezoidProfile.State(angularVelocity.in(RotationsPerSecond), 0); diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSimSpark.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSimSpark.java index 7465f4e1..31dbcaec 100644 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIOSimSpark.java @@ -45,13 +45,13 @@ public HoodIOSimSpark() { hoodConfig .inverted(false) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEO550Constants.defaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.nominalVoltage); + .smartCurrentLimit(NEO550Constants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); hoodConfig .encoder - .positionConversionFactor(encoderPositionFactor) - .velocityConversionFactor(encoderVelocityFactor); + .positionConversionFactor(ENCODER_POSITION_FACTOR) + .velocityConversionFactor(ENCODER_VELOCITY_FACTOR); hoodConfig .closedLoop @@ -61,9 +61,9 @@ public HoodIOSimSpark() { hoodConfig .softLimit - .forwardSoftLimit(maxPosRad) + .forwardSoftLimit(MAX_POS_RAD) .forwardSoftLimitEnabled(true) - .reverseSoftLimit(minPosRad) + .reverseSoftLimit(MIN_POS_RAD) .reverseSoftLimitEnabled(true); hoodConfig @@ -77,13 +77,14 @@ public HoodIOSimSpark() { .outputCurrentPeriodMs(20); max.configure(hoodConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - maxSim = new SparkMaxSim(max, gearbox); + maxSim = new SparkMaxSim(max, GEARBOX); hoodSim = - new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox); + new DCMotorSim( + LinearSystemId.createDCMotorSystem(GEARBOX, 0.004, MOTOR_REDUCTION), GEARBOX); - hoodSim.setState(minPosRad, 0); - maxSim.setPosition(minPosRad); + hoodSim.setState(MIN_POS_RAD, 0); + maxSim.setPosition(MIN_POS_RAD); } @Override @@ -93,9 +94,9 @@ public void updateInputs(HoodIOInputs inputs) { hoodSim.setInput(maxSim.getAppliedOutput() * busVoltage); hoodSim.update(Robot.defaultPeriodSecs); - if (maxSim.getPosition() > maxPosRad) { - hoodSim.setState(maxPosRad, 0); - maxSim.setPosition(maxPosRad); + if (maxSim.getPosition() > MAX_POS_RAD) { + hoodSim.setState(MAX_POS_RAD, 0); + maxSim.setPosition(MAX_POS_RAD); } maxSim.iterate(hoodSim.getAngularVelocityRadPerSec(), busVoltage, Robot.defaultPeriodSecs); @@ -110,16 +111,16 @@ public void updateInputs(HoodIOInputs inputs) { @Override public void setOpenLoop(Voltage volts) { - maxSim.setAppliedOutput(volts.in(Volts) / RobotConstants.nominalVoltage); + maxSim.setAppliedOutput(volts.in(Volts) / RobotConstants.NOMINAL_VOLTAGE); } @Override public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { - double setpoint = MathUtil.clamp(rotation.getRadians(), minPosRad, maxPosRad); + double setpoint = MathUtil.clamp(rotation.getRadians(), MIN_POS_RAD, MAX_POS_RAD); double feedforwardVolts = - RobotConstants.nominalVoltage + RobotConstants.NOMINAL_VOLTAGE * angularVelocity.in(RadiansPerSecond) - / maxAngularVelocity.in(RadiansPerSecond); + / MAX_ANGULAR_VELOCITY.in(RadiansPerSecond); controller.setSetpoint( setpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforwardVolts); } @@ -128,9 +129,9 @@ public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { public void setVelocity(AngularVelocity angularVelocity) { double setpoint = angularVelocity.in(RadiansPerSecond); double feedforwardVolts = - RobotConstants.nominalVoltage + RobotConstants.NOMINAL_VOLTAGE * angularVelocity.in(RadiansPerSecond) - / maxAngularVelocity.in(RadiansPerSecond); + / MAX_ANGULAR_VELOCITY.in(RadiansPerSecond); controller.setSetpoint( setpoint, ControlType.kVelocity, ClosedLoopSlot.kSlot0, feedforwardVolts); } @@ -143,6 +144,6 @@ public void configureSoftLimits(boolean enable) { @Override public void resetEncoder() { - maxSim.setPosition(maxPosRad); + maxSim.setPosition(MAX_POS_RAD); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSimWPI.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSimWPI.java index e9d91531..2ca6c02e 100644 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIOSimWPI.java +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIOSimWPI.java @@ -24,7 +24,8 @@ public class HoodIOSimWPI implements HoodIO { public HoodIOSimWPI() { hoodSim = - new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox); + new DCMotorSim( + LinearSystemId.createDCMotorSystem(GEARBOX, 0.004, MOTOR_REDUCTION), GEARBOX); hoodSim.setState(0, 0.0); @@ -44,7 +45,7 @@ public void updateInputs(HoodIOInputs inputs) { // Update simulation state hoodSim.setInputVoltage( MathUtil.clamp( - appliedVolts, -RobotConstants.nominalVoltage, RobotConstants.nominalVoltage)); + appliedVolts, -RobotConstants.NOMINAL_VOLTAGE, RobotConstants.NOMINAL_VOLTAGE)); hoodSim.update(Robot.defaultPeriodSecs); // Update turn inputs @@ -54,8 +55,8 @@ public void updateInputs(HoodIOInputs inputs) { inputs.appliedVolts = appliedVolts; inputs.currentAmps = Math.abs(hoodSim.getCurrentDrawAmps()); - if (hoodSim.getAngularPositionRad() > maxPosRad) { - hoodSim.setState(maxPosRad, 0); + if (hoodSim.getAngularPositionRad() > MAX_POS_RAD) { + hoodSim.setState(MAX_POS_RAD, 0); } } @@ -68,11 +69,11 @@ public void setOpenLoop(Voltage volts) { @Override public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { closedLoop = true; - double setpoint = MathUtil.clamp(rotation.getRadians(), minPosRad, maxPosRad); + double setpoint = MathUtil.clamp(rotation.getRadians(), MIN_POS_RAD, MAX_POS_RAD); this.feedforwardVolts = - RobotConstants.nominalVoltage + RobotConstants.NOMINAL_VOLTAGE * angularVelocity.in(RadiansPerSecond) - / maxAngularVelocity.in(RadiansPerSecond); + / MAX_ANGULAR_VELOCITY.in(RadiansPerSecond); positionController.setSetpoint(setpoint); } @@ -88,6 +89,6 @@ public void configureSoftLimits(boolean enable) {} @Override public void resetEncoder() { - hoodSim.setAngle(maxPosRad); + hoodSim.setAngle(MAX_POS_RAD); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java index d7ae5184..c4e0f9cc 100644 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java @@ -47,13 +47,13 @@ public HoodIOSpark() { hoodConfig .inverted(true) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEO550Constants.defaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.nominalVoltage); + .smartCurrentLimit(NEO550Constants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); hoodConfig .encoder - .positionConversionFactor(encoderPositionFactor) - .velocityConversionFactor(encoderVelocityFactor); + .positionConversionFactor(ENCODER_POSITION_FACTOR) + .velocityConversionFactor(ENCODER_VELOCITY_FACTOR); hoodConfig .closedLoop @@ -64,9 +64,9 @@ public HoodIOSpark() { hoodConfig .softLimit - .forwardSoftLimit(maxPosRad) + .forwardSoftLimit(MAX_POS_RAD) .forwardSoftLimitEnabled(true) - .reverseSoftLimit(minPosRad) + .reverseSoftLimit(MIN_POS_RAD) .reverseSoftLimitEnabled(true); hoodConfig.signals.appliedOutputPeriodMs(20).busVoltagePeriodMs(20).outputCurrentPeriodMs(20); @@ -99,11 +99,11 @@ public void setOpenLoop(Voltage volts) { @Override public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { - double setpoint = MathUtil.clamp(rotation.getRadians(), minPosRad, maxPosRad); + double setpoint = MathUtil.clamp(rotation.getRadians(), MIN_POS_RAD, MAX_POS_RAD); double feedforwardVolts = - RobotConstants.nominalVoltage + RobotConstants.NOMINAL_VOLTAGE * angularVelocity.in(RadiansPerSecond) - / maxAngularVelocity.in(RadiansPerSecond); + / MAX_ANGULAR_VELOCITY.in(RadiansPerSecond); hoodController.setSetpoint( setpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforwardVolts); } @@ -112,9 +112,9 @@ public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { public void setVelocity(AngularVelocity angularVelocity) { double setpoint = angularVelocity.in(RadiansPerSecond); double feedforwardVolts = - RobotConstants.nominalVoltage + RobotConstants.NOMINAL_VOLTAGE * angularVelocity.in(RadiansPerSecond) - / maxAngularVelocity.in(RadiansPerSecond); + / MAX_ANGULAR_VELOCITY.in(RadiansPerSecond); hoodController.setSetpoint( setpoint, ControlType.kVelocity, ClosedLoopSlot.kSlot1, feedforwardVolts); } @@ -133,6 +133,6 @@ public void configureSoftLimits(boolean enable) { @Override public void resetEncoder() { - encoderSpark.setPosition(maxPosRad); + encoderSpark.setPosition(MAX_POS_RAD); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index a7cb7e8d..4dbf29e3 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -2,7 +2,7 @@ import static edu.wpi.first.units.Units.*; import static frc.robot.subsystems.launcher.LauncherConstants.*; -import static frc.robot.subsystems.launcher.LauncherConstants.HoodConstants.ballToHoodOffset; +import static frc.robot.subsystems.launcher.LauncherConstants.HoodConstants.BALL_TO_HOOD_OFFSET; import static frc.robot.subsystems.launcher.LauncherConstants.TurretConstants.*; import edu.wpi.first.math.MathUtil; @@ -88,22 +88,22 @@ public Launcher( flywheelDisconnectedAlert = new Alert("Disconnected flywheel motor", AlertType.kError); hoodDisconnectedAlert = new Alert("Disconnected hood motor", AlertType.kError); - headingController.setTolerance(marginRad); + headingController.setTolerance(MARGIN_RAD); } @Override public void periodic() { - long t0 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t0 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; turretIO.updateInputs(turretInputs); flywheelIO.updateInputs(flywheelInputs); hoodIO.updateInputs(hoodInputs); - long t1 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t1 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; Logger.processInputs("Turret", turretInputs); Logger.processInputs("Flywheel", flywheelInputs); Logger.processInputs("Hood", hoodInputs); - long t2 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t2 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; turretDisconnectedAlert.set(!turretInputs.motorControllerConnected); flywheelDisconnectedAlert.set(!flywheelInputs.connected); @@ -118,34 +118,34 @@ public void periodic() { logCachedAimData(); hasCachedAimData = false; } - long t3 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t3 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Update and plot ball trajectories - if (logFuelTrajectories) { + if (LOG_FUEL_TRAJECTORIES) { double dt = Robot.defaultPeriodSecs; ballisticSimTimer += dt; ballisticLogTimer += dt; - boolean doSim = ballisticSimTimer >= ballisticSimPeriod; - boolean doLog = ballisticLogTimer >= ballisticLogPeriod; + boolean doSim = ballisticSimTimer >= BALLISTIC_SIM_PERIOD; + boolean doLog = ballisticLogTimer >= BALLISTIC_LOG_PERIOD; if (doSim) { ballisticSimTimer = 0.0; - updateBallisticsSim(fuelNominal, nominalKey, ballisticSimPeriod, doLog); - updateBallisticsSim(fuelReplanned, replannedKey, ballisticSimPeriod, doLog); - updateBallisticsSim(fuelActual, actualKey, ballisticSimPeriod, doLog); + updateBallisticsSim(fuelNominal, NOMINAL_KEY, BALLISTIC_SIM_PERIOD, doLog); + updateBallisticsSim(fuelReplanned, REPLANNED_KEY, BALLISTIC_SIM_PERIOD, doLog); + updateBallisticsSim(fuelActual, ACTUAL_KEY, BALLISTIC_SIM_PERIOD, doLog); } if (doLog) { ballisticLogTimer = 0.0; } } - long t4 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t4 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Profiling output - if (Constants.FeatureFlags.profilingEnabled) { + if (Constants.FeatureFlags.PROFILING_ENABLED) { long totalMs = (t4 - t0) / 1_000_000; if (totalMs > 3) { System.out.println( @@ -176,10 +176,10 @@ public double getSimCurrentDrawAmps() { } public void aim(Translation3d target) { - long aimStart = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long aimStart = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Get vector from static target to turret - turretBasePose = new Pose3d(chassisPoseSupplier.get()).plus(chassisToTurretBase); + turretBasePose = new Pose3d(chassisPoseSupplier.get()).plus(CHASSIS_TO_TURRET_BASE); vectorTurretBaseToTarget = target.minus(turretBasePose.getTranslation()); // Compute distance-based impact angle @@ -188,10 +188,10 @@ public void aim(Translation3d target) { Rotation2d dynamicImpactAngle = getImpactAngle(horizontalDistance); // Set flywheel speed assuming a motionless robot - var v0_nominal = getV0Nominal(vectorTurretBaseToTarget, dynamicImpactAngle, nominalKey); + var v0_nominal = getV0Nominal(vectorTurretBaseToTarget, dynamicImpactAngle, NOMINAL_KEY); var flywheelSetpoint = MetersPerSecond.of(flywheelSetpointfromBallistics(v0_nominal.getNorm())); flywheelIO.setVelocity(flywheelSetpoint); - long t1 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t1 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Get translation velocities (m/s) of the turret caused by motion of the chassis var robotRelative = chassisSpeedsSupplier.get(); @@ -199,15 +199,16 @@ public void aim(Translation3d target) { ChassisSpeeds.fromRobotRelativeSpeeds( robotRelative, turretBasePose.toPose2d().getRotation()); var v_base = getTurretBaseSpeeds(turretBasePose.toPose2d().getRotation(), fieldRelative); - long t2 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t2 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Get actual initial shot speed double initialSpeedMetersPerSec = ballisticsFromFlywheelSetpoint(flywheelInputs.velocityMetersPerSec); // Replan shot using actual initial shot speed speed - var v0_total = getV0Replanned(vectorTurretBaseToTarget, initialSpeedMetersPerSec, replannedKey); - long t3 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + var v0_total = + getV0Replanned(vectorTurretBaseToTarget, initialSpeedMetersPerSec, REPLANNED_KEY); + long t3 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Point turret to align velocity vectors var v0_flywheel = v0_total.minus(v_base); @@ -227,12 +228,12 @@ public void aim(Translation3d target) { turretIO.setPosition( turretSetpoint, RadiansPerSecond.of(robotRelative.omegaRadiansPerSecond).unaryMinus().times(2.0)); - hoodSetpoint = new Rotation2d(v0_horizontal, v0_flywheel.getZ()).minus(ballToHoodOffset); + hoodSetpoint = new Rotation2d(v0_horizontal, v0_flywheel.getZ()).minus(BALL_TO_HOOD_OFFSET); hoodIO.setPosition(hoodSetpoint, RadiansPerSecond.of(0)); - long t4 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t4 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Get actual hood & turret position - Rotation2d hoodPosition = hoodInputs.position.plus(ballToHoodOffset); + Rotation2d hoodPosition = hoodInputs.position.plus(BALL_TO_HOOD_OFFSET); Rotation2d turretPosition = turretInputs.relativePosition.plus(turretBasePose.toPose2d().getRotation()); @@ -252,11 +253,11 @@ public void aim(Translation3d target) { cachedActualD = vectorTurretBaseToTarget; cachedActualV = v0_actual; hasCachedAimData = true; - long t5 = Constants.FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t5 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Spawn simulated fuel fuelSpawnTimer += Robot.defaultPeriodSecs; - if (fuelSpawnTimer >= fuelSpawnPeriod && logFuelTrajectories) { + if (fuelSpawnTimer >= FUEL_SPAWN_PERIOD && LOG_FUEL_TRAJECTORIES) { fuelSpawnTimer = 0.0; fuelNominal.add( @@ -268,7 +269,7 @@ public void aim(Translation3d target) { } // Profiling output for aim() - if (Constants.FeatureFlags.profilingEnabled) { + if (Constants.FeatureFlags.PROFILING_ENABLED) { long totalUs = (t5 - aimStart) / 1_000; if (totalUs > 500) { System.out.println( @@ -313,7 +314,7 @@ public boolean isTurretDesaturated() { @AutoLogOutput(key = "Launcher/IsOnTarget") public boolean isOnTarget() { - double tolerance = isOnTargetTolerance.in(Radians); + double tolerance = IS_ON_TARGET_TOLERANCE.in(Radians); double hoodError = Math.abs(hoodInputs.position.minus(hoodSetpoint).getRadians()); double turretError = Math.abs(turretInputs.relativePosition.minus(turretSetpoint).getRadians()); return hoodError < tolerance && turretError < tolerance; @@ -324,8 +325,8 @@ private Translation3d getTurretBaseSpeeds(Rotation2d rotation, ChassisSpeeds cha double vy = chassisSpeeds.vyMetersPerSecond; double omega = chassisSpeeds.omegaRadiansPerSecond; - double rx = chassisToTurretBase.getX(); - double ry = chassisToTurretBase.getY(); + double rx = CHASSIS_TO_TURRET_BASE.getX(); + double ry = CHASSIS_TO_TURRET_BASE.getY(); double offX = -omega * ry; double offY = omega * rx; @@ -346,12 +347,12 @@ private Translation3d getTurretBaseSpeeds(Rotation2d rotation, ChassisSpeeds cha private Rotation2d getImpactAngle(double horizontalDistanceMeters) { double t = - (horizontalDistanceMeters - impactAngleCloseDistance.in(Meters)) - / (impactAngleFarDistance.in(Meters) - impactAngleCloseDistance.in(Meters)); + (horizontalDistanceMeters - IMPACT_ANGLE_CLOSE_DISTANCE.in(Meters)) + / (IMPACT_ANGLE_FAR_DISTANCE.in(Meters) - IMPACT_ANGLE_CLOSE_DISTANCE.in(Meters)); t = MathUtil.clamp(t, 0.0, 1.0); double angleDeg = - MathUtil.interpolate(impactAngleClose.getDegrees(), impactAngleFar.getDegrees(), t); + MathUtil.interpolate(IMPACT_ANGLE_CLOSE.getDegrees(), IMPACT_ANGLE_FAR.getDegrees(), t); return Rotation2d.fromDegrees(angleDeg); } @@ -369,18 +370,18 @@ private Translation3d getV0Nominal(Translation3d d, Rotation2d impactAngle, Stri double denominator = 2 * (dz + dr * impactAngle.getTan()); // Guard: denominator <= 0 means target is unreachable with this impact angle (would require // negative velocity or infinite speed). Using < 1e-6 threshold also catches near-zero values - // that would cause numerical instability in sqrt(g / denominator). + // that would cause numerical instability in sqrt(G / denominator). // Use !(x >= threshold) instead of (x < threshold) to catch NaN if (!(denominator >= 1e-6)) { Logger.recordOutput("Launcher/" + key + "/Reachable", false); return v0nominalLast; } - double v_0r = dr * Math.sqrt(g / denominator); + double v_0r = dr * Math.sqrt(G / denominator); if (!(v_0r >= 1e-6)) { Logger.recordOutput("Launcher/" + key + "/Reachable", false); return v0nominalLast; } - double v_0z = (g * dr) / v_0r - v_0r * impactAngle.getTan(); + double v_0z = (G * dr) / v_0r - v_0r * impactAngle.getTan(); double v_0x = v_0r * d.toTranslation2d().getAngle().getCos(); double v_0y = v_0r * d.toTranslation2d().getAngle().getSin(); @@ -408,7 +409,7 @@ private Translation3d getV0Replanned(Translation3d d, double v_flywheel, String double rHatY = d.getY() / dr; double v_sq = v_flywheel * v_flywheel; - double discriminant = v_sq * v_sq - g * (g * dr * dr + 2 * dz * v_sq); + double discriminant = v_sq * v_sq - G * (G * dr * dr + 2 * dz * v_sq); // Guard: discriminant < 0 means target is beyond maximum range for current flywheel speed. // Using < 1e-6 threshold adds safety margin against sqrt of tiny negative values from @@ -420,12 +421,12 @@ private Translation3d getV0Replanned(Translation3d d, double v_flywheel, String } // High-arc solution (lower trajectory would use v_sq - sqrt(discriminant)) - double tanTheta = (v_sq + Math.sqrt(discriminant)) / (g * dr); + double tanTheta = (v_sq + Math.sqrt(discriminant)) / (G * dr); // sin(2*atan(x)) = 2x/(1+x²) - avoids two trig function calls Logger.recordOutput( "Launcher/" + key + "/PredictedRange", - (v_sq * 2.0 * tanTheta) / (g * (1.0 + tanTheta * tanTheta))); + (v_sq * 2.0 * tanTheta) / (G * (1.0 + tanTheta * tanTheta))); // Effective velocity available for ballistics double v_r = v_flywheel / Math.sqrt(1 + tanTheta * tanTheta); @@ -459,7 +460,7 @@ private void logCachedAimData() { Logger.recordOutput("Launcher/Flywheel velocity too low", cachedFlywheelVelocityTooLow); if (!cachedFlywheelVelocityTooLow) { - log(cachedActualD, cachedActualV, actualKey); + log(cachedActualD, cachedActualV, ACTUAL_KEY); } } @@ -488,10 +489,10 @@ private void log(Translation3d d, Translation3d v, String key) { Logger.recordOutput("Launcher/" + key + "/TravelTime", dr / v_r); } - var max_height = turretBasePose.getZ() + v_z * v_z / (2 * g); + var max_height = turretBasePose.getZ() + v_z * v_z / (2 * G); Logger.recordOutput("Launcher/" + key + "/MaxHeight", max_height); - boolean clearsCeiling = Meters.of(max_height).plus(fuelRadius).lt(ceilingHeight); + boolean clearsCeiling = Meters.of(max_height).plus(FUEL_RADIUS).lt(CEILING_HEIGHT); Logger.recordOutput("Launcher/" + key + "/ClearsCeiling", clearsCeiling); } @@ -521,7 +522,7 @@ private void updateBallisticsSim( for (int i = traj.size() - 1; i >= 0; i--) { BallisticObject o = traj.get(i); - o.vz -= g * dt; + o.vz -= G * dt; o.px += o.vx * dt; o.py += o.vy * dt; o.pz += o.vz * dt; @@ -563,10 +564,10 @@ public Command initializeHoodCommand() { } private double flywheelSetpointfromBallistics(double ballistics) { - return FlywheelScaling.coefficient * Math.pow(ballistics, FlywheelScaling.exponent); + return FlywheelScaling.COEFFICIENT * Math.pow(ballistics, FlywheelScaling.EXPONENT); } private double ballisticsFromFlywheelSetpoint(double setpoint) { - return Math.pow(setpoint / FlywheelScaling.coefficient, 1.0 / FlywheelScaling.exponent); + return Math.pow(setpoint / FlywheelScaling.COEFFICIENT, 1.0 / FlywheelScaling.EXPONENT); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index 42a0ea21..6cc3255d 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -19,70 +19,70 @@ public final class LauncherConstants { // Geometry - public static final Distance fuelRadius = Inches.of(3); - public static final Distance ceilingHeight = Feet.of(11).plus(Inches.of(2)); - public static final double g = 9.81; + public static final Distance FUEL_RADIUS = Inches.of(3); + public static final Distance CEILING_HEIGHT = Feet.of(11).plus(Inches.of(2)); + public static final double G = 9.81; // Distance-based impact angle: steeper at close range, shallower at far range - public static final Distance impactAngleCloseDistance = Meters.of(2.0); - public static final Distance impactAngleFarDistance = Meters.of(6.0); - public static final Rotation2d impactAngleClose = Rotation2d.fromDegrees(55); - public static final Rotation2d impactAngleFar = Rotation2d.fromDegrees(40); + public static final Distance IMPACT_ANGLE_CLOSE_DISTANCE = Meters.of(2.0); + public static final Distance IMPACT_ANGLE_FAR_DISTANCE = Meters.of(6.0); + public static final Rotation2d IMPACT_ANGLE_CLOSE = Rotation2d.fromDegrees(55); + public static final Rotation2d IMPACT_ANGLE_FAR = Rotation2d.fromDegrees(40); // Logging / simulation periods - public static final boolean logFuelTrajectories; - public static final double fuelSpawnPeriod; - public static final double ballisticSimPeriod; - public static final double ballisticLogPeriod; + public static final boolean LOG_FUEL_TRAJECTORIES; + public static final double FUEL_SPAWN_PERIOD; + public static final double BALLISTIC_SIM_PERIOD; + public static final double BALLISTIC_LOG_PERIOD; static { switch (Constants.currentMode) { case REAL -> { - logFuelTrajectories = true; - fuelSpawnPeriod = 0.2; - ballisticSimPeriod = 0.1; - ballisticLogPeriod = 0.25; + LOG_FUEL_TRAJECTORIES = true; + FUEL_SPAWN_PERIOD = 0.2; + BALLISTIC_SIM_PERIOD = 0.1; + BALLISTIC_LOG_PERIOD = 0.25; } case SIM -> { - logFuelTrajectories = true; - fuelSpawnPeriod = 0.1; - ballisticSimPeriod = 0.05; - ballisticLogPeriod = 0.1; + LOG_FUEL_TRAJECTORIES = true; + FUEL_SPAWN_PERIOD = 0.1; + BALLISTIC_SIM_PERIOD = 0.05; + BALLISTIC_LOG_PERIOD = 0.1; } case REPLAY -> { - logFuelTrajectories = false; - fuelSpawnPeriod = 0.0; - ballisticSimPeriod = 0.0; - ballisticLogPeriod = 0.0; + LOG_FUEL_TRAJECTORIES = false; + FUEL_SPAWN_PERIOD = 0.0; + BALLISTIC_SIM_PERIOD = 0.0; + BALLISTIC_LOG_PERIOD = 0.0; } default -> { - logFuelTrajectories = true; - fuelSpawnPeriod = 0.1; - ballisticSimPeriod = 0.05; - ballisticLogPeriod = 0.1; + LOG_FUEL_TRAJECTORIES = true; + FUEL_SPAWN_PERIOD = 0.1; + BALLISTIC_SIM_PERIOD = 0.05; + BALLISTIC_LOG_PERIOD = 0.1; } } } - public static final String nominalKey = "Nominal"; - public static final String replannedKey = "Replanned"; - public static final String actualKey = "Actual"; + public static final String NOMINAL_KEY = "Nominal"; + public static final String REPLANNED_KEY = "Replanned"; + public static final String ACTUAL_KEY = "Actual"; // Tolerance for isOnTarget() check (independent of motor controller allowable error) - public static final Angle isOnTargetTolerance = Degrees.of(2.0); + public static final Angle IS_ON_TARGET_TOLERANCE = Degrees.of(2.0); public static final class TurretConstants { // Geometry - public static final Transform3d chassisToTurretBase = + public static final Transform3d CHASSIS_TO_TURRET_BASE = new Transform3d(Inches.of(-4.000), Inches.of(6.500), Inches.of(16.331), Rotation3d.kZero); - public static final Rotation2d absEncoderOffset = new Rotation2d(5.157); - public static final Rotation2d mechanismOffset = Rotation2d.kZero; - public static final double upperLimitRad = Units.degreesToRadians(270); - public static final double lowerLimitRad = Units.degreesToRadians(45); - public static final double marginRad = Units.degreesToRadians(5); + public static final Rotation2d ABS_ENCODER_OFFSET = new Rotation2d(5.157); + public static final Rotation2d MECHANISM_OFFSET = Rotation2d.kZero; + public static final double UPPER_LIMIT_RAD = Units.degreesToRadians(270); + public static final double LOWER_LIMIT_RAD = Units.degreesToRadians(45); + public static final double MARGIN_RAD = Units.degreesToRadians(5); // Position controller public static final double kP = 0.5; @@ -90,66 +90,65 @@ public static final class TurretConstants { public static final Angle kAllowableError = Degrees.of(0.25); // Motor controller - public static final double motorReduction = 9.0 * 72.0 / 12.0; - public static final AngularVelocity maxAngularVelocity = - NEO550Constants.freeSpeed.div(motorReduction); - public static final double encoderPositionFactor = (2 * Math.PI) / motorReduction; // Radians - public static final double encoderVelocityFactor = - (2 * Math.PI) / (60.0 * motorReduction); // Rad/sec - + public static final double MOTOR_REDUCTION = 9.0 * 72.0 / 12.0; + public static final AngularVelocity MAX_ANGULAR_VELOCITY = + NEO550Constants.FREE_SPEED.div(MOTOR_REDUCTION); + public static final double ENCODER_POSITION_FACTOR = (2 * Math.PI) / MOTOR_REDUCTION; // Radians + public static final double ENCODER_VELOCITY_FACTOR = + (2 * Math.PI) / (60.0 * MOTOR_REDUCTION); // Rad/sec } public static final class FlywheelConstants { public static final class FlywheelScaling { - public static final double exponent = 1.8; - public static final double coefficient = 0.335; + public static final double EXPONENT = 1.8; + public static final double COEFFICIENT = 0.335; } - public static final Distance wheelRadius = Inches.of(1.5); + public static final Distance WHEEL_RADIUS = Inches.of(1.5); // Velocity Controller - public static final double maxAcceleration = 4000.0; - public static final double maxJerk = 40000.0; + public static final double MAX_ACCELERATION = 4000.0; + public static final double MAX_JERK = 40000.0; // Motor controller - public static final double motorReduction = 1.0; - public static final AngularVelocity maxAngularVelocity = - KrakenX60Constants.freeSpeed.div(motorReduction); - public static final Slot0Configs velocityVoltageGains = + public static final double MOTOR_REDUCTION = 1.0; + public static final AngularVelocity MAX_ANGULAR_VELOCITY = + KrakenX60Constants.FREE_SPEED.div(MOTOR_REDUCTION); + public static final Slot0Configs VELOCITY_VOLTAGE_GAINS = new Slot0Configs().withKP(0.11).withKI(0.0).withKD(0.0).withKS(0.1).withKV(0.12); - public static final Slot1Configs velocityTorqueCurrentGains = + public static final Slot1Configs VELOCITY_TORQUE_CURRENT_GAINS = new Slot1Configs().withKP(12).withKI(0.0).withKD(0.0).withKS(2.5); // Simulation - public static final DCMotor gearbox = DCMotor.getKrakenX60(2); + public static final DCMotor GEARBOX = DCMotor.getKrakenX60(2); } public static final class HoodConstants { - public static final Rotation2d ballToHoodOffset = new Rotation2d(Degrees.of(0)); + public static final Rotation2d BALL_TO_HOOD_OFFSET = new Rotation2d(Degrees.of(0)); public static final Angle kAllowableError = Degrees.of(0.25); // Position controller public static final double kPRealPos = 0.35; public static final double kPSimPos = 1.5; public static final double kDSimPos = 0.05; - public static final Angle minPosition = Degrees.of(60); - public static final Angle maxPosition = Degrees.of(80); - public static final double minPosRad = minPosition.in(Radians); - public static final double maxPosRad = maxPosition.in(Radians); + public static final Angle MIN_POSITION = Degrees.of(60); + public static final Angle MAX_POSITION = Degrees.of(80); + public static final double MIN_POS_RAD = MIN_POSITION.in(Radians); + public static final double MAX_POS_RAD = MAX_POSITION.in(Radians); // Velocity controller public static final double kPRealVel = 0.2; // Motor controller - public static final double motorReduction = 5.0 * 256.0 / 16.0; - public static final AngularVelocity maxAngularVelocity = - NEO550Constants.freeSpeed.div(motorReduction); - public static final double encoderPositionFactor = 2 * Math.PI / motorReduction; // Radians - public static final double encoderVelocityFactor = - (2 * Math.PI) / (60.0 * motorReduction); // Rad/sec + public static final double MOTOR_REDUCTION = 5.0 * 256.0 / 16.0; + public static final AngularVelocity MAX_ANGULAR_VELOCITY = + NEO550Constants.FREE_SPEED.div(MOTOR_REDUCTION); + public static final double ENCODER_POSITION_FACTOR = 2 * Math.PI / MOTOR_REDUCTION; // Radians + public static final double ENCODER_VELOCITY_FACTOR = + (2 * Math.PI) / (60.0 * MOTOR_REDUCTION); // Rad/sec // Simulation - public static final DCMotor gearbox = DCMotor.getNeo550(1); + public static final DCMotor GEARBOX = DCMotor.getNeo550(1); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java index 67683d11..58b5610a 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java @@ -49,19 +49,19 @@ public TurretIOSimSpark() { turnConfig .inverted(false) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEO550Constants.defaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.nominalVoltage); + .smartCurrentLimit(NEO550Constants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); turnConfig .encoder - .positionConversionFactor(encoderPositionFactor) - .velocityConversionFactor(encoderVelocityFactor); + .positionConversionFactor(ENCODER_POSITION_FACTOR) + .velocityConversionFactor(ENCODER_VELOCITY_FACTOR); turnConfig .softLimit - .forwardSoftLimit(upperLimitRad) + .forwardSoftLimit(UPPER_LIMIT_RAD) .forwardSoftLimitEnabled(true) - .reverseSoftLimit(lowerLimitRad) + .reverseSoftLimit(LOWER_LIMIT_RAD) .reverseSoftLimitEnabled(true); turnConfig @@ -77,9 +77,10 @@ public TurretIOSimSpark() { turnSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(GEARBOX, TURRET_MOI_KG_M2, motorReduction), GEARBOX); + LinearSystemId.createDCMotorSystem(GEARBOX, TURRET_MOI_KG_M2, MOTOR_REDUCTION), + GEARBOX); - turnSim.setState(2.0 * Math.PI - mechanismOffset.getRadians(), 0); + turnSim.setState(2.0 * Math.PI - MECHANISM_OFFSET.getRadians(), 0); turnSparkSim.setPosition(turnSim.getAngularPositionRad()); } @@ -94,13 +95,13 @@ public void updateInputs(TurretIOInputs inputs) { // Update inputs inputs.motorControllerConnected = true; - inputs.relativePosition = new Rotation2d(turnSparkSim.getPosition()).plus(mechanismOffset); + inputs.relativePosition = new Rotation2d(turnSparkSim.getPosition()).plus(MECHANISM_OFFSET); inputs.velocityRadPerSec = turnSparkSim.getVelocity(); inputs.appliedVolts = turnSparkSim.getAppliedOutput() * turnSparkSim.getBusVoltage(); inputs.currentAmps = Math.abs(turnSparkSim.getMotorCurrent()); inputs.absoluteEncoderConnected = true; - inputs.absolutePosition = new Rotation2d(turnSparkSim.getPosition()).plus(mechanismOffset); + inputs.absolutePosition = new Rotation2d(turnSparkSim.getPosition()).plus(MECHANISM_OFFSET); inputs.oversaturation = oversaturation; inputs.oversaturationLessMargin = oversaturationLessMargin; @@ -117,16 +118,16 @@ public void setOpenLoop(Voltage volts) { public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { double setpoint = MathUtil.inputModulus( - rotation.getRadians() - mechanismOffset.getRadians(), 0.0, 2.0 * Math.PI); - double clampedSetpoint = MathUtil.clamp(setpoint, lowerLimitRad, upperLimitRad); + rotation.getRadians() - MECHANISM_OFFSET.getRadians(), 0.0, 2.0 * Math.PI); + double clampedSetpoint = MathUtil.clamp(setpoint, LOWER_LIMIT_RAD, UPPER_LIMIT_RAD); double clampedSetpointWithMargin = - MathUtil.clamp(setpoint, lowerLimitRad + marginRad, upperLimitRad - marginRad); + MathUtil.clamp(setpoint, LOWER_LIMIT_RAD + MARGIN_RAD, UPPER_LIMIT_RAD - MARGIN_RAD); oversaturation = setpoint - clampedSetpoint; oversaturationLessMargin = setpoint - clampedSetpointWithMargin; double feedforwardVolts = - RobotConstants.nominalVoltage + RobotConstants.NOMINAL_VOLTAGE * angularVelocity.in(RadiansPerSecond) - / maxAngularVelocity.in(RadiansPerSecond); + / MAX_ANGULAR_VELOCITY.in(RadiansPerSecond); controller.setSetpoint( clampedSetpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforwardVolts); } diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java index d7e85267..cebf75f4 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java @@ -54,26 +54,26 @@ public TurretIOSpark() { new DutyCycleEncoder( new DigitalInput(DIOPorts.turretAbsEncoder), 2 * Math.PI, - absEncoderOffset.getRadians() + mechanismOffset.getRadians()); + ABS_ENCODER_OFFSET.getRadians() + MECHANISM_OFFSET.getRadians()); var turnConfig = new SparkMaxConfig(); turnConfig .inverted(true) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEO550Constants.defaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.nominalVoltage); + .smartCurrentLimit(NEO550Constants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); turnConfig .encoder - .positionConversionFactor(encoderPositionFactor) - .velocityConversionFactor(encoderVelocityFactor); + .positionConversionFactor(ENCODER_POSITION_FACTOR) + .velocityConversionFactor(ENCODER_VELOCITY_FACTOR); turnConfig .softLimit - .forwardSoftLimit(upperLimitRad) + .forwardSoftLimit(UPPER_LIMIT_RAD) .forwardSoftLimitEnabled(true) - .reverseSoftLimit(lowerLimitRad) + .reverseSoftLimit(LOWER_LIMIT_RAD) .reverseSoftLimitEnabled(true); turnConfig @@ -103,7 +103,7 @@ public void updateInputs(TurretIOInputs inputs) { } // Read from cached values (non-blocking) - updated by SparkOdometryThread - inputs.relativePosition = new Rotation2d(sparkInputs.getPosition()).plus(mechanismOffset); + inputs.relativePosition = new Rotation2d(sparkInputs.getPosition()).plus(MECHANISM_OFFSET); inputs.velocityRadPerSec = sparkInputs.getVelocity(); inputs.appliedVolts = sparkInputs.getAppliedVolts(); inputs.currentAmps = sparkInputs.getOutputCurrent(); @@ -130,16 +130,16 @@ public void setOpenLoop(Voltage volts) { public void setPosition(Rotation2d rotation, AngularVelocity angularVelocity) { double setpoint = MathUtil.inputModulus( - rotation.getRadians() - mechanismOffset.getRadians(), 0.0, 2 * Math.PI); - double clampedSetpoint = MathUtil.clamp(setpoint, lowerLimitRad, upperLimitRad); + rotation.getRadians() - MECHANISM_OFFSET.getRadians(), 0.0, 2 * Math.PI); + double clampedSetpoint = MathUtil.clamp(setpoint, LOWER_LIMIT_RAD, UPPER_LIMIT_RAD); double clampedSetpointWithMargin = - MathUtil.clamp(setpoint, lowerLimitRad + marginRad, upperLimitRad - marginRad); + MathUtil.clamp(setpoint, LOWER_LIMIT_RAD + MARGIN_RAD, UPPER_LIMIT_RAD - MARGIN_RAD); oversaturation = setpoint - clampedSetpoint; oversaturationLessMargin = setpoint - clampedSetpointWithMargin; double feedforwardVolts = - RobotConstants.nominalVoltage + RobotConstants.NOMINAL_VOLTAGE * angularVelocity.in(RadiansPerSecond) - / maxAngularVelocity.in(RadiansPerSecond); + / MAX_ANGULAR_VELOCITY.in(RadiansPerSecond); controller.setSetpoint( clampedSetpoint, ControlType.kPosition, ClosedLoopSlot.kSlot0, feedforwardVolts); } diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 3fe74f0d..da291d45 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -123,14 +123,14 @@ public Rotation2d getTargetX(int cameraIndex) { @Override public void periodic() { - long visionStart = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long visionStart = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; loopCounter++; // Copy cached inputs from background thread (should be fast - volatile reads) for (int i = 0; i < io.length; i++) { visionInputs[i].getSnapshot().copyTo(inputs[i]); } - long t1 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t1 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Log inputs via AdvantageKit (throttled - serialization is expensive) // Note: Throttling reduces CPU load but loses data granularity for replay @@ -139,7 +139,7 @@ public void periodic() { Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]); } } - long t2 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t2 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Initialize logging values allTagPoses.clear(); @@ -217,7 +217,7 @@ public void periodic() { allRobotPosesRejected.addAll(robotPosesRejected); } - long t3 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t3 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Process observations in batches every processingIntervalLoops // This allows cameras to accumulate observations before fusion decides what agrees @@ -254,7 +254,7 @@ public void periodic() { // Clear buffer after processing observationBuffer.clear(); } - long t4 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t4 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Log summary data (throttled along with processInputs) if (loopCounter % kLoggingDivisor == 0) { @@ -271,10 +271,10 @@ public void periodic() { "Vision/Summary/RobotPosesRejected", allRobotPosesRejected.toArray(Pose3d[]::new)); } } - long t5 = FeatureFlags.profilingEnabled ? System.nanoTime() : 0; + long t5 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Profiling output - if (FeatureFlags.profilingEnabled) { + if (FeatureFlags.PROFILING_ENABLED) { long totalMs = (t5 - visionStart) / 1_000_000; if (totalMs > 5) { System.out.println( diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index d54b8ac7..3787bd2e 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -91,7 +91,7 @@ public class VisionConstants { public static double velocityCheckTimeoutSeconds = 0.5; // Skip check if no observation for this long public static final double maxReasonableVelocityMps = - DriveConstants.drivetrainSpeedLimit.in(MetersPerSecond) * 1.5; // Allow some margin + DriveConstants.DRIVETRAIN_SPEED_LIMIT.in(MetersPerSecond) * 1.5; // Allow some margin // Score returned by velocityConsistency when it can't verify the observation // (no history from this camera, or last observation is older than velocityCheckTimeoutSeconds). From e9672bae16e7b05bf4ac7e3146a7c98351b93a50 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Thu, 4 Jun 2026 11:25:40 -0400 Subject: [PATCH 3/8] Normalize all line endings to LF Add .gitattributes enforcing eol=lf for all text files, and convert all existing tracked files from CRLF to LF. Co-Authored-By: Claude Sonnet 4.6 --- .gitattributes | 10 ++++++++++ ascope-assets/Robot_Rho/model.glb | Bin 22540916 -> 22540427 bytes 2 files changed, 10 insertions(+) create mode 100644 .gitattributes diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 00000000..266e9b3d --- /dev/null +++ b/.gitattributes @@ -0,0 +1,10 @@ +# Enforce LF line endings for all text files +* text=auto eol=lf + +# Explicitly binary files +*.png binary +*.jpg binary +*.gif binary +*.ico binary +*.jar binary +*.whl binary diff --git a/ascope-assets/Robot_Rho/model.glb b/ascope-assets/Robot_Rho/model.glb index 4faa01349bc7304cdbc6bc79fb3d7b895f9cadec..9fb3294d9a11f61aada7f2d5ef3eb996a849778d 100644 GIT binary patch delta 5116 zcmd7Uc~lfvz6WrA-9UH!s&01!&8jSdfJj_WKt&W+&7+(0TLkzxYG4Vx3`=1d)WR$9D!c~E zVFj#&Rp5cu@H)H!YhW#`gZ1Er4X_b5!JF_~_&3-LCcFi2!|z}VY=z&$HrNh3U?=Q? z-TpMoo={(HkGehn9(8+*5Bujm?Q`n>*D8F)hS#j|4f49HeWl*Smwl<;71h2xZ(=Qu z1=rx%zFJ?rx1z?^U7c6?_a}dv_ln_78ZPbLFPHcRs|ytU{de6FwXKibhuStJ?L%#M<*4#MIuic5ZF{Lq)wbzT z)UdW?+CHfBw@llAyU_d9?)@JBoIU&dvUl4%`z!qk{{4L?drKV$JpQj72XHhh_CRIr zs@MbY2RI0a;2rS8yKorZgMat0iaqk7UR#=a1dhV{AmJE%0LS4&_z3<3K86$UpYUH$ z51+uNa1#Cqr=S5ogVS&Z8sT#|3tzxF_!Bh2dAQ&&O>MrE?CmqExtD+HsOBx~4J|`4 zx2#!)d2dZaF>QMD^X%O;3&r|b&HfSInu91FIM_U2_x3!7;$z2}i(KAq4Je*%X#P<5 zZo1IitG50^GhBpA@FiS^ui(${HGBi#!gp{5u0jj6!Zr8{d=GzxAK*Ith!KC|;~M|# zkGGXY`oF1Y8*Zu1sBD9qa0_nxGb-;?yyo9b?J0k#J$|qq?!rCj@EU8b`8&4W!&6n7(%>vAA#8Sp;Fj!h)?X ztb&jvG+_vfc`%u2atsT*aO0?k)I_+5WDy}Eg(1^Ym}bm!aVL($l{+&o-Wfa6e1qwc zxC9m#jAmT9&CpofG}=(qa7ag)g)U?EGJRshH*X5f!4A!FgB>?SgV3>~^5*-8m>O4x zu!if(^TTwy=!$0H5@Gl@HgxLy!p2gEJB8g$q`6{e7fQ6USSagInm5(Su{guQG1xjl z4m6T!=Nc#fsNK-m&Y@^bl+D{_GNlT>p}Ekcc6Ncxd@GrPV_mAJ(;!`>qNzoy9CkRF zG|f|dJed$FD!P&^KAuYT5>2nwXna6hqM0c*X@m;aXl}N#IUjpvR*Wur2BqA1liy0C zL4rjH`d=5J$WU_5#$F4N$|86X|YAqUnuC;31rhTrVwrv`> zXg_N5!PIohi@_MGLq&B_XO5VkPOTwqXvS}4?t*mshP_YqVqV z-ZSnZJB)kCc0-em{kl=Q-P2Lfjbukb2Gwi|!HsTe=F+AN%CzxKG!gE27<=|;+%W>P z=)MtQetJEFdPS(lio`g^%t9#)87^HvX3)_CDa><41@jAHE_0=Lfw@XlGFONMwD^mn z3T;~@Z*A>P?~u87TX%|&;Ptv!=Qs6-c$2 zNR}S%LCLKye5Uh#H9U0`F=1=B_N4BeF%ld?pn09@S2~5m^U<-MBtJUVi{hp;2k~_c zrw6fYP3=uqGXI*^TaCpumJ#W_X@eS;NuIjG-X!Y^v*>Ow#@X47{~R{`FJ#e^8V}}N z9)y&sm05IYOcckX3I_7$)d*kcgie(4A7@cdHyS?^wb4YToYa&>oopB_?}@{*>T(vv ztr-{iW|LZQbc`7Tj^HPq!vs`64~=tFJEuAMyml0M4j(!30h;YRc3s#(}s10C8fOP6NTf=A6$7qcne#>=%8 zYO>$N-qvWer%i6Yl}*E~xc?fl79p>-XVcV4%vZH>V z#RE@rc@D|s@?5Hjj6e&tqPbcyJ6$L_(Kd4T(p-8dN}hW&m&SY+f>w91+2h%MG%*RK zXyzz{T8g4ka-tl=7@a?9YMt7S(=NuF%a-?}L~|Q5#Z!-z^~?LykPzMQQ+E>&ZR?dsM+wWKF8YhF z(bN59c~%}>#_xAjiR1~Vo?w))ah{;!iE^;EBSTjpx@ZNm7VTn~eI2)cq zJ|(6jr;B`~F2n2cDepoEu8Z22tr$ep1g3ImU^E7+u^B8vW&OrMwAv@9j2cWEM7Vl1 z<3{SytY$hM&(c0=Fuk!wHGE)Y!KJph52h=cTx~6&1q)44T|kefppT0~9}XQJO3W56 z2`xsk1crs8`3Q>x8g(VkoDtz^TU$V~ZS4@cI4B&4qkbL^Mqn7=>ffCnLTmLX7O^4* z6^TPd>;mIdLorsay*PwM6uJT@2fk|r29s>_{+rgr{dXi;g6IeWI zhEn9;77x#ySBDZ}n2O;lMv$EI>PY(jQV0jztTywhrwb`j)*|PmVpixP5pPcxPa(ZA z(#=Vn$oN+SrVDl)Aa^$w(v~rdacxzP+MhIYN=gxpHFqN?YFN&~(Nb8{^sYs8#VOnR z7SUm=$Jeijq_5v7>S+JpFZ)5(XxbBoP6|>35ev%CuX&4xkEZcv0}{&x9z$-_?iTiz z+5=CE2Y6ZpVeYCIv-_S^#iI%F3l$HmC?>h8xP%(J>3l|O#ArXQozdj1xg|6SQwEPJ z+$llC%hStBXx^O2!0Us9g99_$hOHAv%qQDR=xH^()H`ap>R5MRe#^`F;JJ7#Te)&V z^cdQRmk-Bx;tD&ih^yw^_%SrVlbtYz5MxPZCyb*fPFmGW+}Jvf?#E#$=%O25Lpmcn zi+GWtUK%qzb)n-4QHsY(Xc;x^HmeVmQLL5w;pG*jiwSbkXJxeGNDLR?$%4Kqz|*e} zXCbrjIKhA%x@rQ|xbZ4#6G3WPqv6zG(`D1|CeW&323|hZqY!@>mzCd4pfB}6&*Sw~ zK5=~l<*$qi^t^#i5A?ixmZ<)hhZaqwlMC#D9fVwYZz6T^g<~}eX8fy%lX?roWk1|G z2}`;;^Mgr&_qQ;)0;T6Ftb7D-H#w83+#$h5jVuD)v;<`pQ36P3L@*5z zVx!~J0R*3QvehrFL6kSF!EdY|W$9JF2TZy5n%@&&Q4Toh@9ST8*x&S* z-~QYm0w55AAQ(cx0vlZ5fO*ypp%4b)5CM@81<}wPVjvdcARZDR5t5(rrB4$u)gK^Am|E|3j5kPBU*8+3;}=mGiA6M8`b^oBmr7y3be z7ytvI5C*|uxEt<)A}EF-FcgM)Z-0L8eHTSlmFyWlhh_Y9&u|z4BViPbh7!0B#=!mX z06ggZbk8r=yuv$Eru-7ddZ$czXif*(^{XF(aWEbxz(jZ$Cc$Kw0>6S^!&LYUJOb08 z)LXxLdj3?#N$`6V9)lS$6K26|m;-a+ahL~Bz;SO>4c>#!a+ zz(#lj-t;E;Z3?Q!=akKxyeXUCDtea`AD2_(7vL>@q*?{jn%=vn$RSWWLux80#HmTE znqFL!NBvb*)3fUPUDd;>_t{-tNKrPVhH(CehihCl<^f(b5B$SBX{-NK{Y$!6lvgKK zJ+-8|-JM6w7%hKW{nLa=RXc~*LBme?06v6|;9p@E?1nw?G5iVk!YA-&*ax4&XRsd*z(F_!hv5i(4qw1u z;7j-lzJ{aT^R4UZQ}O7}g>@bA=tg1PTI27_!+p(t!|ODAvufEyU-OR>>t>i=JCypG zzbLKq_NN)O)z|E|weAVrs(N4hn&)+0VHnNq{l4ae{dLCas`R6#Sw8A$9UOyt_y&%{ zU*TIg0e^#&@Ex3j({KjP!Z|n(7vOui2$$fpchu2_-HUJ}VeFL=(Uj9>Ug>A9{@U4B z;41t8*Sxi}8)q)%_cUp0^)|l`+S&xy;Rf9F4%+%-#xVLz18%|XXG;U50*v?>@i*e@ z3pBMLBf&;Oj98*e18k`daVb06cH0iBbh@(TLR7G_)h#DPdE!!SNf719rYuRIH$+7# zm$FbxMW|?cB2h=_wLCGYG*a0Kc08jc+KLeqX(=7I(w6(;Q+W2MQ|H*~iepi~>Rh32 z(dVLWGperGWXmVPNy>I#RhHX!TkZzx4Qjt~(K32f>6u$?IlLSTEhbR9xUWl&g&EVr zR5A!vE$eh4#%4UEC%CetQWy)pSb{Rbj)I#Oh zE_)$8WonT!J}jbVp<1lmc(holiseTf(aVo0+4pjmvYRz?;JVeUnTxWC@&=fG({~fx z)J@yeRYmzsw;-WL!i*BXjKms=6J0(tUZ(ovq%gQJku*!DuC&aJ zmpwh>jJ4M-m)&ID!2j#kP1aD^H!Y>>{tz$C*(4PWyW<5(Fp_8_$w&*)4ZD-2abBRZ zck=j!J}o7WjI3zWzoqCl{Zk};C?h)d9y6jIRhc5oNLXQ~#&=y5fdtua^Wh;^Cr4xc@qk6+7_q!?wdc2zOki>~(e z8rLer4rFgy4mf0Ll)fRJu+3 zc5=b;S4!#Tx$R`{qr@oQuuvzN~MlDmD%1Bzn_;EVqiWZULhSu%lD=w;{HJyE3-XJ<+M z5CWWpbq@N|`N(St=DgJ5yRxK1l!;k8EVNKP=*uig2sX+0o_bg5Dc@vCd?hET8Q?2l z^0-Jr2e#wxU)cP{b>(?wMrVm3NyF@~txz-+yTYl5nZ@Nz-%{&?JT)(wB`m7!EHy=y zUE~Ya#j!71!4Se&N{{(-$z+wj|-sv(_@-33GtHQ-Un?rN*Z@S7cb3P4N+Es{I6@8V>froljD|2hq>w!YEBf}Jx@KZbWmQNxY%wL;XU#M=^;A2N4~T! z!i1dieA&pA_CzH}uS24hSFs&tE``7*DC`2ujMkI1-Pjv}hdoYjt7t#+zVUArM) zUNvtIe=}ctkRqy>M?%q14*X)JU!uRkJWx@c?{j$rW!_O-iSWhryQMv4B1frBo||kB zpcaS*DgD3;J>_@&<<(~g_L3JkPR*ByIbO_{h&f`+*N9#kHYPL(Z&MR%0(Y z#dW6R{0e0LlT5#AL4l0PFd0)6bWc#fAYpC&?Bbvz>c!AE$gH+-I+-wpmlepQNEU2P zWr659mA&OyPhV0t=OL3|jxVvyRFe<%mgimOc8OE5m==#wA*eY8~Wjb&1431ie)l7r`X=IJ`(-&DOPc!ypJG# zjr23pU-XIc0dk?9^$rO34_2NpCJ!`euYaI?EY0jjmr6FDnWf7I%E|%UH!baCOiac| zx1B=E4N6BJ8YpXru*^SN{``E|Vnca8OevJ1EN*yep^U6bV}y0ZxLDUqR~=O+%6hxYM~xs&Tp50hK*CWlB Date: Thu, 4 Jun 2026 11:27:51 -0400 Subject: [PATCH 4/8] Remove conflicting .vscode/ gitignore entry The blanket .vscode/ rule at the bottom conflicted with the selective allowlist above it (.vscode/settings.json, launch.json, etc.). Removing it so the allowlist takes effect and tracked .vscode files remain tracked. Co-Authored-By: Claude Sonnet 4.6 --- .gitignore | 1 - 1 file changed, 1 deletion(-) diff --git a/.gitignore b/.gitignore index bb807f0f..bf12c9ff 100644 --- a/.gitignore +++ b/.gitignore @@ -157,7 +157,6 @@ gradle-app.setting .project .settings/ bin/ -.vscode/ # IntelliJ *.iml From dc2fb7264cfa05af2308ea525cdd861b36452360 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Thu, 4 Jun 2026 12:02:03 -0400 Subject: [PATCH 5/8] Continue UPPER_SNAKE_CASE renaming across remaining subsystems and centralize roller gains Extends the naming convention refactor to VisionConstants, DriveConstants, FeederConstants, LauncherConstants, LEDConstants, and general Constants; centralizes shared roller PID/torque gains into IntakeConstants.RollerConstants so real and sim IO classes share a single source of truth. Co-Authored-By: Claude Sonnet 4.6 --- src/main/java/frc/robot/Constants.java | 66 ++++++------- src/main/java/frc/robot/Robot.java | 34 +++---- .../subsystems/drive/DriveConstants.java | 26 ++--- .../robot/subsystems/drive/GyroIOBoron.java | 2 +- .../subsystems/drive/ModuleIOTalonFX.java | 6 +- .../subsystems/feeder/FeederConstants.java | 8 +- .../subsystems/feeder/KickerIOSimSpark.java | 6 +- .../subsystems/feeder/KickerIOSpark.java | 6 +- .../feeder/SpindexerIOSimSpark.java | 6 +- .../subsystems/feeder/SpindexerIOSpark.java | 6 +- .../robot/subsystems/hopper/HopperIOReal.java | 2 +- .../robot/subsystems/hopper/HopperIOSim.java | 3 +- .../subsystems/intake/IntakeArmIOReal.java | 2 +- .../subsystems/intake/IntakeArmIOSim.java | 2 +- .../subsystems/intake/IntakeConstants.java | 23 +++-- .../subsystems/intake/RollerIOSimSpark.java | 14 +-- .../subsystems/intake/RollerIOSimTalonFX.java | 14 +-- .../subsystems/intake/RollerIOSpark.java | 12 +-- .../subsystems/intake/RollerIOTalonFX.java | 13 +-- .../launcher/FlywheelIOSimTalonFX.java | 6 +- .../launcher/FlywheelIOTalonFX.java | 6 +- .../subsystems/launcher/HoodIOSimSpark.java | 2 +- .../subsystems/launcher/HoodIOSpark.java | 2 +- .../robot/subsystems/launcher/Launcher.java | 14 +-- .../launcher/LauncherConstants.java | 2 +- .../subsystems/launcher/TurretIOSimSpark.java | 2 +- .../subsystems/launcher/TurretIOSpark.java | 4 +- .../robot/subsystems/leds/LEDConstants.java | 10 +- .../robot/subsystems/leds/LEDController.java | 10 +- .../frc/robot/subsystems/vision/Vision.java | 31 +++--- .../subsystems/vision/VisionConstants.java | 98 ++++++++++--------- .../robot/subsystems/vision/VisionFilter.java | 30 +++--- .../subsystems/vision/VisionFilterTest.java | 20 ++-- 33 files changed, 239 insertions(+), 249 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bb79dcb2..20c0bd3e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -78,69 +78,69 @@ public static final class KrakenX60Constants { public static final class DIOPorts { // max length is 8 - public static final int[] autonomousModeSelector = {0, 1, 2}; + public static final int[] AUTONOMOUS_MODE_SELECTOR = {0, 1, 2}; - public static final int allianceColorSelector = 3; + public static final int ALLIANCE_COLOR_SELECTOR = 3; - public static final int turretAbsEncoder = 4; + public static final int TURRET_ABS_ENCODER = 4; } public static final class CANBusPorts { public static final class CAN2 { - public static final CANBus bus = CANBus.roboRIO(); + public static final CANBus BUS = CANBus.roboRIO(); // Power distribution - public static final int pd = 1; + public static final int PD = 1; // Drivetrain - public static final int gyro = 0; + public static final int GYRO = 0; // Launcher - public static final int turret = 12; - public static final int hood = 13; - public static final int flywheelLeader = 14; - public static final int flywheelFollower = 15; + public static final int TURRET = 12; + public static final int HOOD = 13; + public static final int FLYWHEEL_LEADER = 14; + public static final int FLYWHEEL_FOLLOWER = 15; // Feeder - public static final int spindexer = 16; - public static final int kicker = 17; + public static final int SPINDEXER = 16; + public static final int KICKER = 17; // Intake - public static final int intakeRollerLower = 22; - public static final int intakeRollerUpper = 23; + public static final int INTAKE_ROLLER_LOWER = 22; + public static final int INTAKE_ROLLER_UPPER = 23; } public static final class CANHD { // CAN bus that the devices are located on; // All swerve devices must share the same CAN bus - public static final CANBus bus = new CANBus("canivore"); + public static final CANBus BUS = new CANBus("canivore"); // Drivetrain - public static final int backLeftDrive = 10; - public static final int backRightDrive = 18; - public static final int frontRightDrive = 20; - public static final int frontLeftDrive = 28; - - public static final int backLeftTurn = 11; - public static final int backRightTurn = 19; - public static final int frontRightTurn = 21; - public static final int frontLeftTurn = 29; - - public static final int backRightTurnAbsEncoder = 31; - public static final int frontRightTurnAbsEncoder = 33; - public static final int frontLeftTurnAbsEncoder = 43; - public static final int backLeftTurnAbsEncoder = 45; + public static final int BACK_LEFT_DRIVE = 10; + public static final int BACK_RIGHT_DRIVE = 18; + public static final int FRONT_RIGHT_DRIVE = 20; + public static final int FRONT_LEFT_DRIVE = 28; + + public static final int BACK_LEFT_TURN = 11; + public static final int BACK_RIGHT_TURN = 19; + public static final int FRONT_RIGHT_TURN = 21; + public static final int FRONT_LEFT_TURN = 29; + + public static final int BACK_RIGHT_TURN_ABS_ENC = 31; + public static final int FRONT_RIGHT_TURN_ABS_ENC = 33; + public static final int FRONT_LEFT_TURN_ABS_ENC = 43; + public static final int BACK_LEFT_TURN_ABS_ENC = 45; } } public static final class PneumaticChannels { // hopper - public static final int hopperForward = 14; - public static final int hopperReverse = 15; + public static final int HOPPER_FWD = 14; + public static final int HOPPER_REV = 15; // intake arm - public static final int intakeArmForward = 0; - public static final int intakeArmReverse = 1; + public static final int INTAKE_ARM_FWD = 0; + public static final int INTAKE_ARM_REV = 1; } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7a764b30..bea6b102 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -123,11 +123,11 @@ public class Robot extends LoggedRobot { } public static final AllianceSelector allianceSelector = - new AllianceSelector(DIOPorts.allianceColorSelector); + new AllianceSelector(DIOPorts.ALLIANCE_COLOR_SELECTOR); public static final AutoSelector autoSelector = - new AutoSelector(DIOPorts.autonomousModeSelector, allianceSelector::getAllianceColor); + new AutoSelector(DIOPorts.AUTONOMOUS_MODE_SELECTOR, allianceSelector::getAllianceColor); public final LoggedPowerDistribution powerDistribution = - new LoggedPowerDistribution(CAN2.pd, ModuleType.kRev, "PD"); + new LoggedPowerDistribution(CAN2.PD, ModuleType.kRev, "PD"); private final java.util.Set activeCommands = new java.util.LinkedHashSet<>(); @@ -185,10 +185,10 @@ public Robot() { new Vision( drive::addVisionMeasurement, drive::getFieldRelativeHeading, - new VisionIOPhotonVision(cameraFrontRightName, robotToFrontRightCamera), - new VisionIOPhotonVision(cameraFrontLeftName, robotToFrontLeftCamera), - new VisionIOPhotonVision(cameraBackRightName, robotToBackRightCamera), - new VisionIOPhotonVision(cameraBackLeftName, robotToBackLeftCamera)); + new VisionIOPhotonVision(CAMERA_FRONT_RIGHT_NAME, ROBOT_TO_FRONT_RIGHT_CAMERA), + new VisionIOPhotonVision(CAMERA_FRONT_LEFT_NAME, ROBOT_TO_FRONT_LEFT_CAMERA), + new VisionIOPhotonVision(CAMERA_BACK_RIGHT_NAME, ROBOT_TO_BACK_RIGHT_CAMERA), + new VisionIOPhotonVision(CAMERA_BACK_LEFT_NAME, ROBOT_TO_BACK_LEFT_CAMERA)); launcher = new Launcher( drive::getPose, @@ -199,8 +199,8 @@ public Robot() { if (FeatureFlags.HOPPER_ENABLED) hopper = new Hopper(new HopperIOReal()); intake = new Intake( - new RollerIOSpark(RollerConstants.upperRollerConfig), - new RollerIOSpark(RollerConstants.lowerRollerConfig), + new RollerIOSpark(RollerConstants.UPPER_ROLLER_CONFIG), + new RollerIOSpark(RollerConstants.LOWER_ROLLER_CONFIG), new IntakeArmIOReal()); feeder = new Feeder(new SpindexerIOSpark(), new KickerIOSpark()); compressor = new LoggedCompressor(PneumaticsModuleType.REVPH, "Compressor"); @@ -227,13 +227,13 @@ public Robot() { drive::addVisionMeasurement, drive::getFieldRelativeHeading, new VisionIOPhotonVisionSim( - cameraFrontRightName, robotToFrontRightCamera, drive::getPose), + CAMERA_FRONT_RIGHT_NAME, ROBOT_TO_FRONT_RIGHT_CAMERA, drive::getPose), new VisionIOPhotonVisionSim( - cameraFrontLeftName, robotToFrontLeftCamera, drive::getPose), + CAMERA_FRONT_LEFT_NAME, ROBOT_TO_FRONT_LEFT_CAMERA, drive::getPose), new VisionIOPhotonVisionSim( - cameraBackRightName, robotToBackRightCamera, drive::getPose), + CAMERA_BACK_RIGHT_NAME, ROBOT_TO_BACK_RIGHT_CAMERA, drive::getPose), new VisionIOPhotonVisionSim( - cameraBackLeftName, robotToBackLeftCamera, drive::getPose)); + CAMERA_BACK_LEFT_NAME, ROBOT_TO_BACK_LEFT_CAMERA, drive::getPose)); launcher = new Launcher( drive::getPose, @@ -246,8 +246,8 @@ public Robot() { var intakeArmIOSim = new IntakeArmIOSim(); intake = new Intake( - new RollerIOSimSpark(RollerConstants.upperRollerConfig), - new RollerIOSimSpark(RollerConstants.lowerRollerConfig), + new RollerIOSimSpark(RollerConstants.UPPER_ROLLER_CONFIG), + new RollerIOSimSpark(RollerConstants.LOWER_ROLLER_CONFIG), intakeArmIOSim); pneumaticsSimulator = new PneumaticsSimulator(intakeArmIOSim.intakeArmPneumatic, new REVPHSim(1)); @@ -353,8 +353,8 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); long t1 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - logCANBus("CAN2", Constants.CANBusPorts.CAN2.bus); - logCANBus("CANHD", Constants.CANBusPorts.CANHD.bus); + logCANBus("CAN2", Constants.CANBusPorts.CAN2.BUS); + logCANBus("CANHD", Constants.CANBusPorts.CANHD.BUS); powerDistribution.log(); if (compressor != null) compressor.log(); logHIDs(); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 001fc339..5de8a646 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -209,7 +209,7 @@ public class DriveConstants { private static final Voltage DRIVE_FRICTION_VOLTAGE = Volts.of(0.2); public static final SwerveDrivetrainConstants DRIVETRAIN_CONSTANTS = - new SwerveDrivetrainConstants().withCANBusName(CANHD.bus.getName()); + new SwerveDrivetrainConstants().withCANBusName(CANHD.BUS.getName()); private static final SwerveModuleConstantsFactory< TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> @@ -240,9 +240,9 @@ public class DriveConstants { TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FRONT_LEFT = CONSTANT_CREATOR.createModuleConstants( - CANHD.frontLeftTurn, - CANHD.frontLeftDrive, - CANHD.frontLeftTurnAbsEncoder, + CANHD.FRONT_LEFT_TURN, + CANHD.FRONT_LEFT_DRIVE, + CANHD.FRONT_LEFT_TURN_ABS_ENC, Rotations.of(0), WHEEL_BASE.div(2.0), TRACK_WIDTH.div(2.0), @@ -253,9 +253,9 @@ public class DriveConstants { TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FRONT_RIGHT = CONSTANT_CREATOR.createModuleConstants( - CANHD.frontRightTurn, - CANHD.frontRightDrive, - CANHD.frontRightTurnAbsEncoder, + CANHD.FRONT_RIGHT_TURN, + CANHD.FRONT_RIGHT_DRIVE, + CANHD.FRONT_RIGHT_TURN_ABS_ENC, Rotations.of(0), WHEEL_BASE.div(2.0), TRACK_WIDTH.div(-2.0), @@ -266,9 +266,9 @@ public class DriveConstants { TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BACK_LEFT = CONSTANT_CREATOR.createModuleConstants( - CANHD.backLeftTurn, - CANHD.backLeftDrive, - CANHD.backLeftTurnAbsEncoder, + CANHD.BACK_LEFT_TURN, + CANHD.BACK_LEFT_DRIVE, + CANHD.BACK_LEFT_TURN_ABS_ENC, Rotations.of(0), WHEEL_BASE.div(-2.0), TRACK_WIDTH.div(2.0), @@ -279,9 +279,9 @@ public class DriveConstants { TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BACK_RIGHT = CONSTANT_CREATOR.createModuleConstants( - CANHD.backRightTurn, - CANHD.backRightDrive, - CANHD.backRightTurnAbsEncoder, + CANHD.BACK_RIGHT_TURN, + CANHD.BACK_RIGHT_DRIVE, + CANHD.BACK_RIGHT_TURN_ABS_ENC, Rotations.of(0), WHEEL_BASE.div(-2.0), TRACK_WIDTH.div(-2.0), diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOBoron.java b/src/main/java/frc/robot/subsystems/drive/GyroIOBoron.java index 9e55d9a7..fb10198e 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOBoron.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOBoron.java @@ -29,7 +29,7 @@ public class GyroIOBoron implements GyroIO { private final Queue yawPositionQueue; public GyroIOBoron() { - canandgyro = new Canandgyro(CAN2.gyro); + canandgyro = new Canandgyro(CAN2.GYRO); gyroInputs = CanandgyroThread.getInstance().registerCanandgyro(canandgyro); yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(canandgyro::getYaw); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 91dd77a9..e0c354c3 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -96,9 +96,9 @@ public ModuleIOTalonFX( SwerveModuleConstants constants) { this.constants = constants; - driveTalon = new TalonFX(constants.DriveMotorId, CANHD.bus); - turnTalon = new TalonFX(constants.SteerMotorId, CANHD.bus); - cancoder = new CANcoder(constants.EncoderId, CANHD.bus); + driveTalon = new TalonFX(constants.DriveMotorId, CANHD.BUS); + turnTalon = new TalonFX(constants.SteerMotorId, CANHD.BUS); + cancoder = new CANcoder(constants.EncoderId, CANHD.BUS); // Configure drive motor var driveConfig = constants.DriveMotorInitialConfigs; diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java index 728be50b..6df96f56 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java @@ -12,14 +12,14 @@ public final class FeederConstants { public static final class SpindexerConstants { // Geometry - public static final Distance radius = Inches.of(3.0); + public static final Distance RADIUS = Inches.of(3.0); // Motor public static final double MOTOR_REDUCTION = 1.0; public static final LinearVelocity MAX_TANGENTIAL_VELOCITY = MetersPerSecond.of( NEOVortexConstants.FREE_SPEED.in(RadiansPerSecond) - * radius.in(Meters) + * RADIUS.in(Meters) / MOTOR_REDUCTION); // Encoder @@ -34,14 +34,14 @@ public static final class SpindexerConstants { public static final class KickerConstants { // Geometry - public static final Distance radius = Inches.of(1.5); + public static final Distance RADIUS = Inches.of(1.5); // Motor public static final double MOTOR_REDUCTION = 5.0; public static final LinearVelocity MAX_TANGENTIAL_VELOCITY = MetersPerSecond.of( NEOVortexConstants.FREE_SPEED.in(RadiansPerSecond) - * radius.in(Meters) + * RADIUS.in(Meters) / MOTOR_REDUCTION); // Encoder diff --git a/src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java b/src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java index e9434eb3..abca1e5f 100644 --- a/src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java @@ -36,7 +36,7 @@ public class KickerIOSimSpark implements KickerIO { private final SparkFlexSim flexSim; public KickerIOSimSpark() { - flex = new SparkFlex(CAN2.kicker, MotorType.kBrushless); + flex = new SparkFlex(CAN2.KICKER, MotorType.kBrushless); controller = flex.getClosedLoopController(); var config = new SparkFlexConfig(); @@ -72,7 +72,7 @@ public void updateInputs(KickerIOInputs inputs) { // Update inputs inputs.connected = true; - inputs.velocityMetersPerSec = flexSim.getVelocity() * radius.in(Meters); + inputs.velocityMetersPerSec = flexSim.getVelocity() * RADIUS.in(Meters); inputs.appliedVolts = flexSim.getAppliedOutput() * flexSim.getBusVoltage(); inputs.currentAmps = Math.abs(flexSim.getMotorCurrent()); } @@ -89,7 +89,7 @@ public void setVelocity(LinearVelocity tangentialVelocity) { * tangentialVelocity.in(MetersPerSecond) / MAX_TANGENTIAL_VELOCITY.in(MetersPerSecond); controller.setSetpoint( - tangentialVelocity.in(MetersPerSecond) / radius.in(Meters), + tangentialVelocity.in(MetersPerSecond) / RADIUS.in(Meters), ControlType.kVelocity, ClosedLoopSlot.kSlot0, feedforwardVolts); diff --git a/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java b/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java index f88b7074..97b6dd73 100644 --- a/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java +++ b/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java @@ -32,7 +32,7 @@ public class KickerIOSpark implements KickerIO { private final SparkInputs sparkInputs; public KickerIOSpark() { - flex = new SparkFlex(CAN2.kicker, MotorType.kBrushless); + flex = new SparkFlex(CAN2.KICKER, MotorType.kBrushless); encoder = flex.getEncoder(); controller = flex.getClosedLoopController(); @@ -65,7 +65,7 @@ public KickerIOSpark() { public void updateInputs(KickerIOInputs inputs) { inputs.connected = sparkInputs.isConnected(); - inputs.velocityMetersPerSec = sparkInputs.getVelocity() * radius.in(Meters); + inputs.velocityMetersPerSec = sparkInputs.getVelocity() * RADIUS.in(Meters); inputs.appliedVolts = sparkInputs.getAppliedVolts(); inputs.currentAmps = sparkInputs.getOutputCurrent(); } @@ -83,7 +83,7 @@ public void setVelocity(LinearVelocity tangentialVelocity) { * tangentialVelocity.in(MetersPerSecond) / MAX_TANGENTIAL_VELOCITY.in(MetersPerSecond); controller.setSetpoint( - tangentialVelocity.in(MetersPerSecond) / radius.in(Meters), + tangentialVelocity.in(MetersPerSecond) / RADIUS.in(Meters), ControlType.kVelocity, ClosedLoopSlot.kSlot0, feedforwardVolts); diff --git a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java index bb3ef445..770ca0a0 100644 --- a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java @@ -36,7 +36,7 @@ public class SpindexerIOSimSpark implements SpindexerIO { private final SparkFlexSim flexSim; public SpindexerIOSimSpark() { - flex = new SparkFlex(CAN2.spindexer, MotorType.kBrushless); + flex = new SparkFlex(CAN2.SPINDEXER, MotorType.kBrushless); controller = flex.getClosedLoopController(); var config = new SparkFlexConfig(); @@ -73,7 +73,7 @@ public void updateInputs(SpindexerIOInputs inputs) { // Update inputs inputs.connected = true; - inputs.velocityMetersPerSec = flexSim.getVelocity() * radius.in(Meters); + inputs.velocityMetersPerSec = flexSim.getVelocity() * RADIUS.in(Meters); inputs.appliedVolts = flexSim.getAppliedOutput() * flexSim.getBusVoltage(); inputs.currentAmps = Math.abs(flexSim.getMotorCurrent()); } @@ -90,7 +90,7 @@ public void setVelocity(LinearVelocity tangentialVelocity) { * tangentialVelocity.in(MetersPerSecond) / MAX_TANGENTIAL_VELOCITY.in(MetersPerSecond); controller.setSetpoint( - tangentialVelocity.in(MetersPerSecond) / radius.in(Meters), + tangentialVelocity.in(MetersPerSecond) / RADIUS.in(Meters), ControlType.kVelocity, ClosedLoopSlot.kSlot0, feedforwardVolts); diff --git a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java index 1a166059..a8dae22a 100644 --- a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java +++ b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java @@ -32,7 +32,7 @@ public class SpindexerIOSpark implements SpindexerIO { private final SparkInputs sparkInputs; public SpindexerIOSpark() { - flex = new SparkFlex(CAN2.spindexer, MotorType.kBrushless); + flex = new SparkFlex(CAN2.SPINDEXER, MotorType.kBrushless); encoder = flex.getEncoder(); controller = flex.getClosedLoopController(); @@ -65,7 +65,7 @@ public SpindexerIOSpark() { public void updateInputs(SpindexerIOInputs inputs) { inputs.connected = sparkInputs.isConnected(); - inputs.velocityMetersPerSec = sparkInputs.getVelocity() * radius.in(Meters); + inputs.velocityMetersPerSec = sparkInputs.getVelocity() * RADIUS.in(Meters); inputs.appliedVolts = sparkInputs.getAppliedVolts(); inputs.currentAmps = sparkInputs.getOutputCurrent(); } @@ -83,7 +83,7 @@ public void setVelocity(LinearVelocity tangentialVelocity) { * tangentialVelocity.in(MetersPerSecond) / MAX_TANGENTIAL_VELOCITY.in(MetersPerSecond); controller.setSetpoint( - tangentialVelocity.in(MetersPerSecond) / radius.in(Meters), + tangentialVelocity.in(MetersPerSecond) / RADIUS.in(Meters), ControlType.kVelocity, ClosedLoopSlot.kSlot0, feedforwardVolts); diff --git a/src/main/java/frc/robot/subsystems/hopper/HopperIOReal.java b/src/main/java/frc/robot/subsystems/hopper/HopperIOReal.java index b867dc4c..1e3951ba 100644 --- a/src/main/java/frc/robot/subsystems/hopper/HopperIOReal.java +++ b/src/main/java/frc/robot/subsystems/hopper/HopperIOReal.java @@ -10,7 +10,7 @@ public class HopperIOReal implements HopperIO { public final DoubleSolenoid hopperPneumatic; public HopperIOReal() { - hopperPneumatic = new DoubleSolenoid(PneumaticsModuleType.REVPH, hopperForward, hopperReverse); + hopperPneumatic = new DoubleSolenoid(PneumaticsModuleType.REVPH, HOPPER_FWD, HOPPER_REV); } @Override diff --git a/src/main/java/frc/robot/subsystems/hopper/HopperIOSim.java b/src/main/java/frc/robot/subsystems/hopper/HopperIOSim.java index eedcf1da..8f29c5fb 100644 --- a/src/main/java/frc/robot/subsystems/hopper/HopperIOSim.java +++ b/src/main/java/frc/robot/subsystems/hopper/HopperIOSim.java @@ -10,8 +10,7 @@ public class HopperIOSim implements HopperIO { public final DoubleSolenoidSim hopperPneumatic; public HopperIOSim() { - hopperPneumatic = - new DoubleSolenoidSim(PneumaticsModuleType.REVPH, hopperForward, hopperReverse); + hopperPneumatic = new DoubleSolenoidSim(PneumaticsModuleType.REVPH, HOPPER_FWD, HOPPER_REV); } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOReal.java index 4aeb79c9..6cfdefb2 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOReal.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOReal.java @@ -11,7 +11,7 @@ public class IntakeArmIOReal implements IntakeArmIO { public IntakeArmIOReal() { intakeArmPneumatic = - new DoubleSolenoid(PneumaticsModuleType.REVPH, intakeArmForward, intakeArmReverse); + new DoubleSolenoid(PneumaticsModuleType.REVPH, INTAKE_ARM_FWD, INTAKE_ARM_REV); } @Override diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSim.java index 278ef7f2..f7bebbdc 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSim.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeArmIOSim.java @@ -11,7 +11,7 @@ public class IntakeArmIOSim implements IntakeArmIO { public IntakeArmIOSim() { intakeArmPneumatic = - new DoubleSolenoidSim(PneumaticsModuleType.REVPH, intakeArmForward, intakeArmReverse); + new DoubleSolenoidSim(PneumaticsModuleType.REVPH, INTAKE_ARM_FWD, INTAKE_ARM_REV); intakeArmPneumatic.set(kReverse); } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index c707bbbc..81643725 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -3,6 +3,8 @@ import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.Slot1Configs; import edu.wpi.first.units.measure.Distance; import frc.robot.Constants.CANBusPorts.CAN2; @@ -11,11 +13,10 @@ public class IntakeConstants { public static final double kInterlockSettleSeconds = 1.0; public static class RollerConstants { - public static final Distance rollerRadius = Inches.of(0.85); + public static final Distance RADIUS = Inches.of(0.85); // motor controller public static final double MOTOR_REDUCTION = 1.0; - public static final int numMotors = 1; public static final double MAX_ACCELERATION = 4000.0; public static final double MAX_JERK = 40000.0; @@ -24,11 +25,21 @@ public static class RollerConstants { public static final double ENCODER_VELOCITY_FACTOR = ENCODER_POSITION_FACTOR / 60.0; // Meters/sec + // Spark gains + public static final double kP = 0.001; + public static final double kD = 0.0; + + // Talon gains + public static final Slot0Configs VELOCITY_VOLTAGE_GAINS = + new Slot0Configs().withKP(0.11).withKI(0.0).withKD(0.0).withKS(0.1).withKV(0.12); + public static final Slot1Configs VELOCITY_TORQUE_CURRENT_GAINS = + new Slot1Configs().withKP(0.11).withKI(0.0).withKD(0.0).withKS(2.5); + // configs - public static final RollerConfig upperRollerConfig = - new RollerConfig(CAN2.intakeRollerUpper, CAN2.bus, true); - public static final RollerConfig lowerRollerConfig = - new RollerConfig(CAN2.intakeRollerLower, CAN2.bus, true); + public static final RollerConfig UPPER_ROLLER_CONFIG = + new RollerConfig(CAN2.INTAKE_ROLLER_UPPER, CAN2.BUS, true); + public static final RollerConfig LOWER_ROLLER_CONFIG = + new RollerConfig(CAN2.INTAKE_ROLLER_LOWER, CAN2.BUS, true); } public static class RollerConfig { diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java index d148055e..5cbc600f 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java @@ -27,14 +27,10 @@ public class RollerIOSimSpark implements RollerIO { private static final double KICKER_MOI_KG_M2 = 0.00052; - private static final double KP = 0.11; - private static final double KD = 0.0; private static final LinearVelocity MAX_TANGENTIAL_VELOCITY = MetersPerSecond.of( - NEOVortexConstants.FREE_SPEED.in(RadiansPerSecond) - * rollerRadius.in(Meters) - / MOTOR_REDUCTION); - private static final DCMotor GEARBOX = DCMotor.getNeoVortex(numMotors); + NEOVortexConstants.FREE_SPEED.in(RadiansPerSecond) * RADIUS.in(Meters) / MOTOR_REDUCTION); + private static final DCMotor GEARBOX = DCMotor.getNeoVortex(1); private final DCMotorSim rollerSim; @@ -58,7 +54,7 @@ public RollerIOSimSpark(RollerConfig rollerConfig) { .positionConversionFactor(ENCODER_POSITION_FACTOR) .velocityConversionFactor(ENCODER_VELOCITY_FACTOR); - config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(KP, 0.0, KD); + config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kP, 0.0, kD); flex.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); flexSim = new SparkFlexSim(flex, GEARBOX); @@ -79,7 +75,7 @@ public void updateInputs(RollerIOInputs inputs) { // Update inputs inputs.connected = true; - inputs.velocityMetersPerSec = flexSim.getVelocity() * rollerRadius.in(Meters); + inputs.velocityMetersPerSec = flexSim.getVelocity() * RADIUS.in(Meters); inputs.appliedVolts = flexSim.getAppliedOutput() * flexSim.getBusVoltage(); inputs.currentAmps = Math.abs(flexSim.getMotorCurrent()); } @@ -96,7 +92,7 @@ public void setVelocity(LinearVelocity tangentialVelocity) { * tangentialVelocity.in(MetersPerSecond) / MAX_TANGENTIAL_VELOCITY.in(MetersPerSecond); controller.setSetpoint( - tangentialVelocity.in(MetersPerSecond) / rollerRadius.in(Meters), + tangentialVelocity.in(MetersPerSecond) / RADIUS.in(Meters), ControlType.kVelocity, ClosedLoopSlot.kSlot0, feedforwardVolts); diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java index af2b2a89..f53d250e 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java @@ -6,8 +6,6 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.Slot1Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; @@ -29,13 +27,7 @@ import frc.robot.subsystems.intake.IntakeConstants.RollerConfig; public class RollerIOSimTalonFX implements RollerIO { - private static final double KP = 0.11; - private static final double KD = 0.0; - private static final Slot0Configs VELOCITY_VOLTAGE_GAINS = - new Slot0Configs().withKP(KP).withKI(0.0).withKD(KD).withKS(0.1).withKV(0.12); - private static final Slot1Configs VELOCITY_TORQUE_CURRENT_GAINS = - new Slot1Configs().withKP(KP).withKI(0.0).withKD(KD).withKS(2.5); - private static final DCMotor GEARBOX = DCMotor.getKrakenX60(numMotors); + private static final DCMotor GEARBOX = DCMotor.getKrakenX60(1); private final DCMotorSim rollerSim; @@ -101,7 +93,7 @@ public void updateInputs(RollerIOInputs inputs) { inputs.appliedVolts = appliedVolts.getValueAsDouble(); inputs.currentAmps = supplyCurrent.getValueAsDouble(); inputs.velocityMetersPerSec = - velocity.getValue().in(RadiansPerSecond) * rollerRadius.in(Meters) / MOTOR_REDUCTION; + velocity.getValue().in(RadiansPerSecond) * RADIUS.in(Meters) / MOTOR_REDUCTION; } @Override @@ -117,7 +109,7 @@ public void setOpenLoop(Voltage volts) { public void setVelocity(LinearVelocity tangentialVelocity) { AngularVelocity angularVelocity = RadiansPerSecond.of( - tangentialVelocity.in(MetersPerSecond) * MOTOR_REDUCTION / rollerRadius.in(Meters)); + tangentialVelocity.in(MetersPerSecond) * MOTOR_REDUCTION / RADIUS.in(Meters)); motor.setControl(velocityTorqueCurrentRequest.withVelocity(angularVelocity)); } } diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java index bda11365..1e5559ce 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java @@ -24,13 +24,9 @@ import frc.robot.util.SparkOdometryThread.SparkInputs; public class RollerIOSpark implements RollerIO { - private static final double KP = 0.001; - private static final double KD = 0.0; private static final LinearVelocity MAX_TANGENTIAL_VELOCITY = MetersPerSecond.of( - NEOVortexConstants.FREE_SPEED.in(RadiansPerSecond) - * rollerRadius.in(Meters) - / MOTOR_REDUCTION); + NEOVortexConstants.FREE_SPEED.in(RadiansPerSecond) * RADIUS.in(Meters) / MOTOR_REDUCTION); private final SparkFlex flex; private final RelativeEncoder encoder; @@ -56,7 +52,7 @@ public RollerIOSpark(RollerConfig rollerConfig) { .uvwAverageDepth(2) .uvwMeasurementPeriod(8); - config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(KP, 0.0, KD); + config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder).pid(kP, 0.0, kD); tryUntilOk( flex, @@ -70,7 +66,7 @@ public RollerIOSpark(RollerConfig rollerConfig) { @Override public void updateInputs(RollerIOInputs inputs) { inputs.connected = sparkInputs.isConnected(); - inputs.velocityMetersPerSec = sparkInputs.getVelocity() * rollerRadius.in(Meters); + inputs.velocityMetersPerSec = sparkInputs.getVelocity() * RADIUS.in(Meters); inputs.appliedVolts = sparkInputs.getAppliedVolts(); inputs.currentAmps = sparkInputs.getOutputCurrent(); } @@ -88,7 +84,7 @@ public void setVelocity(LinearVelocity tangentialVelocity) { * tangentialVelocity.in(MetersPerSecond) / MAX_TANGENTIAL_VELOCITY.in(MetersPerSecond); controller.setSetpoint( - tangentialVelocity.in(MetersPerSecond) / rollerRadius.in(Meters), + tangentialVelocity.in(MetersPerSecond) / RADIUS.in(Meters), ControlType.kVelocity, ClosedLoopSlot.kSlot0, feedforwardVolts); diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java index 328804cc..f8944769 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java @@ -6,8 +6,6 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.Slot1Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; @@ -25,13 +23,6 @@ import frc.robot.subsystems.intake.IntakeConstants.RollerConfig; public class RollerIOTalonFX implements RollerIO { - private static final double KP = 0.11; - private static final double KD = 0.0; - private static final Slot0Configs VELOCITY_VOLTAGE_GAINS = - new Slot0Configs().withKP(KP).withKI(0.0).withKD(KD).withKS(0.1).withKV(0.12); - private static final Slot1Configs VELOCITY_TORQUE_CURRENT_GAINS = - new Slot1Configs().withKP(KP).withKI(0.0).withKD(KD).withKS(2.5); - private final TalonFX motor; private final TalonFXConfiguration config; private final Debouncer connectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); @@ -86,7 +77,7 @@ public void updateInputs(RollerIOInputs inputs) { inputs.appliedVolts = appliedVolts.getValueAsDouble(); inputs.currentAmps = supplyCurrent.getValueAsDouble(); inputs.velocityMetersPerSec = - velocity.getValue().in(RadiansPerSecond) * rollerRadius.in(Meters) / MOTOR_REDUCTION; + velocity.getValue().in(RadiansPerSecond) * RADIUS.in(Meters) / MOTOR_REDUCTION; } @Override @@ -102,7 +93,7 @@ public void setOpenLoop(Voltage volts) { public void setVelocity(LinearVelocity tangentialVelocity) { AngularVelocity angularVelocity = RadiansPerSecond.of( - tangentialVelocity.in(MetersPerSecond) * MOTOR_REDUCTION / rollerRadius.in(Meters)); + tangentialVelocity.in(MetersPerSecond) * MOTOR_REDUCTION / RADIUS.in(Meters)); // TrapezoidProfile.State goal = // new TrapezoidProfile.State(angularVelocity.in(RotationsPerSecond), 0); diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java index 5cceafd6..7395f076 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java @@ -50,8 +50,8 @@ public class FlywheelIOSimTalonFX implements FlywheelIO { private final StatusSignal flywheelCurrent; public FlywheelIOSimTalonFX() { - flywheelLeaderTalon = new TalonFX(CAN2.flywheelLeader, CAN2.bus); - flywheelFollowerTalon = new TalonFX(CAN2.flywheelFollower, CAN2.bus); + flywheelLeaderTalon = new TalonFX(CAN2.FLYWHEEL_LEADER, CAN2.BUS); + flywheelFollowerTalon = new TalonFX(CAN2.FLYWHEEL_FOLLOWER, CAN2.BUS); // Configuration config = new TalonFXConfiguration(); config.MotorOutput.withInverted(InvertedValue.CounterClockwise_Positive) @@ -78,7 +78,7 @@ public FlywheelIOSimTalonFX() { 50.0, flywheelVelocity, flywheelAppliedVolts, flywheelCurrent); flywheelFollowerTalon.setControl( - new Follower(CAN2.flywheelLeader, MotorAlignmentValue.Opposed)); + new Follower(CAN2.FLYWHEEL_LEADER, MotorAlignmentValue.Opposed)); } @Override diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java index ebeb3742..d2cc3d30 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOTalonFX.java @@ -48,8 +48,8 @@ public class FlywheelIOTalonFX implements FlywheelIO { private final StatusSignal flywheelCurrent, followerCurrent; public FlywheelIOTalonFX() { - flywheelLeaderTalon = new TalonFX(CAN2.flywheelLeader, CAN2.bus); - flywheelFollowerTalon = new TalonFX(CAN2.flywheelFollower, CAN2.bus); + flywheelLeaderTalon = new TalonFX(CAN2.FLYWHEEL_LEADER, CAN2.BUS); + flywheelFollowerTalon = new TalonFX(CAN2.FLYWHEEL_FOLLOWER, CAN2.BUS); // Configuration config = new TalonFXConfiguration(); config.MotorOutput.withNeutralMode(NeutralModeValue.Brake) @@ -87,7 +87,7 @@ public FlywheelIOTalonFX() { // configurations require certain status signals for proper follower behavior flywheelFollowerTalon.setControl( - new Follower(CAN2.flywheelLeader, MotorAlignmentValue.Opposed)); + new Follower(CAN2.FLYWHEEL_LEADER, MotorAlignmentValue.Opposed)); } @Override diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSimSpark.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSimSpark.java index 31dbcaec..12cd1fec 100644 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIOSimSpark.java @@ -37,7 +37,7 @@ public class HoodIOSimSpark implements HoodIO { private final SparkMaxConfig hoodConfig; public HoodIOSimSpark() { - max = new SparkMax(CAN2.hood, MotorType.kBrushless); + max = new SparkMax(CAN2.HOOD, MotorType.kBrushless); controller = max.getClosedLoopController(); hoodConfig = new SparkMaxConfig(); diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java index c4e0f9cc..807fd941 100644 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java @@ -38,7 +38,7 @@ public class HoodIOSpark implements HoodIO { private final Debouncer connectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); public HoodIOSpark() { - hoodSpark = new SparkMax(CAN2.hood, MotorType.kBrushless); + hoodSpark = new SparkMax(CAN2.HOOD, MotorType.kBrushless); encoderSpark = hoodSpark.getEncoder(); hoodController = hoodSpark.getClosedLoopController(); diff --git a/src/main/java/frc/robot/subsystems/launcher/Launcher.java b/src/main/java/frc/robot/subsystems/launcher/Launcher.java index 4dbf29e3..99ea78dd 100644 --- a/src/main/java/frc/robot/subsystems/launcher/Launcher.java +++ b/src/main/java/frc/robot/subsystems/launcher/Launcher.java @@ -376,12 +376,12 @@ private Translation3d getV0Nominal(Translation3d d, Rotation2d impactAngle, Stri Logger.recordOutput("Launcher/" + key + "/Reachable", false); return v0nominalLast; } - double v_0r = dr * Math.sqrt(G / denominator); + double v_0r = dr * Math.sqrt(GRAVITY / denominator); if (!(v_0r >= 1e-6)) { Logger.recordOutput("Launcher/" + key + "/Reachable", false); return v0nominalLast; } - double v_0z = (G * dr) / v_0r - v_0r * impactAngle.getTan(); + double v_0z = (GRAVITY * dr) / v_0r - v_0r * impactAngle.getTan(); double v_0x = v_0r * d.toTranslation2d().getAngle().getCos(); double v_0y = v_0r * d.toTranslation2d().getAngle().getSin(); @@ -409,7 +409,7 @@ private Translation3d getV0Replanned(Translation3d d, double v_flywheel, String double rHatY = d.getY() / dr; double v_sq = v_flywheel * v_flywheel; - double discriminant = v_sq * v_sq - G * (G * dr * dr + 2 * dz * v_sq); + double discriminant = v_sq * v_sq - GRAVITY * (GRAVITY * dr * dr + 2 * dz * v_sq); // Guard: discriminant < 0 means target is beyond maximum range for current flywheel speed. // Using < 1e-6 threshold adds safety margin against sqrt of tiny negative values from @@ -421,12 +421,12 @@ private Translation3d getV0Replanned(Translation3d d, double v_flywheel, String } // High-arc solution (lower trajectory would use v_sq - sqrt(discriminant)) - double tanTheta = (v_sq + Math.sqrt(discriminant)) / (G * dr); + double tanTheta = (v_sq + Math.sqrt(discriminant)) / (GRAVITY * dr); // sin(2*atan(x)) = 2x/(1+x²) - avoids two trig function calls Logger.recordOutput( "Launcher/" + key + "/PredictedRange", - (v_sq * 2.0 * tanTheta) / (G * (1.0 + tanTheta * tanTheta))); + (v_sq * 2.0 * tanTheta) / (GRAVITY * (1.0 + tanTheta * tanTheta))); // Effective velocity available for ballistics double v_r = v_flywheel / Math.sqrt(1 + tanTheta * tanTheta); @@ -489,7 +489,7 @@ private void log(Translation3d d, Translation3d v, String key) { Logger.recordOutput("Launcher/" + key + "/TravelTime", dr / v_r); } - var max_height = turretBasePose.getZ() + v_z * v_z / (2 * G); + var max_height = turretBasePose.getZ() + v_z * v_z / (2 * GRAVITY); Logger.recordOutput("Launcher/" + key + "/MaxHeight", max_height); boolean clearsCeiling = Meters.of(max_height).plus(FUEL_RADIUS).lt(CEILING_HEIGHT); @@ -522,7 +522,7 @@ private void updateBallisticsSim( for (int i = traj.size() - 1; i >= 0; i--) { BallisticObject o = traj.get(i); - o.vz -= G * dt; + o.vz -= GRAVITY * dt; o.px += o.vx * dt; o.py += o.vy * dt; o.pz += o.vz * dt; diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index 6cc3255d..b73ee43c 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -21,7 +21,7 @@ public final class LauncherConstants { // Geometry public static final Distance FUEL_RADIUS = Inches.of(3); public static final Distance CEILING_HEIGHT = Feet.of(11).plus(Inches.of(2)); - public static final double G = 9.81; + public static final double GRAVITY = 9.81; // Distance-based impact angle: steeper at close range, shallower at far range public static final Distance IMPACT_ANGLE_CLOSE_DISTANCE = Meters.of(2.0); diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java index 58b5610a..de4010ca 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java @@ -41,7 +41,7 @@ public class TurretIOSimSpark implements TurretIO { private double oversaturationLessMargin = 0.0; public TurretIOSimSpark() { - turnSpark = new SparkMax(CAN2.turret, MotorType.kBrushless); + turnSpark = new SparkMax(CAN2.TURRET, MotorType.kBrushless); controller = turnSpark.getClosedLoopController(); var turnConfig = new SparkMaxConfig(); diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java index cebf75f4..dfeb8f4b 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java @@ -47,12 +47,12 @@ public class TurretIOSpark implements TurretIO { private double oversaturationLessMargin = 0.0; public TurretIOSpark() { - turnSpark = new SparkMax(CAN2.turret, MotorType.kBrushless); + turnSpark = new SparkMax(CAN2.TURRET, MotorType.kBrushless); controller = turnSpark.getClosedLoopController(); turnSparkEncoder = turnSpark.getEncoder(); absoluteEncoder = new DutyCycleEncoder( - new DigitalInput(DIOPorts.turretAbsEncoder), + new DigitalInput(DIOPorts.TURRET_ABS_ENCODER), 2 * Math.PI, ABS_ENCODER_OFFSET.getRadians() + MECHANISM_OFFSET.getRadians()); diff --git a/src/main/java/frc/robot/subsystems/leds/LEDConstants.java b/src/main/java/frc/robot/subsystems/leds/LEDConstants.java index 8c2286a7..6afb4bfa 100644 --- a/src/main/java/frc/robot/subsystems/leds/LEDConstants.java +++ b/src/main/java/frc/robot/subsystems/leds/LEDConstants.java @@ -10,17 +10,17 @@ private LEDConstants() {} // ==================== ANIMATION CONFIGURATION ==================== - public static final int kLEDsPerBlock = 1; - public static final int kLEDsBetweenBlocks = 1; + public static final int LEDS_PER_BLOCK = 1; + public static final int LEDS_BETWEEN_BLOCKS = 1; // ==================== POSE SEEK TOLERANCES ==================== /** Heading tolerance in degrees for pose-seek feedback. */ - public static final double kPoseSeekHeadingToleranceDegrees = 3.0; + public static final double POSE_SEEK_HEADING_TOL_DEGREES = 3.0; /** X position tolerance in centimeters for pose-seek feedback. */ - public static final double kPoseSeekXToleranceCm = 5.0; + public static final double POSE_SEEK_X_TOL_CM = 5.0; /** Y position tolerance in centimeters for pose-seek feedback. */ - public static final double kPoseSeekYToleranceCm = 6.0; + public static final double POSE_SEEK_Y_TOL_CM = 6.0; } diff --git a/src/main/java/frc/robot/subsystems/leds/LEDController.java b/src/main/java/frc/robot/subsystems/leds/LEDController.java index 0e102dac..e81d9823 100644 --- a/src/main/java/frc/robot/subsystems/leds/LEDController.java +++ b/src/main/java/frc/robot/subsystems/leds/LEDController.java @@ -75,7 +75,7 @@ public void displayPoseSeek(Pose2d currentPose, Pose2d targetPose) { // X feedback on center LEDs (robot-relative: positive = forward) var x = robotRelativeDelta.getMeasureX().in(Centimeters); - if (Math.abs(x) < LEDConstants.kPoseSeekXToleranceCm) { + if (Math.abs(x) < LEDConstants.POSE_SEEK_X_TOL_CM) { LEDSeries.POSE_X.applyPattern(solidWhitePattern); } else if (x > 0) { // Need to move forward @@ -87,7 +87,7 @@ public void displayPoseSeek(Pose2d currentPose, Pose2d targetPose) { // Heading feedback on rotation LEDs (angular error is frame-independent) var theta = MathUtil.inputModulus(delta.getRotation().getDegrees(), -180, 180); - if (Math.abs(theta) < LEDConstants.kPoseSeekHeadingToleranceDegrees) { + if (Math.abs(theta) < LEDConstants.POSE_SEEK_HEADING_TOL_DEGREES) { LEDSeries.POSE_ROTATION.applyPattern(solidWhitePattern); } else if (theta > 0) { // Need to rotate CCW @@ -101,7 +101,7 @@ public void displayPoseSeek(Pose2d currentPose, Pose2d targetPose) { // Y feedback on end LEDs (robot-relative: positive = move left) var y = robotRelativeDelta.getMeasureY().in(Centimeters); - if (Math.abs(y) < LEDConstants.kPoseSeekYToleranceCm) { + if (Math.abs(y) < LEDConstants.POSE_SEEK_Y_TOL_CM) { LEDSeries.POSE_Y.applyPattern(solidWhitePattern); } else if (y > 0) { // Need to move left @@ -145,8 +145,8 @@ public void displayPoseSeek(Pose2d currentPose, Pose2d targetPose) { LEDCustomPattern.countingBlocks( () -> Robot.autoSelector.get().get().getOptionNumber(), () -> Robot.autoSelector.get().get().getAllianceColor(), - LEDConstants.kLEDsPerBlock, - LEDConstants.kLEDsBetweenBlocks); + LEDConstants.LEDS_PER_BLOCK, + LEDConstants.LEDS_BETWEEN_BLOCKS); /** * Pattern displaying a progress bar for the current match phase. Shows remaining time as a diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index da291d45..579e0e46 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -134,7 +134,7 @@ public void periodic() { // Log inputs via AdvantageKit (throttled - serialization is expensive) // Note: Throttling reduces CPU load but loses data granularity for replay - if (loopCounter % kLoggingDivisor == 0) { + if (loopCounter % LOGGING_DIVISOR == 0) { for (int i = 0; i < io.length; i++) { Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]); } @@ -184,7 +184,7 @@ public void periodic() { // Add pose to log robotPoses.add(observation.pose()); - if (tested.score() > minScore) { + if (tested.score() > MIN_SCORE) { robotPosesAccepted.add(observation.pose()); } else { robotPosesRejected.add(observation.pose()); @@ -194,7 +194,7 @@ public void periodic() { } // Log camera datadata - if (kLogIndividualCameraPoses) { + if (LOG_INDIVIDUAL_CAMERA_POSES) { Logger.recordOutput( "Vision/Camera" + Integer.toString(cameraIndex) + "/TagPoses", tagPoses.toArray(new Pose3d[tagPoses.size()])); @@ -221,9 +221,9 @@ public void periodic() { // Process observations in batches every processingIntervalLoops // This allows cameras to accumulate observations before fusion decides what agrees - if (loopCounter % processingIntervalLoops == 0) { + if (loopCounter % PROCESSING_INTERVAL_LOOPS == 0) { // Remove unacceptable observations before fusion - observationBuffer.removeIf(o -> o.score() < minScore); + observationBuffer.removeIf(o -> o.score() < MIN_SCORE); // Fuse correlated observations from multiple cameras into averaged poses // This reduces jitter from multiple cameras reporting slightly different poses @@ -237,9 +237,10 @@ public void periodic() { // Calculate standard deviations // Single-camera observations get much higher stddev (less trust) to reduce jitter // Multi-camera fused observations get lower stddev (more trust) - double cameraCountFactor = (fused.cameraCount() == 1) ? singleCameraStdDevMultiplier : 1.0; - double linearStdDev = linearStdDevBaseline * cameraCountFactor / fused.score(); - double angularStdDev = angularStdDevBaseline * cameraCountFactor / fused.score(); + double cameraCountFactor = + (fused.cameraCount() == 1) ? SINGLE_CAMERA_STD_DEV_MULTIPLIER : 1.0; + double linearStdDev = LINEAR_STD_DEV_BASELINE * cameraCountFactor / fused.score(); + double angularStdDev = ANGULAR_STD_DEV_BASELINE * cameraCountFactor / fused.score(); // Send fused vision observation to the pose estimator consumer.accept( @@ -257,16 +258,16 @@ public void periodic() { long t4 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Log summary data (throttled along with processInputs) - if (loopCounter % kLoggingDivisor == 0) { - if (kLogSummaryPoses) { + if (loopCounter % LOGGING_DIVISOR == 0) { + if (LOG_SUMMARY_POSES) { Logger.recordOutput("Vision/Summary/TagPoses", allTagPoses.toArray(Pose3d[]::new)); Logger.recordOutput("Vision/Summary/RobotPoses", allRobotPoses.toArray(Pose3d[]::new)); } - if (kLogAcceptedPoses) { + if (LOG_ACCEPTED_POSES) { Logger.recordOutput( "Vision/Summary/RobotPosesAccepted", allRobotPosesAccepted.toArray(Pose3d[]::new)); } - if (kLogRejectedPoses) { + if (LOG_REJECTED_POSES) { Logger.recordOutput( "Vision/Summary/RobotPosesRejected", allRobotPosesRejected.toArray(Pose3d[]::new)); } @@ -310,16 +311,16 @@ public void accept( public static synchronized AprilTagFieldLayout getAprilTagLayout() { if (cachedLayout == null) { // Try to load custom layout only if requested and not connected to FMS - if (useCustomAprilTagLayout && !DriverStation.isFMSAttached()) { + if (USE_CUSTOM_APRIL_TAG_LAYOUT && !DriverStation.isFMSAttached()) { try { - cachedLayout = new AprilTagFieldLayout(customAprilTagLayoutPath); + cachedLayout = new AprilTagFieldLayout(CUSTOM_APRIL_TAG_LAYOUT_PATH); } catch (IOException e) { System.err.println("Error loading custom AprilTag layout: " + e.getMessage()); } } // Otherwise load default layout if (cachedLayout == null) { - cachedLayout = AprilTagFieldLayout.loadField(defaultAprilTagFieldLayout); + cachedLayout = AprilTagFieldLayout.loadField(DEFAULT_APRIL_TAG_FIELD_LAYOUT); } } return cachedLayout; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 3787bd2e..b53bda1e 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -20,40 +20,40 @@ public class VisionConstants { - public static String customAprilTagLayoutPath = + public static String CUSTOM_APRIL_TAG_LAYOUT_PATH = Filesystem.getDeployDirectory() + "/stemgym-2026.json"; - public static Boolean useCustomAprilTagLayout = false; - public static AprilTagFields defaultAprilTagFieldLayout = AprilTagFields.k2026RebuiltAndymark; + public static Boolean USE_CUSTOM_APRIL_TAG_LAYOUT = false; + public static AprilTagFields DEFAULT_APRIL_TAG_FIELD_LAYOUT = AprilTagFields.k2026RebuiltAndymark; // Camera names, must match names configured on coprocessor - public static String cameraFrontRightName = "OV2311_TH_2026_FR"; - public static String cameraFrontLeftName = "OV2311_TH_2026_FL"; - public static String cameraBackRightName = "OV2311_TH_2026_RR"; - public static String cameraBackLeftName = "OV2311_TH_2026_RL"; + public static String CAMERA_FRONT_RIGHT_NAME = "OV2311_TH_2026_FR"; + public static String CAMERA_FRONT_LEFT_NAME = "OV2311_TH_2026_FL"; + public static String CAMERA_BACK_RIGHT_NAME = "OV2311_TH_2026_RR"; + public static String CAMERA_BACK_LEFT_NAME = "OV2311_TH_2026_RL"; // Robot to camera transforms - public static Transform3d robotToFrontRightCamera = + public static Transform3d ROBOT_TO_FRONT_RIGHT_CAMERA = new Transform3d( Inches.of(-10.572), Inches.of(-12.337), Inches.of(16.688), // pitch 20 degrees up, yaw 55 degrees right new Rotation3d(new Quaternion(0.8735, -0.0802, -0.1540, -0.4547))); - public static Transform3d robotToFrontLeftCamera = + public static Transform3d ROBOT_TO_FRONT_LEFT_CAMERA = new Transform3d( Inches.of(-10.572), Inches.of(12.337), Inches.of(16.688), // pitch 20 degrees up, yaw 55 degrees left new Rotation3d(new Quaternion(0.8735, 0.0802, -0.1540, 0.4547))); - public static Transform3d robotToBackRightCamera = + public static Transform3d ROBOT_TO_BACK_RIGHT_CAMERA = new Transform3d( Inches.of(-13.1623), Inches.of(-12.1623), Inches.of(20.26674), // pitch 15 degrees up, yaw 135 degrees right new Rotation3d(new Quaternion(-0.3794, 0.1206, 0.0500, 0.9160))); - public static Transform3d robotToBackLeftCamera = + public static Transform3d ROBOT_TO_BACK_LEFT_CAMERA = new Transform3d( Inches.of(-13.1623), Inches.of(12.1623), @@ -61,40 +61,41 @@ public class VisionConstants { // pitch 15 degrees up, yaw 135 degrees left new Rotation3d(new Quaternion(0.3794, 0.1206, -0.0500, 0.9160))); - public static Distance minRobotWidth = Inches.of(36.875); + public static Distance MIN_ROBOT_WIDTH = Inches.of(36.875); // Pose filtering thresholds - public static double ambiguityTolerance = 0.15; - public static Distance tagDistanceTolerance = Meters.of(4.0); - public static final double tagDistanceToleranceMeters = tagDistanceTolerance.in(Meters); + public static double AMBIGUITY_TOLERANCE = 0.15; + public static Distance TAG_DISTANCE_TOLERANCE = Meters.of(4.0); + public static final double TAG_DISTANCE_TOLERANCE_METERS = TAG_DISTANCE_TOLERANCE.in(Meters); - public static Distance elevationTolerance = Meters.of(0.25); - public static final double elevationToleranceMeters = elevationTolerance.in(Meters); - public static Angle rollTolerance = Degrees.of(5); - public static final double rollToleranceRadians = rollTolerance.in(Radians); - public static Angle pitchTolerance = Degrees.of(5); - public static final double pitchToleranceRadians = pitchTolerance.in(Radians); + public static Distance ELEVATION_TOLERANCE = Meters.of(0.25); + public static final double ELEVATION_TOLERANCE_METERS = ELEVATION_TOLERANCE.in(Meters); + public static Angle ROLL_TOLERANCE = Degrees.of(5); + public static final double ROLL_TOLERANCE_RADIANS = ROLL_TOLERANCE.in(Radians); + public static Angle PITCH_TOLERANCE = Degrees.of(5); + public static final double PITCH_TOLERANCE_RADIANS = PITCH_TOLERANCE.in(Radians); // Yaw consistency threshold - how much vision yaw can differ from gyro yaw // This catches ambiguous PnP solutions which almost always have incorrect yaw. // Set to 10 degrees to allow for minor calibration differences while rejecting // bad PnP solutions that typically have 30-90+ degree yaw errors. - public static Angle yawTolerance = Degrees.of(10); - public static final double yawToleranceRadians = yawTolerance.in(Radians); + public static Angle YAW_TOLERANCE = Degrees.of(10); + public static final double YAW_TOLERANCE_RADIANS = YAW_TOLERANCE.in(Radians); // Cached values for arena boundary calculations - public static final double minRobotWidthHalfMeters = minRobotWidth.div(2.0).in(Meters); - public static final double fieldXLenMeters = frc.game.Field.field_x_len.in(Meters); - public static final double fieldYLenMeters = frc.game.Field.field_y_len.in(Meters); + public static final double MIN_ROBOT_WIDTH_HALF_METERS = MIN_ROBOT_WIDTH.div(2.0).in(Meters); + public static final double FIELD_X_LEN_METERS = frc.game.Field.field_x_len.in(Meters); + public static final double FIELD_Y_LEN_METERS = frc.game.Field.field_y_len.in(Meters); // Velocity consistency check thresholds - public static double velocityCheckTimeoutSeconds = + public static double VELOCITY_CHECK_TIMEOUT_SECONDS = 0.5; // Skip check if no observation for this long - public static final double maxReasonableVelocityMps = + public static final double MAX_REASONABLE_VELOCITY_MPS = DriveConstants.DRIVETRAIN_SPEED_LIMIT.in(MetersPerSecond) * 1.5; // Allow some margin // Score returned by velocityConsistency when it can't verify the observation - // (no history from this camera, or last observation is older than velocityCheckTimeoutSeconds). + // (no history from this camera, or last observation is older than + // VELOCITY_CHECK_TIMEOUT_SECONDS). // Previously this was 1.0 (perfect pass), which let bad first-in-a-while poses through // unchallenged. Setting this below 1.0 expresses uncertainty: "I can't confirm this pose // is consistent, so it needs stronger evidence from other tests to be accepted." @@ -102,53 +103,54 @@ public class VisionConstants { // A typical good single-tag observation (~0.75 base) drops to ~0.71, still well above 0.6. // At startup (robot placed on field), all cameras get this penalty, but the cross-camera // correlation boost (1.3x) compensates — agreeing cameras still converge the pose quickly. - public static double velocityUncertainScore = 0.6; + public static double VELOCITY_UNCERTAIN_SCORE = 0.6; // Cross-camera correlation thresholds // When multiple cameras report similar poses at similar times, we boost confidence. // This helps validate observations and reject outliers from miscalibrated cameras. // Set to 150ms to allow cameras that don't fire simultaneously to still be fused. // Analysis showed cameras often fire 80-150ms apart, so 50ms was too narrow. - public static double correlationTimeWindowSeconds = 0.150; - public static Distance correlationPoseThreshold = + public static double CORRELATION_TIME_WINDOW_SECONDS = 0.150; + public static Distance CORRELATION_POSE_THRESHOLD = Meters.of(0.15); // How close poses must be to "agree" - public static final double correlationPoseThresholdMeters = correlationPoseThreshold.in(Meters); - public static double correlationBoostFactor = + public static final double CORRELATION_POSE_THRESHOLD_METERS = + CORRELATION_POSE_THRESHOLD.in(Meters); + public static double CORRELATION_BOOST_FACTOR = 1.4; // Score multiplier when corroborated by majority // Standard deviation baselines - public static double linearStdDevBaseline = 0.02; // Meters - public static double angularStdDevBaseline = 0.06; // Radians + public static double LINEAR_STD_DEV_BASELINE = 0.02; // Meters + public static double ANGULAR_STD_DEV_BASELINE = 0.06; // Radians // Single-camera observations get this multiplier on stddev to reduce their influence. // This reduces jitter from multiple single-camera observations reporting slightly // different poses. Multi-camera fused observations (which agree) get no penalty. // A value of 3.0 means single-camera observations have 3x less influence than fused. - public static double singleCameraStdDevMultiplier = 3.0; + public static double SINGLE_CAMERA_STD_DEV_MULTIPLIER = 3.0; - public static double maxStdDev = 1.0; // Meters + public static double MAX_STD_DEV = 1.0; // Meters // Minimum score for a vision observation to be accepted into the pose estimator. // Observations below this threshold are rejected outright. - // Previously derived as linearStdDevBaseline / maxStdDev = 0.02, which accepted nearly + // Previously derived as LINEAR_STD_DEV_BASELINE / MAX_STD_DEV = 0.02, which accepted nearly // everything. Log analysis showed that bad poses (wrong PnP solution, ~6m off) scored - // 0.58-0.61 after the velocityUncertainScore penalty, while good single-tag observations - // scored 0.65+. Setting minScore to 0.6 rejects the penalized bad poses while preserving + // 0.58-0.61 after the VELOCITY_UNCERTAIN_SCORE penalty, while good single-tag observations + // scored 0.65+. Setting MIN_SCORE to 0.6 rejects the penalized bad poses while preserving // 97%+ of legitimate observations. Multi-tag observations (0.9+) are unaffected. - public static double minScore = 0.65; + public static double MIN_SCORE = 0.65; // Feature flags - public static boolean kLogIndividualCameraPoses = false; - public static boolean kLogSummaryPoses = false; - public static boolean kLogAcceptedPoses = true; - public static boolean kLogRejectedPoses = true; + public static boolean LOG_INDIVIDUAL_CAMERA_POSES = false; + public static boolean LOG_SUMMARY_POSES = false; + public static boolean LOG_ACCEPTED_POSES = true; + public static boolean LOG_REJECTED_POSES = true; // Logging frequency (1 = every cycle, 2 = every other cycle, etc.) // Higher values reduce CPU load but loses data granularity for replay - public static int kLoggingDivisor = 2; + public static int LOGGING_DIVISOR = 2; // Vision processing interval (1 = every loop, 5 = every 5th loop = 10Hz at 50Hz robot loop) // Higher values batch more observations together for fusion, reducing jitter but adding latency. // At 5 loops (100ms batches), cameras have time to all report before fusion decides what agrees. - public static int processingIntervalLoops = 5; + public static int PROCESSING_INTERVAL_LOOPS = 5; } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionFilter.java b/src/main/java/frc/robot/subsystems/vision/VisionFilter.java index 982ec258..72df5541 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionFilter.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionFilter.java @@ -34,9 +34,9 @@ public class VisionFilter { private static final Rectangle2d arenaRectangle; static { - double halfWidth = minRobotWidthHalfMeters; + double halfWidth = MIN_ROBOT_WIDTH_HALF_METERS; var cornerA = new Translation2d(halfWidth, halfWidth); - var cornerB = new Translation2d(fieldXLenMeters - halfWidth, fieldYLenMeters - halfWidth); + var cornerB = new Translation2d(FIELD_X_LEN_METERS - halfWidth, FIELD_Y_LEN_METERS - halfWidth); arenaRectangle = new Rectangle2d(cornerA, cornerB); } @@ -140,7 +140,7 @@ public enum Test { @Override public double test(TestContext ctx) { if (ctx.observation().tagCount() == 1) { - return 1.0 - normalizedSigmoid(ctx.observation().ambiguity(), ambiguityTolerance, 4.0); + return 1.0 - normalizedSigmoid(ctx.observation().ambiguity(), AMBIGUITY_TOLERANCE, 4.0); } return 1.0; } @@ -152,7 +152,7 @@ public double test(TestContext ctx) { return 1.0 - normalizedSigmoid( Math.abs(ctx.observation().pose().getRotation().getY()), - pitchToleranceRadians, + PITCH_TOLERANCE_RADIANS, 1.0); } }, @@ -162,7 +162,9 @@ public double test(TestContext ctx) { public double test(TestContext ctx) { return 1.0 - normalizedSigmoid( - Math.abs(ctx.observation().pose().getRotation().getX()), rollToleranceRadians, 1.0); + Math.abs(ctx.observation().pose().getRotation().getX()), + ROLL_TOLERANCE_RADIANS, + 1.0); } }, @@ -171,7 +173,7 @@ public double test(TestContext ctx) { public double test(TestContext ctx) { return 1.0 - normalizedSigmoid( - Math.abs(ctx.observation().pose().getZ()), elevationToleranceMeters, 1.0); + Math.abs(ctx.observation().pose().getZ()), ELEVATION_TOLERANCE_METERS, 1.0); } }, @@ -196,7 +198,7 @@ public double test(TestContext ctx) { public double test(TestContext ctx) { return 1.0 - normalizedSigmoid( - ctx.observation().averageTagDistance(), tagDistanceToleranceMeters, 1.0); + ctx.observation().averageTagDistance(), TAG_DISTANCE_TOLERANCE_METERS, 1.0); } }, @@ -214,12 +216,12 @@ public double test(TestContext ctx) { @Override public double test(TestContext ctx) { // No history from this camera — express uncertainty, not confidence - if (ctx.lastAcceptedPose() == null) return velocityUncertainScore; + if (ctx.lastAcceptedPose() == null) return VELOCITY_UNCERTAIN_SCORE; double dt = ctx.observation().timestamp() - ctx.lastAcceptedTimestamp(); if (dt <= 0.001) return 1.0; // Near-simultaneous, skip check // Stale history — treat the same as no history - if (dt > velocityCheckTimeoutSeconds) return velocityUncertainScore; + if (dt > VELOCITY_CHECK_TIMEOUT_SECONDS) return VELOCITY_UNCERTAIN_SCORE; double distance = ctx.observation() @@ -229,7 +231,7 @@ public double test(TestContext ctx) { .getDistance(ctx.lastAcceptedPose().getTranslation()); double impliedVelocity = distance / dt; - return 1.0 - normalizedSigmoid(impliedVelocity, maxReasonableVelocityMps, 2.0); + return 1.0 - normalizedSigmoid(impliedVelocity, MAX_REASONABLE_VELOCITY_MPS, 2.0); } }, @@ -250,7 +252,7 @@ public double test(TestContext ctx) { double gyroYaw = ctx.gyroYaw().getRadians(); double yawError = Math.abs(MathUtil.angleModulus(visionYaw - gyroYaw)); - return 1.0 - normalizedSigmoid(yawError, yawToleranceRadians, 4.0); + return 1.0 - normalizedSigmoid(yawError, YAW_TOLERANCE_RADIANS, 4.0); } }; @@ -376,10 +378,10 @@ public ArrayList fuseCorrelatedObservations( if (obsA.cameraIndex() == obsB.cameraIndex()) continue; double timeB = obsB.observation().timestamp(); - if (Math.abs(timeA - timeB) > correlationTimeWindowSeconds) continue; + if (Math.abs(timeA - timeB) > CORRELATION_TIME_WINDOW_SECONDS) continue; var transB = obsB.observation().pose().toPose2d().getTranslation(); - if (transA.getDistance(transB) > correlationPoseThresholdMeters) continue; + if (transA.getDistance(transB) > CORRELATION_POSE_THRESHOLD_METERS) continue; // Observations agree - merge their clusters var clusterA = clusterMap.get(i); @@ -440,7 +442,7 @@ public ArrayList fuseCorrelatedObservations( double avgTime = sumTime / sumWeight; // Boost score for correlated observations - double fusedScore = Math.min(1.0, maxScore * correlationBoostFactor); + double fusedScore = Math.min(1.0, maxScore * CORRELATION_BOOST_FACTOR); result.add( new FusedObservation( diff --git a/src/test/java/frc/robot/subsystems/vision/VisionFilterTest.java b/src/test/java/frc/robot/subsystems/vision/VisionFilterTest.java index fdcaa214..97b43a17 100644 --- a/src/test/java/frc/robot/subsystems/vision/VisionFilterTest.java +++ b/src/test/java/frc/robot/subsystems/vision/VisionFilterTest.java @@ -137,7 +137,7 @@ void zeroTagsReturnsOne() { @org.junit.jupiter.api.Test @DisplayName("Ambiguity at tolerance threshold") void ambiguityAtThreshold() { - double tolerance = VisionConstants.ambiguityTolerance; // 0.15 + double tolerance = VisionConstants.AMBIGUITY_TOLERANCE; // 0.15 var ctx = new TestContext().observation(makeObservation(8, 4, 0, 0, 0, 0, 0.0, 1, tolerance, 3.0)); double result = Test.unambiguous.test(ctx); @@ -402,7 +402,7 @@ void farTags() { @org.junit.jupiter.api.Test @DisplayName("Moderate distance at tolerance") void moderateDistance() { - double tolerance = VisionConstants.tagDistanceToleranceMeters; // 4.0 + double tolerance = VisionConstants.TAG_DISTANCE_TOLERANCE_METERS; // 4.0 var ctx = new TestContext().observation(makeObservation(8, 4, 0, 0, 0, 0, 0.0, 1, 0.01, tolerance)); double result = Test.distanceToTags.test(ctx); @@ -424,7 +424,7 @@ void noPreviousPose() { .lastAcceptedTimestamp(0.0); double result = Test.velocityConsistency.test(ctx); assertEquals( - VisionConstants.velocityUncertainScore, + VisionConstants.VELOCITY_UNCERTAIN_SCORE, result, 0.001, "No history should return uncertain score, not a perfect pass"); @@ -480,7 +480,7 @@ void longTimeout() { .lastAcceptedTimestamp(0.0); double result = Test.velocityConsistency.test(ctx); assertEquals( - VisionConstants.velocityUncertainScore, + VisionConstants.VELOCITY_UNCERTAIN_SCORE, result, 0.001, "Stale timestamp should return uncertain score, not a perfect pass"); @@ -625,7 +625,7 @@ void rejectsAmbiguousPnP() { + testedBad.score()); // The bad pose should score below minScore threshold assertTrue( - testedBad.score() < VisionConstants.minScore, + testedBad.score() < VisionConstants.MIN_SCORE, "Bad PnP with 45 degree yaw error should score below minScore, got " + testedBad.score()); } } @@ -939,7 +939,7 @@ void fusedScoreCapsAtOne() { @org.junit.jupiter.api.Test @DisplayName("Edge of time window: exactly at threshold fuses") void edgeOfTimeWindowFuses() { - double threshold = VisionConstants.correlationTimeWindowSeconds; + double threshold = VisionConstants.CORRELATION_TIME_WINDOW_SECONDS; var observations = new ArrayList(); observations.add(makeTestedObs(8.0, 4.0, 0.0, 0, 0.5)); observations.add(makeTestedObs(8.05, 4.05, threshold, 1, 0.5)); // Exactly at threshold @@ -954,7 +954,7 @@ void edgeOfTimeWindowFuses() { @org.junit.jupiter.api.Test @DisplayName("Edge of position threshold: exactly at threshold not fused") void edgeOfPositionThresholdNotFused() { - double threshold = VisionConstants.correlationPoseThresholdMeters; + double threshold = VisionConstants.CORRELATION_POSE_THRESHOLD_METERS; var observations = new ArrayList(); observations.add(makeTestedObs(8.0, 4.0, 0.0, 0, 0.5)); // Exactly at threshold distance @@ -969,7 +969,7 @@ void edgeOfPositionThresholdNotFused() { @org.junit.jupiter.api.Test @DisplayName("Just under position threshold fuses") void justUnderPositionThresholdFuses() { - double threshold = VisionConstants.correlationPoseThresholdMeters; + double threshold = VisionConstants.CORRELATION_POSE_THRESHOLD_METERS; var observations = new ArrayList(); observations.add(makeTestedObs(8.0, 4.0, 0.0, 0, 0.5)); observations.add(makeTestedObs(8.0 + threshold - 0.01, 4.0, 0.01, 1, 0.5)); @@ -982,7 +982,7 @@ void justUnderPositionThresholdFuses() { @org.junit.jupiter.api.Test @DisplayName("Transitive clustering: A-B and B-C agree, all fuse") void transitiveClusteringFuses() { - double dist = VisionConstants.correlationPoseThresholdMeters - 0.01; + double dist = VisionConstants.CORRELATION_POSE_THRESHOLD_METERS - 0.01; var observations = new ArrayList(); // A at origin observations.add(makeTestedObs(8.0, 4.0, 0.0, 0, 0.5)); @@ -1205,7 +1205,7 @@ void perfectObservationScoresWell() { // still score well above minScore — a good observation needs to be accepted even // when it's the first one from a camera. assertTrue( - tested.score() > VisionConstants.minScore, + tested.score() > VisionConstants.MIN_SCORE, "Perfect observation should score above minScore, got " + tested.score()); assertTrue( tested.score() > 0.6, "Perfect observation should score > 0.6, got " + tested.score()); From f12b6f84e2ed96de723a8113007d0d27054470c4 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Thu, 4 Jun 2026 12:07:10 -0400 Subject: [PATCH 6/8] Package camera name and transform into CameraConfig record Replaces the separate CAMERA_*_NAME and ROBOT_TO_*_CAMERA constants with a CameraConfig record, and updates VisionIOPhotonVision and VisionIOPhotonVisionSim to accept a CameraConfig directly. Co-Authored-By: Claude Sonnet 4.6 --- src/main/java/frc/robot/Robot.java | 20 ++--- .../subsystems/vision/VisionConstants.java | 75 ++++++++++--------- .../vision/VisionIOPhotonVision.java | 12 +-- .../vision/VisionIOPhotonVisionSim.java | 9 +-- 4 files changed, 58 insertions(+), 58 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index bea6b102..0d929d52 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -185,10 +185,10 @@ public Robot() { new Vision( drive::addVisionMeasurement, drive::getFieldRelativeHeading, - new VisionIOPhotonVision(CAMERA_FRONT_RIGHT_NAME, ROBOT_TO_FRONT_RIGHT_CAMERA), - new VisionIOPhotonVision(CAMERA_FRONT_LEFT_NAME, ROBOT_TO_FRONT_LEFT_CAMERA), - new VisionIOPhotonVision(CAMERA_BACK_RIGHT_NAME, ROBOT_TO_BACK_RIGHT_CAMERA), - new VisionIOPhotonVision(CAMERA_BACK_LEFT_NAME, ROBOT_TO_BACK_LEFT_CAMERA)); + new VisionIOPhotonVision(FRONT_RIGHT_CAMERA), + new VisionIOPhotonVision(FRONT_LEFT_CAMERA), + new VisionIOPhotonVision(BACK_RIGHT_CAMERA), + new VisionIOPhotonVision(BACK_LEFT_CAMERA)); launcher = new Launcher( drive::getPose, @@ -226,14 +226,10 @@ public Robot() { new Vision( drive::addVisionMeasurement, drive::getFieldRelativeHeading, - new VisionIOPhotonVisionSim( - CAMERA_FRONT_RIGHT_NAME, ROBOT_TO_FRONT_RIGHT_CAMERA, drive::getPose), - new VisionIOPhotonVisionSim( - CAMERA_FRONT_LEFT_NAME, ROBOT_TO_FRONT_LEFT_CAMERA, drive::getPose), - new VisionIOPhotonVisionSim( - CAMERA_BACK_RIGHT_NAME, ROBOT_TO_BACK_RIGHT_CAMERA, drive::getPose), - new VisionIOPhotonVisionSim( - CAMERA_BACK_LEFT_NAME, ROBOT_TO_BACK_LEFT_CAMERA, drive::getPose)); + new VisionIOPhotonVisionSim(FRONT_RIGHT_CAMERA, drive::getPose), + new VisionIOPhotonVisionSim(FRONT_LEFT_CAMERA, drive::getPose), + new VisionIOPhotonVisionSim(BACK_RIGHT_CAMERA, drive::getPose), + new VisionIOPhotonVisionSim(BACK_LEFT_CAMERA, drive::getPose)); launcher = new Launcher( drive::getPose, diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index b53bda1e..77c8d5d2 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -25,41 +25,46 @@ public class VisionConstants { public static Boolean USE_CUSTOM_APRIL_TAG_LAYOUT = false; public static AprilTagFields DEFAULT_APRIL_TAG_FIELD_LAYOUT = AprilTagFields.k2026RebuiltAndymark; - // Camera names, must match names configured on coprocessor - public static String CAMERA_FRONT_RIGHT_NAME = "OV2311_TH_2026_FR"; - public static String CAMERA_FRONT_LEFT_NAME = "OV2311_TH_2026_FL"; - public static String CAMERA_BACK_RIGHT_NAME = "OV2311_TH_2026_RR"; - public static String CAMERA_BACK_LEFT_NAME = "OV2311_TH_2026_RL"; - - // Robot to camera transforms - public static Transform3d ROBOT_TO_FRONT_RIGHT_CAMERA = - new Transform3d( - Inches.of(-10.572), - Inches.of(-12.337), - Inches.of(16.688), - // pitch 20 degrees up, yaw 55 degrees right - new Rotation3d(new Quaternion(0.8735, -0.0802, -0.1540, -0.4547))); - public static Transform3d ROBOT_TO_FRONT_LEFT_CAMERA = - new Transform3d( - Inches.of(-10.572), - Inches.of(12.337), - Inches.of(16.688), - // pitch 20 degrees up, yaw 55 degrees left - new Rotation3d(new Quaternion(0.8735, 0.0802, -0.1540, 0.4547))); - public static Transform3d ROBOT_TO_BACK_RIGHT_CAMERA = - new Transform3d( - Inches.of(-13.1623), - Inches.of(-12.1623), - Inches.of(20.26674), - // pitch 15 degrees up, yaw 135 degrees right - new Rotation3d(new Quaternion(-0.3794, 0.1206, 0.0500, 0.9160))); - public static Transform3d ROBOT_TO_BACK_LEFT_CAMERA = - new Transform3d( - Inches.of(-13.1623), - Inches.of(12.1623), - Inches.of(20.26674), - // pitch 15 degrees up, yaw 135 degrees left - new Rotation3d(new Quaternion(0.3794, 0.1206, -0.0500, 0.9160))); + /** Pairs a camera's coprocessor name with its robot-to-camera transform. */ + public record CameraConfig(String name, Transform3d robotToCamera) {} + + // Camera configurations (name must match name configured on coprocessor) + public static CameraConfig FRONT_RIGHT_CAMERA = + new CameraConfig( + "OV2311_TH_2026_FR", + new Transform3d( + Inches.of(-10.572), + Inches.of(-12.337), + Inches.of(16.688), + // pitch 20 degrees up, yaw 55 degrees right + new Rotation3d(new Quaternion(0.8735, -0.0802, -0.1540, -0.4547)))); + public static CameraConfig FRONT_LEFT_CAMERA = + new CameraConfig( + "OV2311_TH_2026_FL", + new Transform3d( + Inches.of(-10.572), + Inches.of(12.337), + Inches.of(16.688), + // pitch 20 degrees up, yaw 55 degrees left + new Rotation3d(new Quaternion(0.8735, 0.0802, -0.1540, 0.4547)))); + public static CameraConfig BACK_RIGHT_CAMERA = + new CameraConfig( + "OV2311_TH_2026_RR", + new Transform3d( + Inches.of(-13.1623), + Inches.of(-12.1623), + Inches.of(20.26674), + // pitch 15 degrees up, yaw 135 degrees right + new Rotation3d(new Quaternion(-0.3794, 0.1206, 0.0500, 0.9160)))); + public static CameraConfig BACK_LEFT_CAMERA = + new CameraConfig( + "OV2311_TH_2026_RL", + new Transform3d( + Inches.of(-13.1623), + Inches.of(12.1623), + Inches.of(20.26674), + // pitch 15 degrees up, yaw 135 degrees left + new Rotation3d(new Quaternion(0.3794, 0.1206, -0.0500, 0.9160)))); public static Distance MIN_ROBOT_WIDTH = Inches.of(36.875); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index 98323fa6..03deea78 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -12,6 +12,7 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.networktables.DoubleSubscriber; import edu.wpi.first.networktables.NetworkTableInstance; +import frc.robot.subsystems.vision.VisionConstants.CameraConfig; import java.util.ArrayList; import java.util.HashSet; import java.util.List; @@ -32,16 +33,15 @@ public class VisionIOPhotonVision implements VisionIO { /** * Creates a new VisionIOPhotonVision. * - * @param name The configured name of the camera. - * @param rotationSupplier The 3D position of the camera relative to the robot. + * @param config Camera name and robot-to-camera transform. */ - public VisionIOPhotonVision(String name, Transform3d robotToCamera) { - camera = new PhotonCamera(name); - this.robotToCamera = robotToCamera; + public VisionIOPhotonVision(CameraConfig config) { + camera = new PhotonCamera(config.name()); + this.robotToCamera = config.robotToCamera(); fpsSubscriber = NetworkTableInstance.getDefault() .getTable("photonvision") - .getSubTable(name) + .getSubTable(config.name()) .getDoubleTopic("fps") .subscribe(0.0); } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java index f0b3c9da..c67d7b47 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java @@ -9,7 +9,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform3d; +import frc.robot.subsystems.vision.VisionConstants.CameraConfig; import java.util.function.Supplier; import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.SimCameraProperties; @@ -37,12 +37,11 @@ public class VisionIOPhotonVisionSim extends VisionIOPhotonVision { /** * Creates a new VisionIOPhotonVisionSim. * - * @param name The name of the camera. + * @param config Camera name and robot-to-camera transform. * @param poseSupplier Supplier for the robot pose to use in simulation. */ - public VisionIOPhotonVisionSim( - String name, Transform3d robotToCamera, Supplier poseSupplier) { - super(name, robotToCamera); + public VisionIOPhotonVisionSim(CameraConfig config, Supplier poseSupplier) { + super(config); this.poseSupplier = poseSupplier; // Initialize vision sim From bd3b2df591e9f120e796b4a4193eb8fb5aeec737 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Thu, 4 Jun 2026 13:37:14 -0400 Subject: [PATCH 7/8] Finalize UPPER_SNAKE_CASE renaming and tune motor current limits Completes UPPER_SNAKE_CASE constant renaming across drive, feeder, intake, and launcher subsystems. Also removes unused FREE_SPEED constants from motor classes and adjusts default current limits to safer values. Co-Authored-By: Claude Sonnet 4.6 --- src/main/java/frc/robot/Constants.java | 14 ++----- src/main/java/frc/robot/Robot.java | 4 +- .../subsystems/drive/DriveConstants.java | 7 ++-- .../subsystems/drive/ModuleIOSimWPI.java | 14 +++---- .../subsystems/feeder/FeederConstants.java | 15 +++---- .../subsystems/feeder/KickerIOSimSpark.java | 4 +- .../subsystems/feeder/KickerIOSpark.java | 2 +- .../feeder/SpindexerIOSimSpark.java | 4 +- .../subsystems/feeder/SpindexerIOSpark.java | 2 +- .../subsystems/intake/IntakeConstants.java | 42 +++++++++---------- .../subsystems/intake/RollerIOSimSpark.java | 13 ++---- .../subsystems/intake/RollerIOSimTalonFX.java | 17 +++++--- .../subsystems/intake/RollerIOSpark.java | 12 ++---- .../subsystems/intake/RollerIOTalonFX.java | 6 +-- .../launcher/FlywheelIOSimTalonFX.java | 8 ++++ .../subsystems/launcher/HoodIOSimSpark.java | 2 +- .../subsystems/launcher/HoodIOSpark.java | 2 +- .../launcher/LauncherConstants.java | 17 +++----- .../subsystems/launcher/TurretIOSimSpark.java | 4 +- .../subsystems/launcher/TurretIOSpark.java | 2 +- 20 files changed, 85 insertions(+), 106 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 20c0bd3e..cc95ecbc 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,10 +13,7 @@ package frc.robot; -import static edu.wpi.first.units.Units.*; - import com.ctre.phoenix6.CANBus; -import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.RobotBase; /** @@ -53,24 +50,21 @@ public final class RobotConstants { public static final class MotorConstants { public static final class NEOConstants { - public static final AngularVelocity FREE_SPEED = RPM.of(5676); - public static final int DEFAULT_SUPPLY_CURRENT_LIMIT = 60; - public static final int DEFAULT_STATOR_CURRENT_LIMIT = 100; + public static final int DEFAULT_SUPPLY_CURRENT_LIMIT = 40; + public static final int DEFAULT_STATOR_CURRENT_LIMIT = 60; } public static final class NEO550Constants { - public static final AngularVelocity FREE_SPEED = RPM.of(11000); - public static final int DEFAULT_SUPPLY_CURRENT_LIMIT = 10; + public static final int DEFAULT_SUPPLY_CURRENT_LIMIT = 5; + public static final int DEFAULT_STATOR_CURRENT_LIMIT = 10; } public static final class NEOVortexConstants { - public static final AngularVelocity FREE_SPEED = RPM.of(6784); public static final int DEFAULT_SUPPLY_CURRENT_LIMIT = 60; public static final int DEFAULT_STATOR_CURRENT_LIMIT = 100; } public static final class KrakenX60Constants { - public static final AngularVelocity FREE_SPEED = RPM.of(6000); public static final int DEFAULT_SUPPLY_CURRENT_LIMIT = 60; public static final int DEFAULT_STATOR_CURRENT_LIMIT = 100; } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0d929d52..3468466b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -302,10 +302,10 @@ public Robot() { if (FeatureFlags.HOPPER_ENABLED) { intake.setDeployInterlock( hopper::isDeployed, - () -> hopper.getDeployCommand().withTimeout(IntakeConstants.kInterlockSettleSeconds)); + () -> hopper.getDeployCommand().withTimeout(IntakeConstants.INTERLOCK_SETTLE_SECONDS)); hopper.setRetractInterlock( intake::isStowed, - () -> intake.getStopCommand().withTimeout(IntakeConstants.kInterlockSettleSeconds)); + () -> intake.getStopCommand().withTimeout(IntakeConstants.INTERLOCK_SETTLE_SECONDS)); } configureControlPanelBindings(); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 5de8a646..b4690cf1 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -72,12 +72,13 @@ public class DriveConstants { public static final double WHEEL_RADIUS_METERS = WHEEL_RADIUS.in(Meters); public static final double DRIVE_MOTOR_REDUCTION = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); // SDS MK4 L2 - public static final DCMotor DRIVE_GEARBOX = DCMotor.getKrakenX60(1); + public static final DCMotor DRIVE_GEARBOX = DCMotor.getKrakenX60Foc(1); public static final LinearVelocity DRIVETRAIN_SPEED_LIMIT = MetersPerSecond.of( 0.9 * (WHEEL_RADIUS_METERS * 2.0 * Math.PI) - * KrakenX60Constants.FREE_SPEED.in(RotationsPerSecond) + * DRIVE_GEARBOX.freeSpeedRadPerSec + / (2.0 * Math.PI) / DRIVE_MOTOR_REDUCTION); // Chassis movement limits @@ -106,7 +107,7 @@ public class DriveConstants { public static final double TURN_MOTOR_REDUCTION = (32.0 / 15.0) * (60.0 / 10.0); // SDS MK4 // Every 1 rotation of the azimuth results in COUPLE_RATIO drive motor turns private static final double COUPLE_RATIO = (50.0 / 14.0); // SDS MK4 L2 - public static final DCMotor TURN_GEARBOX = DCMotor.getKrakenX60(1); + public static final DCMotor TURN_GEARBOX = DCMotor.getKrakenX60Foc(1); // Absolute turn encoder configuration public static final boolean TURN_ENCODER_INVERTED = false; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSimWPI.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSimWPI.java index 81a05a55..3720b88d 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSimWPI.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSimWPI.java @@ -19,7 +19,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.Timer; @@ -37,9 +36,6 @@ public class ModuleIOSimWPI implements ModuleIO { private static final double DRIVE_KV = 1.0 / Units.rotationsToRadians(1.0 / DRIVE_KV_ROT); private static final double TURN_KP = 8.0; private static final double TURN_KD = 0.0; - private static final DCMotor DRIVE_GEARBOX = DCMotor.getKrakenX60Foc(1); - private static final DCMotor TURN_GEARBOX = DCMotor.getKrakenX60Foc(1); - private final DCMotorSim driveSim; private final DCMotorSim turnSim; @@ -58,13 +54,15 @@ public ModuleIOSimWPI( driveSim = new DCMotorSim( LinearSystemId.createDCMotorSystem( - DRIVE_GEARBOX, constants.DriveInertia, constants.DriveMotorGearRatio), - DRIVE_GEARBOX); + DriveConstants.DRIVE_GEARBOX, + constants.DriveInertia, + constants.DriveMotorGearRatio), + DriveConstants.DRIVE_GEARBOX); turnSim = new DCMotorSim( LinearSystemId.createDCMotorSystem( - TURN_GEARBOX, constants.SteerInertia, constants.SteerMotorGearRatio), - TURN_GEARBOX); + DriveConstants.TURN_GEARBOX, constants.SteerInertia, constants.SteerMotorGearRatio), + DriveConstants.TURN_GEARBOX); // Enable wrapping for turn PID turnController.enableContinuousInput(-Math.PI, Math.PI); diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java index 6df96f56..77e137ef 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java @@ -3,11 +3,10 @@ import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; -import static edu.wpi.first.units.Units.RadiansPerSecond; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearVelocity; -import frc.robot.Constants.MotorConstants.NEOVortexConstants; public final class FeederConstants { public static final class SpindexerConstants { @@ -16,11 +15,9 @@ public static final class SpindexerConstants { // Motor public static final double MOTOR_REDUCTION = 1.0; + public static final DCMotor GEARBOX = DCMotor.getNeoVortex(1); public static final LinearVelocity MAX_TANGENTIAL_VELOCITY = - MetersPerSecond.of( - NEOVortexConstants.FREE_SPEED.in(RadiansPerSecond) - * RADIUS.in(Meters) - / MOTOR_REDUCTION); + MetersPerSecond.of(GEARBOX.freeSpeedRadPerSec * RADIUS.in(Meters) / MOTOR_REDUCTION); // Encoder public static final double ENCODER_POSITION_FACTOR = 2.0 * Math.PI / MOTOR_REDUCTION; // Meters @@ -38,11 +35,9 @@ public static final class KickerConstants { // Motor public static final double MOTOR_REDUCTION = 5.0; + public static final DCMotor GEARBOX = DCMotor.getNeoVortex(1); public static final LinearVelocity MAX_TANGENTIAL_VELOCITY = - MetersPerSecond.of( - NEOVortexConstants.FREE_SPEED.in(RadiansPerSecond) - * RADIUS.in(Meters) - / MOTOR_REDUCTION); + MetersPerSecond.of(GEARBOX.freeSpeedRadPerSec * RADIUS.in(Meters) / MOTOR_REDUCTION); // Encoder public static final double ENCODER_POSITION_FACTOR = 2.0 * Math.PI / MOTOR_REDUCTION; // Meters diff --git a/src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java b/src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java index abca1e5f..592734a6 100644 --- a/src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java @@ -14,7 +14,6 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkFlexConfig; -import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Voltage; @@ -27,7 +26,6 @@ public class KickerIOSimSpark implements KickerIO { private static final double KICKER_MOI_KG_M2 = 0.00052; - private static final DCMotor GEARBOX = DCMotor.getNeoVortex(1); private final DCMotorSim kickerSim; @@ -43,7 +41,7 @@ public KickerIOSimSpark() { config .inverted(false) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .smartCurrentLimit(NEOVortexConstants.DEFAULT_STATOR_CURRENT_LIMIT) .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); config diff --git a/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java b/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java index 97b6dd73..06dd3a05 100644 --- a/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java +++ b/src/main/java/frc/robot/subsystems/feeder/KickerIOSpark.java @@ -40,7 +40,7 @@ public KickerIOSpark() { config .inverted(false) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .smartCurrentLimit(NEOVortexConstants.DEFAULT_STATOR_CURRENT_LIMIT) .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); config diff --git a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java index 770ca0a0..d674b6ee 100644 --- a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java @@ -14,7 +14,6 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkFlexConfig; -import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Voltage; @@ -27,7 +26,6 @@ public class SpindexerIOSimSpark implements SpindexerIO { private static final double SPINDEXER_MOI_KG_M2 = 0.00207; - private static final DCMotor GEARBOX = DCMotor.getNeoVortex(1); private final DCMotorSim spindexerSim; @@ -43,7 +41,7 @@ public SpindexerIOSimSpark() { config .inverted(false) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .smartCurrentLimit(NEOVortexConstants.DEFAULT_STATOR_CURRENT_LIMIT) .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); config diff --git a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java index a8dae22a..70465a13 100644 --- a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java +++ b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSpark.java @@ -40,7 +40,7 @@ public SpindexerIOSpark() { config .inverted(false) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .smartCurrentLimit(NEOVortexConstants.DEFAULT_STATOR_CURRENT_LIMIT) .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); config diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 81643725..c695723a 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -5,52 +5,50 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.Slot1Configs; +import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; import frc.robot.Constants.CANBusPorts.CAN2; public class IntakeConstants { /** Time (seconds) to wait after resolving an intake/hopper interlock before proceeding. */ - public static final double kInterlockSettleSeconds = 1.0; + public static final double INTERLOCK_SETTLE_SECONDS = 1.0; public static class RollerConstants { public static final Distance RADIUS = Inches.of(0.85); - // motor controller + // Motor controller public static final double MOTOR_REDUCTION = 1.0; public static final double MAX_ACCELERATION = 4000.0; public static final double MAX_JERK = 40000.0; - // roller constants + // Encoder public static final double ENCODER_POSITION_FACTOR = 2.0 * Math.PI / MOTOR_REDUCTION; // Meters public static final double ENCODER_VELOCITY_FACTOR = ENCODER_POSITION_FACTOR / 60.0; // Meters/sec - // Spark gains - public static final double kP = 0.001; - public static final double kD = 0.0; + // Configs + public record RollerConfig(int port, CANBus bus, boolean inverted) {} - // Talon gains - public static final Slot0Configs VELOCITY_VOLTAGE_GAINS = - new Slot0Configs().withKP(0.11).withKI(0.0).withKD(0.0).withKS(0.1).withKV(0.12); - public static final Slot1Configs VELOCITY_TORQUE_CURRENT_GAINS = - new Slot1Configs().withKP(0.11).withKI(0.0).withKD(0.0).withKS(2.5); - - // configs public static final RollerConfig UPPER_ROLLER_CONFIG = new RollerConfig(CAN2.INTAKE_ROLLER_UPPER, CAN2.BUS, true); public static final RollerConfig LOWER_ROLLER_CONFIG = new RollerConfig(CAN2.INTAKE_ROLLER_LOWER, CAN2.BUS, true); - } - public static class RollerConfig { - public final int port; - public final CANBus bus; - public final boolean inverted; + public static class SparkConfig { + public static final DCMotor GEARBOX = DCMotor.getNeoVortex(1); + public static final LinearVelocity MAX_TANGENTIAL_VELOCITY = + MetersPerSecond.of(GEARBOX.freeSpeedRadPerSec * RADIUS.in(Meters) / MOTOR_REDUCTION); + public static final double kP = 0.001; + public static final double kD = 0.0; + } - public RollerConfig(int port, CANBus bus, boolean inverted) { - this.port = port; - this.bus = bus; - this.inverted = inverted; + public static class TalonConfig { + public static final DCMotor GEARBOX = DCMotor.getKrakenX60(1); + public static final Slot0Configs VELOCITY_VOLTAGE_GAINS = + new Slot0Configs().withKP(0.11).withKI(0.0).withKD(0.0).withKS(0.1).withKV(0.12); + public static final Slot1Configs VELOCITY_TORQUE_CURRENT_GAINS = + new Slot1Configs().withKP(0.11).withKI(0.0).withKD(0.0).withKS(2.5); } } } diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java index 5cbc600f..b4f52b87 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOSimSpark.java @@ -2,6 +2,7 @@ import static edu.wpi.first.units.Units.*; import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.*; +import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.SparkConfig.*; import com.revrobotics.PersistMode; import com.revrobotics.ResetMode; @@ -14,7 +15,6 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkFlexConfig; -import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Voltage; @@ -23,14 +23,9 @@ import frc.robot.Constants.MotorConstants.NEOVortexConstants; import frc.robot.Constants.RobotConstants; import frc.robot.Robot; -import frc.robot.subsystems.intake.IntakeConstants.RollerConfig; public class RollerIOSimSpark implements RollerIO { private static final double KICKER_MOI_KG_M2 = 0.00052; - private static final LinearVelocity MAX_TANGENTIAL_VELOCITY = - MetersPerSecond.of( - NEOVortexConstants.FREE_SPEED.in(RadiansPerSecond) * RADIUS.in(Meters) / MOTOR_REDUCTION); - private static final DCMotor GEARBOX = DCMotor.getNeoVortex(1); private final DCMotorSim rollerSim; @@ -39,14 +34,14 @@ public class RollerIOSimSpark implements RollerIO { private final SparkFlexSim flexSim; public RollerIOSimSpark(RollerConfig rollerConfig) { - flex = new SparkFlex(rollerConfig.port, MotorType.kBrushless); + flex = new SparkFlex(rollerConfig.port(), MotorType.kBrushless); controller = flex.getClosedLoopController(); var config = new SparkFlexConfig(); config - .inverted(rollerConfig.inverted) + .inverted(rollerConfig.inverted()) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .smartCurrentLimit(NEOVortexConstants.DEFAULT_STATOR_CURRENT_LIMIT) .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); config diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java index f53d250e..72c42814 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java @@ -2,6 +2,7 @@ import static edu.wpi.first.units.Units.*; import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.*; +import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.TalonConfig.*; import static frc.robot.util.PhoenixUtil.tryUntilOk; import com.ctre.phoenix6.BaseStatusSignal; @@ -14,7 +15,6 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.units.measure.AngularAcceleration; import edu.wpi.first.units.measure.AngularVelocity; @@ -23,12 +23,10 @@ import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import frc.robot.Constants.MotorConstants.KrakenX60Constants; import frc.robot.Robot; -import frc.robot.subsystems.intake.IntakeConstants.RollerConfig; public class RollerIOSimTalonFX implements RollerIO { - private static final DCMotor GEARBOX = DCMotor.getKrakenX60(1); - private final DCMotorSim rollerSim; private final TalonFX motor; @@ -48,15 +46,22 @@ public class RollerIOSimTalonFX implements RollerIO { private final StatusSignal dutyCycle; public RollerIOSimTalonFX(RollerConfig rollerConfig) { - motor = new TalonFX(rollerConfig.port, rollerConfig.bus); + motor = new TalonFX(rollerConfig.port(), rollerConfig.bus()); config = new TalonFXConfiguration(); config.MotorOutput.Inverted = - rollerConfig.inverted + rollerConfig.inverted() ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; config.MotorOutput.withNeutralMode(NeutralModeValue.Brake); config.Slot0 = VELOCITY_VOLTAGE_GAINS; config.Slot1 = VELOCITY_TORQUE_CURRENT_GAINS; + config.TorqueCurrent.PeakForwardTorqueCurrent = KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; + config.TorqueCurrent.PeakReverseTorqueCurrent = + -KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; + config.CurrentLimits.StatorCurrentLimit = KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLimit = KrakenX60Constants.DEFAULT_SUPPLY_CURRENT_LIMIT; + config.CurrentLimits.SupplyCurrentLimitEnable = true; tryUntilOk(5, () -> motor.getConfigurator().apply(config, 0.25)); rollerSim = diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java b/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java index 1e5559ce..4a422d2c 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOSpark.java @@ -2,6 +2,7 @@ import static edu.wpi.first.units.Units.*; import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.*; +import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.SparkConfig.*; import static frc.robot.util.SparkUtil.*; import com.revrobotics.PersistMode; @@ -19,30 +20,25 @@ import edu.wpi.first.units.measure.Voltage; import frc.robot.Constants.MotorConstants.NEOVortexConstants; import frc.robot.Constants.RobotConstants; -import frc.robot.subsystems.intake.IntakeConstants.RollerConfig; import frc.robot.util.SparkOdometryThread; import frc.robot.util.SparkOdometryThread.SparkInputs; public class RollerIOSpark implements RollerIO { - private static final LinearVelocity MAX_TANGENTIAL_VELOCITY = - MetersPerSecond.of( - NEOVortexConstants.FREE_SPEED.in(RadiansPerSecond) * RADIUS.in(Meters) / MOTOR_REDUCTION); - private final SparkFlex flex; private final RelativeEncoder encoder; private final SparkClosedLoopController controller; private final SparkInputs sparkInputs; public RollerIOSpark(RollerConfig rollerConfig) { - flex = new SparkFlex(rollerConfig.port, MotorType.kBrushless); + flex = new SparkFlex(rollerConfig.port(), MotorType.kBrushless); encoder = flex.getEncoder(); controller = flex.getClosedLoopController(); var config = new SparkFlexConfig(); config - .inverted(rollerConfig.inverted) + .inverted(rollerConfig.inverted()) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .smartCurrentLimit(NEOVortexConstants.DEFAULT_STATOR_CURRENT_LIMIT) .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); config diff --git a/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java index f8944769..0c3ff7d3 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java @@ -2,6 +2,7 @@ import static edu.wpi.first.units.Units.*; import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.*; +import static frc.robot.subsystems.intake.IntakeConstants.RollerConstants.TalonConfig.*; import static frc.robot.util.PhoenixUtil.tryUntilOk; import com.ctre.phoenix6.BaseStatusSignal; @@ -20,7 +21,6 @@ import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.Voltage; import frc.robot.Constants.MotorConstants.KrakenX60Constants; -import frc.robot.subsystems.intake.IntakeConstants.RollerConfig; public class RollerIOTalonFX implements RollerIO { private final TalonFX motor; @@ -40,7 +40,7 @@ public class RollerIOTalonFX implements RollerIO { private final StatusSignal supplyCurrent; public RollerIOTalonFX(RollerConfig rollerConfig) { - motor = new TalonFX(rollerConfig.port, rollerConfig.bus); + motor = new TalonFX(rollerConfig.port(), rollerConfig.bus()); config = new TalonFXConfiguration(); config.TorqueCurrent.PeakForwardTorqueCurrent = KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; config.TorqueCurrent.PeakReverseTorqueCurrent = @@ -50,7 +50,7 @@ public RollerIOTalonFX(RollerConfig rollerConfig) { config.CurrentLimits.SupplyCurrentLimit = KrakenX60Constants.DEFAULT_SUPPLY_CURRENT_LIMIT; config.CurrentLimits.SupplyCurrentLimitEnable = true; config.MotorOutput.Inverted = - rollerConfig.inverted + rollerConfig.inverted() ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; config.MotorOutput.withNeutralMode(NeutralModeValue.Brake); diff --git a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java index 7395f076..efdda7ac 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimTalonFX.java @@ -25,6 +25,7 @@ import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; import frc.robot.Constants.CANBusPorts.CAN2; +import frc.robot.Constants.MotorConstants.KrakenX60Constants; import frc.robot.Robot; public class FlywheelIOSimTalonFX implements FlywheelIO { @@ -58,6 +59,13 @@ public FlywheelIOSimTalonFX() { .withNeutralMode(NeutralModeValue.Brake); config.Slot0 = VELOCITY_VOLTAGE_GAINS; config.Slot1 = VELOCITY_TORQUE_CURRENT_GAINS; + config.TorqueCurrent.PeakForwardTorqueCurrent = KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; + config.TorqueCurrent.PeakReverseTorqueCurrent = + -KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; + config.CurrentLimits.StatorCurrentLimit = KrakenX60Constants.DEFAULT_STATOR_CURRENT_LIMIT; + config.CurrentLimits.StatorCurrentLimitEnable = true; + config.CurrentLimits.SupplyCurrentLimit = KrakenX60Constants.DEFAULT_SUPPLY_CURRENT_LIMIT; + config.CurrentLimits.SupplyCurrentLimitEnable = true; tryUntilOk(5, () -> flywheelLeaderTalon.getConfigurator().apply(config, 0.25)); tryUntilOk(5, () -> flywheelFollowerTalon.getConfigurator().apply(config, 0.25)); diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSimSpark.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSimSpark.java index 12cd1fec..25c42a0b 100644 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIOSimSpark.java @@ -45,7 +45,7 @@ public HoodIOSimSpark() { hoodConfig .inverted(false) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEO550Constants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .smartCurrentLimit(NEO550Constants.DEFAULT_STATOR_CURRENT_LIMIT) .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); hoodConfig diff --git a/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java index 807fd941..0cc78f4c 100644 --- a/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/HoodIOSpark.java @@ -47,7 +47,7 @@ public HoodIOSpark() { hoodConfig .inverted(true) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEO550Constants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .smartCurrentLimit(NEO550Constants.DEFAULT_STATOR_CURRENT_LIMIT) .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); hoodConfig diff --git a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java index b73ee43c..a01a9238 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -13,8 +13,6 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; import frc.robot.Constants; -import frc.robot.Constants.MotorConstants.KrakenX60Constants; -import frc.robot.Constants.MotorConstants.NEO550Constants; public final class LauncherConstants { @@ -91,8 +89,9 @@ public static final class TurretConstants { // Motor controller public static final double MOTOR_REDUCTION = 9.0 * 72.0 / 12.0; + public static final DCMotor GEARBOX = DCMotor.getNeo550(1); public static final AngularVelocity MAX_ANGULAR_VELOCITY = - NEO550Constants.FREE_SPEED.div(MOTOR_REDUCTION); + RadiansPerSecond.of(GEARBOX.freeSpeedRadPerSec / MOTOR_REDUCTION); public static final double ENCODER_POSITION_FACTOR = (2 * Math.PI) / MOTOR_REDUCTION; // Radians public static final double ENCODER_VELOCITY_FACTOR = (2 * Math.PI) / (60.0 * MOTOR_REDUCTION); // Rad/sec @@ -113,15 +112,13 @@ public static final class FlywheelScaling { // Motor controller public static final double MOTOR_REDUCTION = 1.0; + public static final DCMotor GEARBOX = DCMotor.getKrakenX60(2); public static final AngularVelocity MAX_ANGULAR_VELOCITY = - KrakenX60Constants.FREE_SPEED.div(MOTOR_REDUCTION); + RadiansPerSecond.of(GEARBOX.freeSpeedRadPerSec / MOTOR_REDUCTION); public static final Slot0Configs VELOCITY_VOLTAGE_GAINS = new Slot0Configs().withKP(0.11).withKI(0.0).withKD(0.0).withKS(0.1).withKV(0.12); public static final Slot1Configs VELOCITY_TORQUE_CURRENT_GAINS = new Slot1Configs().withKP(12).withKI(0.0).withKD(0.0).withKS(2.5); - - // Simulation - public static final DCMotor GEARBOX = DCMotor.getKrakenX60(2); } public static final class HoodConstants { @@ -142,13 +139,11 @@ public static final class HoodConstants { // Motor controller public static final double MOTOR_REDUCTION = 5.0 * 256.0 / 16.0; + public static final DCMotor GEARBOX = DCMotor.getNeo550(1); public static final AngularVelocity MAX_ANGULAR_VELOCITY = - NEO550Constants.FREE_SPEED.div(MOTOR_REDUCTION); + RadiansPerSecond.of(GEARBOX.freeSpeedRadPerSec / MOTOR_REDUCTION); public static final double ENCODER_POSITION_FACTOR = 2 * Math.PI / MOTOR_REDUCTION; // Radians public static final double ENCODER_VELOCITY_FACTOR = (2 * Math.PI) / (60.0 * MOTOR_REDUCTION); // Rad/sec - - // Simulation - public static final DCMotor GEARBOX = DCMotor.getNeo550(1); } } diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java index de4010ca..db1f0b92 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java @@ -16,7 +16,6 @@ import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Voltage; @@ -29,7 +28,6 @@ public class TurretIOSimSpark implements TurretIO { private static final double TURRET_MOI_KG_M2 = 0.237; - private static final DCMotor GEARBOX = DCMotor.getNeo550(1); private final DCMotorSim turnSim; @@ -49,7 +47,7 @@ public TurretIOSimSpark() { turnConfig .inverted(false) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEO550Constants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .smartCurrentLimit(NEO550Constants.DEFAULT_STATOR_CURRENT_LIMIT) .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); turnConfig diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java index dfeb8f4b..48760c27 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java @@ -61,7 +61,7 @@ public TurretIOSpark() { turnConfig .inverted(true) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEO550Constants.DEFAULT_SUPPLY_CURRENT_LIMIT) + .smartCurrentLimit(NEO550Constants.DEFAULT_STATOR_CURRENT_LIMIT) .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); turnConfig From e292327b8e08cda31329c83b4c1ce1d723f8dfc7 Mon Sep 17 00:00:00 2001 From: nlaverdure Date: Thu, 4 Jun 2026 13:45:46 -0400 Subject: [PATCH 8/8] Mark *.glb as binary and restore model.glb Adds *.glb binary to .gitattributes so line-ending normalization never touches GLB files. Restores model.glb to its pre-normalization state. Co-Authored-By: Claude Sonnet 4.6 --- .gitattributes | 1 + ascope-assets/Robot_Rho/model.glb | Bin 22540427 -> 22540916 bytes 2 files changed, 1 insertion(+) diff --git a/.gitattributes b/.gitattributes index 266e9b3d..7103e4f6 100644 --- a/.gitattributes +++ b/.gitattributes @@ -8,3 +8,4 @@ *.ico binary *.jar binary *.whl binary +*.glb binary diff --git a/ascope-assets/Robot_Rho/model.glb b/ascope-assets/Robot_Rho/model.glb index 9fb3294d9a11f61aada7f2d5ef3eb996a849778d..4faa01349bc7304cdbc6bc79fb3d7b895f9cadec 100644 GIT binary patch delta 5742 zcmd6qd304pp2zuRzxUmG*~v~wAPEEn0!e@n2zx*iK(>h5jVuD)v;<`pQ36P3L@*5z zVx!~J0R*3QvehrFL6kSF!EdY|W$9JF2TZy5n%@&&Q4Toh@9ST8*x&S* z-~QYm0w55AAQ(cx0vlZ5fO*ypp%4b)5CM@81<}wPVjvdcARZDR5t5(rrB4$u)gK^Am|E|3j5kPBU*8+3;}=mGiA6M8`b^oBmr7y3be z7ytvI5C*|uxEt<)A}EF-FcgM)Z-0L8eHTSlmFyWlhh_Y9&u|z4BViPbh7!0B#=!mX z06ggZbk8r=yuv$Eru-7ddZ$czXif*(^{XF(aWEbxz(jZ$Cc$Kw0>6S^!&LYUJOb08 z)LXxLdj3?#N$`6V9)lS$6K26|m;-a+ahL~Bz;SO>4c>#!a+ zz(#lj-t;E;Z3?Q!=akKxyeXUCDtea`AD2_(7vL>@q*?{jn%=vn$RSWWLux80#HmTE znqFL!NBvb*)3fUPUDd;>_t{-tNKrPVhH(CehihCl<^f(b5B$SBX{-NK{Y$!6lvgKK zJ+-8|-JM6w7%hKW{nLa=RXc~*LBme?06v6|;9p@E?1nw?G5iVk!YA-&*ax4&XRsd*z(F_!hv5i(4qw1u z;7j-lzJ{aT^R4UZQ}O7}g>@bA=tg1PTI27_!+p(t!|ODAvufEyU-OR>>t>i=JCypG zzbLKq_NN)O)z|E|weAVrs(N4hn&)+0VHnNq{l4ae{dLCas`R6#Sw8A$9UOyt_y&%{ zU*TIg0e^#&@Ex3j({KjP!Z|n(7vOui2$$fpchu2_-HUJ}VeFL=(Uj9>Ug>A9{@U4B z;41t8*Sxi}8)q)%_cUp0^)|l`+S&xy;Rf9F4%+%-#xVLz18%|XXG;U50*v?>@i*e@ z3pBMLBf&;Oj98*e18k`daVb06cH0iBbh@(TLR7G_)h#DPdE!!SNf719rYuRIH$+7# zm$FbxMW|?cB2h=_wLCGYG*a0Kc08jc+KLeqX(=7I(w6(;Q+W2MQ|H*~iepi~>Rh32 z(dVLWGperGWXmVPNy>I#RhHX!TkZzx4Qjt~(K32f>6u$?IlLSTEhbR9xUWl&g&EVr zR5A!vE$eh4#%4UEC%CetQWy)pSb{Rbj)I#Oh zE_)$8WonT!J}jbVp<1lmc(holiseTf(aVo0+4pjmvYRz?;JVeUnTxWC@&=fG({~fx z)J@yeRYmzsw;-WL!i*BXjKms=6J0(tUZ(ovq%gQJku*!DuC&aJ zmpwh>jJ4M-m)&ID!2j#kP1aD^H!Y>>{tz$C*(4PWyW<5(Fp_8_$w&*)4ZD-2abBRZ zck=j!J}o7WjI3zWzoqCl{Zk};C?h)d9y6jIRhc5oNLXQ~#&=y5fdtua^Wh;^Cr4xc@qk6+7_q!?wdc2zOki>~(e z8rLer4rFgy4mf0Ll)fRJu+3 zc5=b;S4!#Tx$R`{qr@oQuuvzN~MlDmD%1Bzn_;EVqiWZULhSu%lD=w;{HJyE3-XJ<+M z5CWWpbq@N|`N(St=DgJ5yRxK1l!;k8EVNKP=*uig2sX+0o_bg5Dc@vCd?hET8Q?2l z^0-Jr2e#wxU)cP{b>(?wMrVm3NyF@~txz-+yTYl5nZ@Nz-%{&?JT)(wB`m7!EHy=y zUE~Ya#j!71!4Se&N{{(-$z+wj|-sv(_@-33GtHQ-Un?rN*Z@S7cb3P4N+Es{I6@8V>froljD|2hq>w!YEBf}Jx@KZbWmQNxY%wL;XU#M=^;A2N4~T! z!i1dieA&pA_CzH}uS24hSFs&tE``7*DC`2ujMkI1-Pjv}hdoYjt7t#+zVUArM) zUNvtIe=}ctkRqy>M?%q14*X)JU!uRkJWx@c?{j$rW!_O-iSWhryQMv4B1frBo||kB zpcaS*DgD3;J>_@&<<(~g_L3JkPR*ByIbO_{h&f`+*N9#kHYPL(Z&MR%0(Y z#dW6R{0e0LlT5#AL4l0PFd0)6bWc#fAYpC&?Bbvz>c!AE$gH+-I+-wpmlepQNEU2P zWr659mA&OyPhV0t=OL3|jxVvyRFe<%mgimOc8OE5m==#wA*eY8~Wjb&1431ie)l7r`X=IJ`(-&DOPc!ypJG# zjr23pU-XIc0dk?9^$rO34_2NpCJ!`euYaI?EY0jjmr6FDnWf7I%E|%UH!baCOiac| zx1B=E4N6BJ8YpXru*^SN{``E|Vnca8OevJ1EN*yep^U6bV}y0ZxLDUqR~=O+%6hxYM~xs&Tp50hK*CWlB7+(0TLkzxYG4Vx3`=1d)WR$9D!c~E zVFj#&Rp5cu@H)H!YhW#`gZ1Er4X_b5!JF_~_&3-LCcFi2!|z}VY=z&$HrNh3U?=Q? z-TpMoo={(HkGehn9(8+*5Bujm?Q`n>*D8F)hS#j|4f49HeWl*Smwl<;71h2xZ(=Qu z1=rx%zFJ?rx1z?^U7c6?_a}dv_ln_78ZPbLFPHcRs|ytU{de6FwXKibhuStJ?L%#M<*4#MIuic5ZF{Lq)wbzT z)UdW?+CHfBw@llAyU_d9?)@JBoIU&dvUl4%`z!qk{{4L?drKV$JpQj72XHhh_CRIr zs@MbY2RI0a;2rS8yKorZgMat0iaqk7UR#=a1dhV{AmJE%0LS4&_z3<3K86$UpYUH$ z51+uNa1#Cqr=S5ogVS&Z8sT#|3tzxF_!Bh2dAQ&&O>MrE?CmqExtD+HsOBx~4J|`4 zx2#!)d2dZaF>QMD^X%O;3&r|b&HfSInu91FIM_U2_x3!7;$z2}i(KAq4Je*%X#P<5 zZo1IitG50^GhBpA@FiS^ui(${HGBi#!gp{5u0jj6!Zr8{d=GzxAK*Ith!KC|;~M|# zkGGXY`oF1Y8*Zu1sBD9qa0_nxGb-;?yyo9b?J0k#J$|qq?!rCj@EU8b`8&4W!&6n7(%>vAA#8Sp;Fj!h)?X ztb&jvG+_vfc`%u2atsT*aO0?k)I_+5WDy}Eg(1^Ym}bm!aVL($l{+&o-Wfa6e1qwc zxC9m#jAmT9&CpofG}=(qa7ag)g)U?EGJRshH*X5f!4A!FgB>?SgV3>~^5*-8m>O4x zu!if(^TTwy=!$0H5@Gl@HgxLy!p2gEJB8g$q`6{e7fQ6USSagInm5(Su{guQG1xjl z4m6T!=Nc#fsNK-m&Y@^bl+D{_GNlT>p}Ekcc6Ncxd@GrPV_mAJ(;!`>qNzoy9CkRF zG|f|dJed$FD!P&^KAuYT5>2nwXna6hqM0c*X@m;aXl}N#IUjpvR*Wur2BqA1liy0C zL4rjH`d=5J$WU_5#$F4N$|86X|YAqUnuC;31rhTrVwrv`> zXg_N5!PIohi@_MGLq&B_XO5VkPOTwqXvS}4?t*mshP_YqVqV z-ZSnZJB)kCc0-em{kl=Q-P2Lfjbukb2Gwi|!HsTe=F+AN%CzxKG!gE27<=|;+%W>P z=)MtQetJEFdPS(lio`g^%t9#)87^HvX3)_CDa><41@jAHE_0=Lfw@XlGFONMwD^mn z3T;~@Z*A>P?~u87TX%|&;Ptv!=Qs6-c$2 zNR}S%LCLKye5Uh#H9U0`F=1=B_N4BeF%ld?pn09@S2~5m^U<-MBtJUVi{hp;2k~_c zrw6fYP3=uqGXI*^TaCpumJ#W_X@eS;NuIjG-X!Y^v*>Ow#@X47{~R{`FJ#e^8V}}N z9)y&sm05IYOcckX3I_7$)d*kcgie(4A7@cdHyS?^wb4YToYa&>oopB_?}@{*>T(vv ztr-{iW|LZQbc`7Tj^HPq!vs`64~=tFJEuAMyml0M4j(!30h;YRc3s#(}s10C8fOP6NTf=A6$7qcne#>=%8 zYO>$N-qvWer%i6Yl}*E~xc?fl79p>-XVcV4%vZH>V z#RE@rc@D|s@?5Hjj6e&tqPbcyJ6$L_(Kd4T(p-8dN}hW&m&SY+f>w91+2h%MG%*RK zXyzz{T8g4ka-tl=7@a?9YMt7S(=NuF%a-?}L~|Q5#Z!-z^~?LykPzMQQ+E>&ZR?dsM+wWKF8YhF z(bN59c~%}>#_xAjiR1~Vo?w))ah{;!iE^;EBSTjpx@ZNm7VTn~eI2)cq zJ|(6jr;B`~F2n2cDepoEu8Z22tr$ep1g3ImU^E7+u^B8vW&OrMwAv@9j2cWEM7Vl1 z<3{SytY$hM&(c0=Fuk!wHGE)Y!KJph52h=cTx~6&1q)44T|kefppT0~9}XQJO3W56 z2`xsk1crs8`3Q>x8g(VkoDtz^TU$V~ZS4@cI4B&4qkbL^Mqn7=>ffCnLTmLX7O^4* z6^TPd>;mIdLorsay*PwM6uJT@2fk|r29s>_{+rgr{dXi;g6IeWI zhEn9;77x#ySBDZ}n2O;lMv$EI>PY(jQV0jztTywhrwb`j)*|PmVpixP5pPcxPa(ZA z(#=Vn$oN+SrVDl)Aa^$w(v~rdacxzP+MhIYN=gxpHFqN?YFN&~(Nb8{^sYs8#VOnR z7SUm=$Jeijq_5v7>S+JpFZ)5(XxbBoP6|>35ev%CuX&4xkEZcv0}{&x9z$-_?iTiz z+5=CE2Y6ZpVeYCIv-_S^#iI%F3l$HmC?>h8xP%(J>3l|O#ArXQozdj1xg|6SQwEPJ z+$llC%hStBXx^O2!0Us9g99_$hOHAv%qQDR=xH^()H`ap>R5MRe#^`F;JJ7#Te)&V z^cdQRmk-Bx;tD&ih^yw^_%SrVlbtYz5MxPZCyb*fPFmGW+}Jvf?#E#$=%O25Lpmcn zi+GWtUK%qzb)n-4QHsY(Xc;x^HmeVmQLL5w;pG*jiwSbkXJxeGNDLR?$%4Kqz|*e} zXCbrjIKhA%x@rQ|xbZ4#6G3WPqv6zG(`D1|CeW&323|hZqY!@>mzCd4pfB}6&*Sw~ zK5=~l<*$qi^t^#i5A?ixmZ<)hhZaqwlMC#D9fVwYZz6T^g<~}eX8fy%lX?roWk1|G z2}`;;^Mgr&_qQ;)0;T6Ftb7D-H#w83+#$