diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 00000000..7103e4f6 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,11 @@ +# 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 +*.glb binary 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..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; /** @@ -44,105 +41,100 @@ public static final class FeatureFlags { public static final boolean PROFILING_ENABLED = false; /** Set to false to disable the hopper subsystem entirely. */ - public static final boolean kHopperEnabled = false; + public static final boolean HOPPER_ENABLED = false; } public final class RobotConstants { - public static final double kNominalVoltage = 12.0; + public static final double NOMINAL_VOLTAGE = 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 int DEFAULT_SUPPLY_CURRENT_LIMIT = 40; + public static final int DEFAULT_STATOR_CURRENT_LIMIT = 60; } public static final class NEO550Constants { - public static final AngularVelocity kFreeSpeed = RPM.of(11000); - public static final int kDefaultSupplyCurrentLimit = 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 kFreeSpeed = RPM.of(6784); - public static final int kDefaultSupplyCurrentLimit = 60; - public static final int kDefaultStatorCurrentLimit = 100; + 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 kFreeSpeed = RPM.of(6000); - public static final int kDefaultSupplyCurrentLimit = 60; - public static final int kDefaultStatorCurrentLimit = 100; + public static final int DEFAULT_SUPPLY_CURRENT_LIMIT = 60; + public static final int DEFAULT_STATOR_CURRENT_LIMIT = 100; } } public static final class DIOPorts { // max length is 8 - public static final int[] autonomousModeSelector = {0, 1, 2}; - - public static final int allianceColorSelector = 3; + public static final int[] AUTONOMOUS_MODE_SELECTOR = {0, 1, 2}; - public static final int turretAbsEncoder = 4; - } + public static final int ALLIANCE_COLOR_SELECTOR = 3; - public static final class OIPorts { - public static final int defaultDriver = 0; - public static final int defaultOperator = 1; + 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; // 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 3a232e6f..3468466b 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; @@ -122,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(1, ModuleType.kRev, "PDH"); + new LoggedPowerDistribution(CAN2.PD, ModuleType.kRev, "PD"); private final java.util.Set activeCommands = new java.util.LinkedHashSet<>(); @@ -176,18 +177,18 @@ 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, drive::getFieldRelativeHeading, - new VisionIOPhotonVision(cameraFrontRightName, robotToFrontRightCamera), - new VisionIOPhotonVision(cameraFrontLeftName, robotToFrontLeftCamera), - new VisionIOPhotonVision(cameraBackRightName, robotToBackRightCamera), - new VisionIOPhotonVision(cameraBackLeftName, robotToBackLeftCamera)); + 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, @@ -195,11 +196,11 @@ public Robot() { new TurretIOSpark(), new FlywheelIOTalonFX(), new HoodIOSpark()); - if (FeatureFlags.kHopperEnabled) hopper = new Hopper(new HopperIOReal()); + 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"); @@ -217,22 +218,18 @@ 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, drive::getFieldRelativeHeading, - new VisionIOPhotonVisionSim( - cameraFrontRightName, robotToFrontRightCamera, drive::getPose), - new VisionIOPhotonVisionSim( - cameraFrontLeftName, robotToFrontLeftCamera, drive::getPose), - new VisionIOPhotonVisionSim( - cameraBackRightName, robotToBackRightCamera, drive::getPose), - new VisionIOPhotonVisionSim( - cameraBackLeftName, robotToBackLeftCamera, 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, @@ -241,12 +238,12 @@ public Robot() { new FlywheelIOSimTalonFX(), new HoodIOSimSpark()); feeder = new Feeder(new SpindexerIOSimSpark(), new KickerIOSimSpark()); - if (FeatureFlags.kHopperEnabled) hopper = new Hopper(new HopperIOSim()); + if (FeatureFlags.HOPPER_ENABLED) hopper = new Hopper(new HopperIOSim()); 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)); @@ -283,7 +280,7 @@ public Robot() { new TurretIO() {}, new FlywheelIO() {}, new HoodIO() {}); - if (FeatureFlags.kHopperEnabled) 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; @@ -302,13 +299,13 @@ 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.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(); @@ -352,8 +349,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(); @@ -548,7 +545,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.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 61e1e1ea..376aab37 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(DRIVETRAIN_CONSTANTS.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 @@ -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[] { @@ -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); @@ -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()); @@ -158,7 +158,7 @@ public Drive( public void periodic() { long startNanos = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - odometryLock.lock(); // Prevents odometry updates while reading data + ODOMETRY_LOCK.lock(); // Prevents odometry updates while reading data long t1 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; gyroIO.updateInputs(gyroInputs); long t2 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; @@ -168,7 +168,7 @@ public void periodic() { module.periodic(); } long t4 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - odometryLock.unlock(); + ODOMETRY_LOCK.unlock(); // Stop moving when disabled if (DriverStation.isDisabled()) { @@ -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 8b39a9d5..b4690cf1 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -52,83 +52,86 @@ 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.getKrakenX60Foc(1); + public static final LinearVelocity DRIVETRAIN_SPEED_LIMIT = MetersPerSecond.of( 0.9 - * (wheelRadius.in(Meters) * 2.0 * Math.PI) - * KrakenX60Constants.kFreeSpeed.in(RotationsPerSecond) - / driveMotorReduction); + * (WHEEL_RADIUS_METERS * 2.0 * Math.PI) + * DRIVE_GEARBOX.freeSpeedRadPerSec + / (2.0 * Math.PI) + / 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 = MetersPerSecondPerSecond.of(3.0); + Math.min( + 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 kCoupleRatio drive motor turns; - private static final double kCoupleRatio = (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.getKrakenX60Foc(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( - wheelRadius.in(Meters), - drivetrainSpeedLimit.in(MetersPerSecond), - wheelCOF, - driveGearbox.withReduction(driveMotorReduction), - KrakenX60Constants.kDefaultSupplyCurrentLimit, + 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) @@ -139,153 +142,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 kSteerClosedLoopOutput = + 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 kDriveClosedLoopOutput = + private static final ClosedLoopOutputType DRIVE_CLOSED_LOOP_OUTPUT = ClosedLoopOutputType.TorqueCurrentFOC; // The type of motor used for the drive motor - private static final DriveMotorArrangement kDriveMotorType = + private static final DriveMotorArrangement DRIVE_MOTOR_TYPE = 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 STEER_MOTOR_TYPE = 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 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 kSlipCurrent = 120; + static final int SLIP_CURRENT = 120; // Hardware stator current limit for drive motors - static final int kDriveStatorCurrentLimit = KrakenX60Constants.kDefaultStatorCurrentLimit; + 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 kSteerStatorCurrentLimit = 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(kSlipCurrent) - .withPeakReverseTorqueCurrent(-kSlipCurrent)) + .withPeakForwardTorqueCurrent(SLIP_CURRENT) + .withPeakReverseTorqueCurrent(-SLIP_CURRENT)) .withCurrentLimits( new CurrentLimitsConfigs() - .withStatorCurrentLimit(kDriveStatorCurrentLimit) + .withStatorCurrentLimit(DRIVE_STATOR_CURRENT_LIMIT) .withStatorCurrentLimitEnable(true) - .withSupplyCurrentLimit(KrakenX60Constants.kDefaultSupplyCurrentLimit) + .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(kSteerStatorCurrentLimit) + .withStatorCurrentLimit(STEER_STATOR_CURRENT_LIMIT) .withStatorCurrentLimitEnable(true) - .withSupplyCurrentLimit(KrakenX60Constants.kDefaultSupplyCurrentLimit) + .withSupplyCurrentLimit(KrakenX60Constants.DEFAULT_SUPPLY_CURRENT_LIMIT) .withSupplyCurrentLimitEnable(true)); - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = 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 kSteerInertia = KilogramSquareMeters.of(0.004); - private static final MomentOfInertia kDriveInertia = 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 kSteerFrictionVoltage = Volts.of(0.2); - private static final Voltage kDriveFrictionVoltage = 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 = - new SwerveDrivetrainConstants().withCANBusName(CANHD.bus.getName()); + 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(kCoupleRatio) - .withWheelRadius(wheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(Amps.of(kSlipCurrent)) - .withSpeedAt12Volts(drivetrainSpeedLimit) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(turnInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); + .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( - CANHD.frontLeftTurn, - CANHD.frontLeftDrive, - CANHD.frontLeftTurnAbsEncoder, + FRONT_LEFT = + CONSTANT_CREATOR.createModuleConstants( + CANHD.FRONT_LEFT_TURN, + CANHD.FRONT_LEFT_DRIVE, + CANHD.FRONT_LEFT_TURN_ABS_ENC, Rotations.of(0), - wheelBase.div(2.0), - trackWidth.div(2.0), - kInvertLeftSide, - 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( - CANHD.frontRightTurn, - CANHD.frontRightDrive, - CANHD.frontRightTurnAbsEncoder, + FRONT_RIGHT = + CONSTANT_CREATOR.createModuleConstants( + CANHD.FRONT_RIGHT_TURN, + CANHD.FRONT_RIGHT_DRIVE, + CANHD.FRONT_RIGHT_TURN_ABS_ENC, Rotations.of(0), - wheelBase.div(2.0), - trackWidth.div(-2.0), - kInvertRightSide, - 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( - CANHD.backLeftTurn, - CANHD.backLeftDrive, - CANHD.backLeftTurnAbsEncoder, + BACK_LEFT = + CONSTANT_CREATOR.createModuleConstants( + CANHD.BACK_LEFT_TURN, + CANHD.BACK_LEFT_DRIVE, + CANHD.BACK_LEFT_TURN_ABS_ENC, Rotations.of(0), - wheelBase.div(-2.0), - trackWidth.div(2.0), - kInvertLeftSide, - 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( - CANHD.backRightTurn, - CANHD.backRightDrive, - CANHD.backRightTurnAbsEncoder, + BACK_RIGHT = + CONSTANT_CREATOR.createModuleConstants( + CANHD.BACK_RIGHT_TURN, + CANHD.BACK_RIGHT_DRIVE, + CANHD.BACK_RIGHT_TURN_ABS_ENC, Rotations.of(0), - wheelBase.div(-2.0), - trackWidth.div(-2.0), - kInvertRightSide, - 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 @@ -293,7 +296,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/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/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java index dc6c1eab..e1b24426 100644 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/Module.java @@ -27,46 +27,43 @@ 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(ZERO_ROTATION_KEY + name, turnZeroFromCancoder.getRadians()); Rotation2d turnZeroFromPreferences = new Rotation2d( - Preferences.getDouble(zeroRotationKey + index, turnZeroFromCancoder.getRadians())); + Preferences.getDouble(ZERO_ROTATION_KEY + 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; io.updateInputs(inputs); long t1 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; - Logger.processInputs("Drive/Module" + Integer.toString(index), inputs); + Logger.processInputs("Drive/Module" + name, inputs); 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); } @@ -74,8 +71,8 @@ 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); + Logger.recordOutput("Faults/Module" + name + "/DriveDisconnected", !inputs.driveConnected); + Logger.recordOutput("Faults/Module" + name + "/TurnDisconnected", !inputs.turnConnected); long t3 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Profiling output @@ -84,7 +81,7 @@ public void periodic() { if (totalMs > 2) { System.out.println( "[Module" - + index + + name + "] updateInputs=" + (t1 - t0) / 1_000_000 + "ms log=" @@ -103,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); } @@ -126,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). */ @@ -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(ZERO_ROTATION_KEY + name, newTurnZero.getRadians()); + Logger.recordOutput("Drive/Module" + name + "/TurnZeroRad", newTurnZero.getRadians()); } } 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/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/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 92640fc9..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() { @@ -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/FeederConstants.java b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java index a39b2aca..77e137ef 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java @@ -3,55 +3,49 @@ 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 { // Geometry - public static final Distance radius = Inches.of(3.0); + public static final Distance RADIUS = Inches.of(3.0); // Motor - public static final double motorReduction = 1.0; - public static final LinearVelocity maxTangentialVelocity = - MetersPerSecond.of( - NEOVortexConstants.kFreeSpeed.in(RadiansPerSecond) - * radius.in(Meters) - / motorReduction); + 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(GEARBOX.freeSpeedRadPerSec * RADIUS.in(Meters) / 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 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 { // Geometry - public static final Distance radius = Inches.of(1.5); + public static final Distance RADIUS = Inches.of(1.5); // Motor - public static final double motorReduction = 5.0; - public static final LinearVelocity maxTangentialVelocity = - MetersPerSecond.of( - NEOVortexConstants.kFreeSpeed.in(RadiansPerSecond) - * radius.in(Meters) - / motorReduction); + 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(GEARBOX.freeSpeedRadPerSec * RADIUS.in(Meters) / 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 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..592734a6 100644 --- a/src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/feeder/KickerIOSimSpark.java @@ -34,29 +34,30 @@ 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(); config .inverted(false) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.kDefaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.kNominalVoltage); + .smartCurrentLimit(NEOVortexConstants.DEFAULT_STATOR_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(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, MOTOR_REDUCTION), + GEARBOX); } @Override @@ -69,24 +70,24 @@ 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()); } @Override public void setOpenLoop(Voltage volts) { - flexSim.setAppliedOutput(volts.in(Volts) / RobotConstants.kNominalVoltage); + flexSim.setAppliedOutput(volts.in(Volts) / RobotConstants.NOMINAL_VOLTAGE); } @Override public void setVelocity(LinearVelocity tangentialVelocity) { double feedforwardVolts = - RobotConstants.kNominalVoltage + RobotConstants.NOMINAL_VOLTAGE * tangentialVelocity.in(MetersPerSecond) - / maxTangentialVelocity.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 a1b7e430..06dd3a05 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(); @@ -40,17 +40,17 @@ public KickerIOSpark() { config .inverted(false) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.kDefaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.kNominalVoltage); + .smartCurrentLimit(NEOVortexConstants.DEFAULT_STATOR_CURRENT_LIMIT) + .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); config .encoder - .positionConversionFactor(encoderPositionFactor) - .velocityConversionFactor(encoderVelocityFactor) + .positionConversionFactor(ENCODER_POSITION_FACTOR) + .velocityConversionFactor(ENCODER_VELOCITY_FACTOR) .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, @@ -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(); } @@ -79,11 +79,11 @@ public void setOpenLoop(Voltage volts) { @Override public void setVelocity(LinearVelocity tangentialVelocity) { double feedforwardVolts = - RobotConstants.kNominalVoltage + RobotConstants.NOMINAL_VOLTAGE * tangentialVelocity.in(MetersPerSecond) - / maxTangentialVelocity.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 dfd30bad..d674b6ee 100644 --- a/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/feeder/SpindexerIOSimSpark.java @@ -34,30 +34,30 @@ 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(); config .inverted(false) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.kDefaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.kNominalVoltage); + .smartCurrentLimit(NEOVortexConstants.DEFAULT_STATOR_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(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, MOTOR_REDUCTION), + GEARBOX); } @Override @@ -71,24 +71,24 @@ 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()); } @Override public void setOpenLoop(Voltage volts) { - flexSim.setAppliedOutput(volts.in(Volts) / RobotConstants.kNominalVoltage); + flexSim.setAppliedOutput(volts.in(Volts) / RobotConstants.NOMINAL_VOLTAGE); } @Override public void setVelocity(LinearVelocity tangentialVelocity) { double feedforwardVolts = - RobotConstants.kNominalVoltage + RobotConstants.NOMINAL_VOLTAGE * tangentialVelocity.in(MetersPerSecond) - / maxTangentialVelocity.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 5b6403f8..70465a13 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(); @@ -40,17 +40,17 @@ public SpindexerIOSpark() { config .inverted(false) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEOVortexConstants.kDefaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.kNominalVoltage); + .smartCurrentLimit(NEOVortexConstants.DEFAULT_STATOR_CURRENT_LIMIT) + .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); config .encoder - .positionConversionFactor(encoderPositionFactor) - .velocityConversionFactor(encoderVelocityFactor) + .positionConversionFactor(ENCODER_POSITION_FACTOR) + .velocityConversionFactor(ENCODER_VELOCITY_FACTOR) .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, @@ -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(); } @@ -79,11 +79,11 @@ public void setOpenLoop(Voltage volts) { @Override public void setVelocity(LinearVelocity tangentialVelocity) { double feedforwardVolts = - RobotConstants.kNominalVoltage + RobotConstants.NOMINAL_VOLTAGE * tangentialVelocity.in(MetersPerSecond) - / maxTangentialVelocity.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 8b89b3ed..c695723a 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -3,42 +3,52 @@ 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.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 rollerRadius = Inches.of(0.85); - - // motor controller - public static final double motorReduction = 1.0; - public static final int numMotors = 1; - public static final double maxAcceleration = 4000.0; - public static final double maxJerk = 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 - - // 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 class RollerConfig { - public final int port; - public final CANBus bus; - public final boolean inverted; + public static final Distance RADIUS = Inches.of(0.85); + + // 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; + + // 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 + + // Configs + public record RollerConfig(int port, CANBus bus, boolean inverted) {} + + 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 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 ef4e0449..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,18 +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 double KP = 0.11; - private static final double KD = 0.0; - private static final LinearVelocity maxTangentialVelocity = - MetersPerSecond.of( - NEOVortexConstants.kFreeSpeed.in(RadiansPerSecond) - * rollerRadius.in(Meters) - / motorReduction); - private static final DCMotor gearbox = DCMotor.getNeoVortex(numMotors); private final DCMotorSim rollerSim; @@ -43,29 +34,30 @@ 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.kDefaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.kNominalVoltage); + .smartCurrentLimit(NEOVortexConstants.DEFAULT_STATOR_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); + 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, MOTOR_REDUCTION), + GEARBOX); } @Override @@ -78,24 +70,24 @@ 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()); } @Override public void setOpenLoop(Voltage volts) { - flexSim.setAppliedOutput(volts.in(Volts) / RobotConstants.kNominalVoltage); + flexSim.setAppliedOutput(volts.in(Volts) / RobotConstants.NOMINAL_VOLTAGE); } @Override public void setVelocity(LinearVelocity tangentialVelocity) { double feedforwardVolts = - RobotConstants.kNominalVoltage + RobotConstants.NOMINAL_VOLTAGE * tangentialVelocity.in(MetersPerSecond) - / maxTangentialVelocity.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 87096d02..72c42814 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOSimTalonFX.java @@ -2,12 +2,11 @@ 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; 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; @@ -16,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; @@ -25,18 +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 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 final DCMotorSim rollerSim; private final TalonFX motor; @@ -56,20 +46,27 @@ 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 = velocityVoltageGains; - config.Slot1 = velocityTorqueCurrentGains; + 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 = 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 +92,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) * RADIUS.in(Meters) / MOTOR_REDUCTION; } @Override @@ -117,7 +114,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 / 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 48357e13..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,44 +20,35 @@ 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 double KP = 0.001; - private static final double KD = 0.0; - private static final LinearVelocity maxTangentialVelocity = - MetersPerSecond.of( - NEOVortexConstants.kFreeSpeed.in(RadiansPerSecond) - * rollerRadius.in(Meters) - / motorReduction); - 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.kDefaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.kNominalVoltage); + .smartCurrentLimit(NEOVortexConstants.DEFAULT_STATOR_CURRENT_LIMIT) + .voltageCompensation(RobotConstants.NOMINAL_VOLTAGE); config .encoder - .positionConversionFactor(encoderPositionFactor) - .velocityConversionFactor(encoderVelocityFactor) + .positionConversionFactor(ENCODER_POSITION_FACTOR) + .velocityConversionFactor(ENCODER_VELOCITY_FACTOR) .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 +62,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(); } @@ -84,11 +76,11 @@ public void setOpenLoop(Voltage volts) { @Override public void setVelocity(LinearVelocity tangentialVelocity) { double feedforwardVolts = - RobotConstants.kNominalVoltage + RobotConstants.NOMINAL_VOLTAGE * tangentialVelocity.in(MetersPerSecond) - / maxTangentialVelocity.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 05cc7657..0c3ff7d3 100644 --- a/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/RollerIOTalonFX.java @@ -2,12 +2,11 @@ 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; 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; @@ -22,16 +21,8 @@ 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 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 final TalonFX motor; private final TalonFXConfiguration config; private final Debouncer connectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); @@ -42,28 +33,29 @@ 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; 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.kDefaultStatorCurrentLimit; - config.TorqueCurrent.PeakReverseTorqueCurrent = -KrakenX60Constants.kDefaultStatorCurrentLimit; - config.CurrentLimits.StatorCurrentLimit = KrakenX60Constants.kDefaultStatorCurrentLimit; + 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.kDefaultSupplyCurrentLimit; + 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); - 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(); @@ -85,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) / motorReduction; + velocity.getValue().in(RadiansPerSecond) * RADIUS.in(Meters) / MOTOR_REDUCTION; } @Override @@ -101,7 +93,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 / 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 e3b38c58..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 { @@ -50,14 +51,21 @@ 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) .withNeutralMode(NeutralModeValue.Brake); - config.Slot0 = velocityVoltageGains; - config.Slot1 = velocityTorqueCurrentGains; + 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)); @@ -67,8 +75,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(); @@ -78,7 +86,7 @@ public FlywheelIOSimTalonFX() { 50.0, flywheelVelocity, flywheelAppliedVolts, flywheelCurrent); flywheelFollowerTalon.setControl( - new Follower(CAN2.flywheelLeader, MotorAlignmentValue.Opposed)); + new Follower(CAN2.FLYWHEEL_LEADER, MotorAlignmentValue.Opposed)); } @Override @@ -95,14 +103,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 +122,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 26a8c409..f5fcd129 100644 --- a/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimWPI.java +++ b/src/main/java/frc/robot/subsystems/launcher/FlywheelIOSimWPI.java @@ -13,16 +13,19 @@ 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; public FlywheelIOSimWPI() { flywheelSim = - new DCMotorSim(LinearSystemId.createDCMotorSystem(gearbox, 0.004, motorReduction), gearbox); + new DCMotorSim( + LinearSystemId.createDCMotorSystem(GEARBOX, 0.004, MOTOR_REDUCTION), GEARBOX); } @Override @@ -39,13 +42,13 @@ public void updateInputs(FlywheelIOInputs inputs) { // Update simulation state flywheelSim.setInputVoltage( MathUtil.clamp( - appliedVolts, -RobotConstants.kNominalVoltage, RobotConstants.kNominalVoltage)); + 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()); } @@ -60,9 +63,9 @@ public void setOpenLoop(Voltage volts) { public void setVelocity(LinearVelocity tangentialVelocity) { closedLoop = true; this.feedforwardVolts = - RobotConstants.kNominalVoltage + 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 9ea59578..d2cc3d30 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; @@ -48,19 +48,20 @@ 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) .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.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.kDefaultSupplyCurrentLimit; + 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)); @@ -86,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 @@ -106,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) @@ -128,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 daa1885f..25c42a0b 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(); @@ -45,13 +45,13 @@ public HoodIOSimSpark() { hoodConfig .inverted(false) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEO550Constants.kDefaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.kNominalVoltage); + .smartCurrentLimit(NEO550Constants.DEFAULT_STATOR_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.kNominalVoltage); + 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.kNominalVoltage + 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.kNominalVoltage + 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 80af5024..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.kNominalVoltage, RobotConstants.kNominalVoltage)); + 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.kNominalVoltage + 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 0336c204..0cc78f4c 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(); @@ -47,13 +47,13 @@ public HoodIOSpark() { hoodConfig .inverted(true) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEO550Constants.kDefaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.kNominalVoltage); + .smartCurrentLimit(NEO550Constants.DEFAULT_STATOR_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.kNominalVoltage + 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.kNominalVoltage + 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 cc4e621c..99ea78dd 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,7 +88,7 @@ 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 @@ -121,21 +121,21 @@ public void periodic() { 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) { @@ -179,7 +179,7 @@ public void aim(Translation3d target) { 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,7 +188,7 @@ 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.PROFILING_ENABLED ? System.nanoTime() : 0; @@ -206,7 +206,8 @@ public void aim(Translation3d target) { ballisticsFromFlywheelSetpoint(flywheelInputs.velocityMetersPerSec); // Replan shot using actual initial shot speed speed - var v0_total = getV0Replanned(vectorTurretBaseToTarget, initialSpeedMetersPerSec, replannedKey); + var v0_total = + getV0Replanned(vectorTurretBaseToTarget, initialSpeedMetersPerSec, REPLANNED_KEY); long t3 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Point turret to align velocity vectors @@ -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.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()); @@ -256,7 +257,7 @@ public void aim(Translation3d target) { // Spawn simulated fuel fuelSpawnTimer += Robot.defaultPeriodSecs; - if (fuelSpawnTimer >= fuelSpawnPeriod && logFuelTrajectories) { + if (fuelSpawnTimer >= FUEL_SPAWN_PERIOD && LOG_FUEL_TRAJECTORIES) { fuelSpawnTimer = 0.0; fuelNominal.add( @@ -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(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(); @@ -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 - 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 @@ -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)) / (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); @@ -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 * GRAVITY); 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 -= GRAVITY * 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 36c7b08f..a01a9238 100644 --- a/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java +++ b/src/main/java/frc/robot/subsystems/launcher/LauncherConstants.java @@ -13,147 +13,137 @@ 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 { // 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 GRAVITY = 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 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); - 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 double MOTOR_REDUCTION = 9.0 * 72.0 / 12.0; + public static final DCMotor GEARBOX = DCMotor.getNeo550(1); + public static final AngularVelocity MAX_ANGULAR_VELOCITY = + 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 } 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.kFreeSpeed.div(motorReduction); - public static final Slot0Configs velocityVoltageGains = + public static final double MOTOR_REDUCTION = 1.0; + public static final DCMotor GEARBOX = DCMotor.getKrakenX60(2); + public static final AngularVelocity MAX_ANGULAR_VELOCITY = + 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 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 double kPSim = 0.1; - 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.kFreeSpeed.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 MOTOR_REDUCTION = 5.0 * 256.0 / 16.0; + public static final DCMotor GEARBOX = DCMotor.getNeo550(1); + public static final AngularVelocity MAX_ANGULAR_VELOCITY = + 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 } } diff --git a/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java b/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java index a0101ab7..db1f0b92 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSimSpark.java @@ -39,7 +39,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(); @@ -47,37 +47,38 @@ public TurretIOSimSpark() { turnConfig .inverted(false) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEO550Constants.kDefaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.kNominalVoltage); + .smartCurrentLimit(NEO550Constants.DEFAULT_STATOR_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 .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, 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()); } @@ -92,13 +93,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; @@ -115,16 +116,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.kNominalVoltage + 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 7b14f37d..48760c27 100644 --- a/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java +++ b/src/main/java/frc/robot/subsystems/launcher/TurretIOSpark.java @@ -47,39 +47,39 @@ 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, - absEncoderOffset.getRadians() + mechanismOffset.getRadians()); + ABS_ENCODER_OFFSET.getRadians() + MECHANISM_OFFSET.getRadians()); var turnConfig = new SparkMaxConfig(); turnConfig .inverted(true) .idleMode(IdleMode.kBrake) - .smartCurrentLimit(NEO550Constants.kDefaultSupplyCurrentLimit) - .voltageCompensation(RobotConstants.kNominalVoltage); + .smartCurrentLimit(NEO550Constants.DEFAULT_STATOR_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 .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); @@ -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.kNominalVoltage + 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/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 c967e830..579e0e46 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,23 +123,23 @@ public Rotation2d getTargetX(int cameraIndex) { @Override public void periodic() { - long visionStart = Constants.FeatureFlags.PROFILING_ENABLED ? 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 = Constants.FeatureFlags.PROFILING_ENABLED ? 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 - 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]); } } - long t2 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t2 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Initialize logging values allTagPoses.clear(); @@ -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()])); @@ -217,13 +217,13 @@ public void periodic() { allRobotPosesRejected.addAll(robotPosesRejected); } - long t3 = Constants.FeatureFlags.PROFILING_ENABLED ? 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 - 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( @@ -254,27 +255,27 @@ public void periodic() { // Clear buffer after processing observationBuffer.clear(); } - long t4 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + 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)); } } - long t5 = Constants.FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; + long t5 = FeatureFlags.PROFILING_ENABLED ? System.nanoTime() : 0; // Profiling output - if (Constants.FeatureFlags.PROFILING_ENABLED) { + if (FeatureFlags.PROFILING_ENABLED) { long totalMs = (t5 - visionStart) / 1_000_000; if (totalMs > 5) { System.out.println( @@ -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 d54b8ac7..77c8d5d2 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -20,81 +20,87 @@ 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; - - // 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"; - - // Robot to camera transforms - public static Transform3d robotToFrontRightCamera = - 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 = - 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 = - 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 = - 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 minRobotWidth = Inches.of(36.875); + public static Boolean USE_CUSTOM_APRIL_TAG_LAYOUT = false; + public static AprilTagFields DEFAULT_APRIL_TAG_FIELD_LAYOUT = AprilTagFields.k2026RebuiltAndymark; + + /** 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); // 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 = - DriveConstants.drivetrainSpeedLimit.in(MetersPerSecond) * 1.5; // Allow some margin + 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 +108,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/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 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());