diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml new file mode 100644 index 0000000..fef6da8 --- /dev/null +++ b/.github/workflows/build.yml @@ -0,0 +1,49 @@ +name: Build & Test + +on: + push: + branches: [main] + pull_request: + branches: [main] + workflow_dispatch: + +jobs: + build: + name: ./gradlew build + runs-on: ubuntu-latest + timeout-minutes: 30 + steps: + - name: Checkout + uses: actions/checkout@v4 + + - name: Set up JDK 17 (matches WPILib 2026) + uses: actions/setup-java@v4 + with: + java-version: '17' + distribution: 'temurin' + + - name: Cache Gradle wrapper + caches + uses: actions/cache@v4 + with: + path: | + ~/.gradle/caches + ~/.gradle/wrapper + key: gradle-${{ runner.os }}-${{ hashFiles('**/*.gradle', 'gradle/wrapper/gradle-wrapper.properties') }} + restore-keys: | + gradle-${{ runner.os }}- + + - name: Make gradlew executable + run: chmod +x ./gradlew + + - name: Build + run all tests + run: ./gradlew build --no-daemon --stacktrace + + - name: Upload test report on failure + if: failure() + uses: actions/upload-artifact@v4 + with: + name: gradle-test-report + path: | + build/reports/tests/test/ + build/test-results/test/ + retention-days: 7 diff --git a/build.gradle b/build.gradle index 322a386..3dab7cd 100644 --- a/build.gradle +++ b/build.gradle @@ -73,11 +73,21 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + testImplementation 'org.assertj:assertj-core:3.26.3' + testImplementation 'org.mockito:mockito-core:5.14.2' + testImplementation 'org.mockito:mockito-junit-jupiter:5.14.2' } test { useJUnitPlatform() systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' + maxParallelForks = Math.max(1, Runtime.runtime.availableProcessors().intdiv(2)) + forkEvery = 50 + testLogging { + events 'failed', 'skipped' + showStandardStreams = false + exceptionFormat = 'full' + } } // Simulation configuration (e.g. environment variables). diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc/robot/Binds.java b/src/main/java/frc/robot/Binds.java index afab588..aa54bd8 100644 --- a/src/main/java/frc/robot/Binds.java +++ b/src/main/java/frc/robot/Binds.java @@ -6,7 +6,6 @@ import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.wpilibj.DriverStation; import frc.robot.commands.AutoAlign; -//import frc.robot.commands.DefenseBoost; import frc.robot.commands.GyroReset; import frc.robot.commands.ShootCommand; import frc.robot.subsystem.Hopper; @@ -69,7 +68,6 @@ public static final void bind(){ HumanControls.OperatorPanel.L1.whileTrue(Intake.get().doOscilateIntake()); HumanControls.OperatorPanel.L2.whileTrue(Swerve.get().xMode()); HumanControls.OperatorPanel.L3.whileTrue(new ShootCommand(()->Field.Alliance_Find.hub)); - //HumanControls.OperatorPanel.L4.whileTrue(new DefenseBoost(120, 50)); HumanControls.OperatorPanel.Processor.whileTrue(Shooter.get().spinUpCommand()); diff --git a/src/main/java/frc/robot/NamedCommandRegistry.java b/src/main/java/frc/robot/NamedCommandRegistry.java new file mode 100644 index 0000000..bcdee22 --- /dev/null +++ b/src/main/java/frc/robot/NamedCommandRegistry.java @@ -0,0 +1,61 @@ +package frc.robot; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.commands.ShootCommand; +import frc.robot.subsystem.Intake; +import frc.robot.subsystem.Shooter; +import frc.robot.util.Auto; +import frc.robot.util.Field; +import java.util.Set; +import java.util.function.Supplier; + +/** + * Single source of truth for every PathPlanner "named command" the robot exposes. + * + *

{@link Robot#Robot()} should build its {@code Auto.NamedCommand[]} from {@link #all()} (so + * adding a new named command happens in exactly one place), and {@link NamedCommandConsistencyTest} + * cross-checks {@link #NAMES} against every {@code .auto} JSON file in the deploy directory so an + * auto referencing a non-existent named command fails at build time instead of silently no-op'ing + * during a match. + */ +public final class NamedCommandRegistry { + private NamedCommandRegistry() {} + + /** Every name that {@link #all()} returns, exposed as a stable set for cross-checking. */ + public static final Set NAMES = + Set.of( + "Shoot", + "ShootForever", + "Shorter Shoot", + "Intake_", + "Intake Forever", + "Do stow", + "Rev up", + "Oscilate", + "OscilateForever", + "Outtake"); + + /** + * Build the live {@link Auto.NamedCommand} array. Lazily constructs each command on call (the + * caller is expected to invoke this exactly once at robot init). + */ + public static Auto.NamedCommand[] all() { + Supplier hub = () -> Field.Alliance_Find.hub; + return new Auto.NamedCommand[] { + named("Shoot", new ShootCommand(hub).withTimeout(5.5)), + named("ShootForever", new ShootCommand(hub)), + named("Shorter Shoot", new ShootCommand(hub).withTimeout(2.5)), + named("Intake_", Intake.get().doIntake().withTimeout(2)), + named("Intake Forever", Intake.get().doIntake()), + named("Do stow", Intake.get().doStow().withTimeout(0.5)), + named("Rev up", Shooter.get().spinUpCommand()), + named("Oscilate", Intake.get().doOscilateIntake().withTimeout(4.0)), + named("OscilateForever", Intake.get().doOscilateIntake()), + named("Outtake", Intake.get().doOuttake().withTimeout(3.5)) + }; + } + + private static Auto.NamedCommand named(String name, Command command) { + return new Auto.NamedCommand(name, command); + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index bc85810..4db4f42 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -14,10 +14,8 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.commands.ShootCommand; import frc.robot.subsystem.Intake; import frc.robot.subsystem.LED; -import frc.robot.subsystem.Shooter; import frc.robot.subsystem.Swerve; import frc.robot.util.Auto; import frc.robot.util.BotConstants; @@ -34,41 +32,23 @@ public class Robot extends TimedRobot { public Robot() { this.logger = new Telemetry(BotConstants.DriveConstants.MaxSpeed); - Auto.initialize( - new Auto.NamedCommand("Shoot", new ShootCommand(()->Field.Alliance_Find.hub).withTimeout(5.5)), - - new Auto.NamedCommand("ShootForever", new ShootCommand(()->Field.Alliance_Find.hub)), - - - new Auto.NamedCommand("Shorter Shoot", new ShootCommand(()->Field.Alliance_Find.hub).withTimeout(2.5)), - - new Auto.NamedCommand("Intake_", Intake.get().doIntake().withTimeout(2)), - - new Auto.NamedCommand("Intake Forever", Intake.get().doIntake()), - - new Auto.NamedCommand("Do stow", Intake.get().doStow().withTimeout(0.5)), - - new Auto.NamedCommand("Rev up", Shooter.get().spinUpCommand()), - - - new Auto.NamedCommand("Oscilate", Intake.get().doOscilateIntake().withTimeout(4.0)), - - new Auto.NamedCommand("OscilateForever", Intake.get().doOscilateIntake()), - - - new Auto.NamedCommand("Outtake", Intake.get().doOuttake().withTimeout(3.5)) - - ); + // The names below are the single source of truth maintained in NamedCommandRegistry; + // NamedCommandConsistencyTest cross-checks them against every .auto JSON file so an + // auto referencing a non-existent named command fails at build time. + Auto.initialize(NamedCommandRegistry.all()); Binds.DriverStation2026.bind(); Binds.OperatorPanel.bind(); + // Brownout threshold is intentionally lowered from the WPILib default of 6.75 V + // to give the motors more sag headroom under simultaneous swerve + shooter + + // intake load. Trade-off: less protection against deeper brown-outs. Revisit if + // we see Rio resets during matches. RobotController.setBrownoutVoltage(6.0); //Binds.Controller.bind(); Intake.get().zeroEncoder(); - Intake.get().SlotZeroConfigIntake(); LED.get().tuffAnimation(); //SignalLogger.start(); //DataLogManager.start(); diff --git a/src/main/java/frc/robot/commands/DefenseBoost.java b/src/main/java/frc/robot/commands/DefenseBoost.java deleted file mode 100644 index 4ea25ec..0000000 --- a/src/main/java/frc/robot/commands/DefenseBoost.java +++ /dev/null @@ -1,55 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystem.Swerve; - - -//Not in use right no, there is no current limit ont the drive - -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ -public class DefenseBoost extends Command { - /** Creates a new DefenseBoost. */ - - private final double boostAmps; - private final double normalAmps; - - - public DefenseBoost(double boostAmps, double normalAmps) { - - this.boostAmps = boostAmps; - this.normalAmps = normalAmps; - - - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - - Swerve.get().setDriveCurrentLimit(boostAmps); - - - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - Swerve.get().setDriveCurrentLimit(normalAmps); - - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ShootCommand.java b/src/main/java/frc/robot/commands/ShootCommand.java index eacdbdf..3812fdd 100644 --- a/src/main/java/frc/robot/commands/ShootCommand.java +++ b/src/main/java/frc/robot/commands/ShootCommand.java @@ -13,62 +13,57 @@ import frc.robot.subsystem.Shooter; import frc.robot.subsystem.Swerve; import frc.robot.util.BotConstants; +import frc.robot.util.ShooterControlPolicy; -/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class ShootCommand extends Command { - Supplier desiredPose; - boolean isAtspeed; - /** Creates a new ShootCommand. */ - public ShootCommand(Supplier desired_pose) { - - desiredPose = desired_pose; + private final Supplier desiredPose; + /** + * Hysteresis latch state. Recomputed per execute() loop by ShooterControlPolicy. + * Reset to false in initialize() so each command instance starts from cold. + */ + private boolean isFiring; - this.addRequirements(Shooter.get(),Hopper.get(), LED.get()); - // Use addRequirements() here to declare subsystem dependencies. + public ShootCommand(Supplier desired_pose) { + this.desiredPose = desired_pose; + addRequirements(Shooter.get(), Hopper.get(), LED.get()); } - // Called when the command is initially scheduled. @Override public void initialize() { - isAtspeed = false; + isFiring = false; } - // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double distMeters=Swerve.get().distTo(desiredPose.get()); - double velocity = BotConstants.Shooter.ShooterTable.get(distMeters); - double intakespeed = BotConstants.Shooter.backSpinTable.get(distMeters); - - Shooter.get().set_velocity(velocity); - + double distMeters = Swerve.get().distTo(desiredPose.get()); + ShooterControlPolicy.Decision decision = ShooterControlPolicy.compute( + distMeters, + Shooter.get().getRollerVelocity(), + BotConstants.Shooter.ShooterTable, + BotConstants.Shooter.backSpinTable, + ShooterControlPolicy.DEFAULT_ENTER_TOL, + ShooterControlPolicy.DEFAULT_EXIT_TOL, + isFiring); + + Shooter.get().set_velocity(decision.flywheelTarget()); LED.get().LEDyellowBlink(); - if((Math.abs(Shooter.get().getRollerVelocity()-velocity))<0.95){ - isAtspeed = true; - } - - - if(isAtspeed){ - Shooter.get().intake_shooter(intakespeed); + isFiring = decision.fireIndexer(); + if (isFiring) { + Shooter.get().intake_shooter(decision.indexerSpeed()); Hopper.get().run_Hopper(); } - } - // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - Hopper.get().Stop(); + Hopper.get().stop(); Shooter.get().Stop(); - } - // Returns true when the command should end. @Override public boolean isFinished() { return false; } - //d } diff --git a/src/main/java/frc/robot/subsystem/Hopper.java b/src/main/java/frc/robot/subsystem/Hopper.java index f988eab..960abe6 100644 --- a/src/main/java/frc/robot/subsystem/Hopper.java +++ b/src/main/java/frc/robot/subsystem/Hopper.java @@ -74,7 +74,20 @@ public Command oscilateHopper(){ } - //Stops + /** + * Synchronously stops the hopper motor right now. Use this from inside other commands' + * {@code end()} / {@code execute()} methods where the caller would otherwise discard the + * {@link Command} returned by {@link #Stop()} and the motor wouldn't actually stop. + */ + public void stop() { + m_Hopper.stopMotor(); + } + + /** + * Returns a Command that holds the motor stopped. ONLY intended for use as the hopper's + * default command in {@code Binds}. Other callers should use {@link #stop()} (void) — see + * HopperStopContractTest. + */ public Command Stop(){ return run(()->{m_Hopper.stopMotor();}); } diff --git a/src/main/java/frc/robot/subsystem/Intake.java b/src/main/java/frc/robot/subsystem/Intake.java index 47641b3..02231db 100644 --- a/src/main/java/frc/robot/subsystem/Intake.java +++ b/src/main/java/frc/robot/subsystem/Intake.java @@ -111,6 +111,14 @@ public void positionIntake(Pivot pivot) { m_IntakePivot.setControl(PivotPositionControl.withPosition(pivot.position).withSlot(0)); // } + /** + * @deprecated Re-applies just the Slot0 PID gains of the pivot config. This was called + * redundantly at robot init on top of the full {@code cfg_Pivot} apply that already + * happens in {@link #Intake()}, and risks clobbering motor-controller settings that + * were configured between the full apply and this partial re-apply. Don't call this + * unless you're hot-tuning gains and know exactly what you're doing. + */ + @Deprecated public void SlotZeroConfigIntake(){ m_IntakePivot.getConfigurator().apply(BotConstants.Intake.cfg_Pivot.Slot0); } diff --git a/src/main/java/frc/robot/subsystem/Shooter.java b/src/main/java/frc/robot/subsystem/Shooter.java index 283d3a6..3bbb5b4 100644 --- a/src/main/java/frc/robot/subsystem/Shooter.java +++ b/src/main/java/frc/robot/subsystem/Shooter.java @@ -93,7 +93,7 @@ public void intake_shooter(double speed){ public Command idle(){ - return run(()->{this.Stop(); Hopper.get().Stop();}); + return run(()->{this.Stop(); Hopper.get().stop();}); } //Commented out, runtime overrun error @@ -140,7 +140,7 @@ public Command ShootPass(){ } else{ m_ShooterIntake.stopMotor(); - Hopper.get().Stop(); + Hopper.get().stop(); } }, @@ -148,7 +148,7 @@ public Command ShootPass(){ m_Shooter.stopMotor(); m_Shooter_2.stopMotor(); m_ShooterIntake.stopMotor(); - Hopper.get().Stop(); + Hopper.get().stop(); }); } @@ -201,7 +201,7 @@ public void Stop(){ m_Shooter.stopMotor(); m_Shooter_2.stopMotor(); m_ShooterIntake.stopMotor(); - Hopper.get().Stop(); + Hopper.get().stop(); } @@ -241,14 +241,30 @@ public void simulationPeriodic(){ m_Shooter.getSimState().setSupplyVoltage(12.0); } + /** + * Aggregates the two flywheel motors' signed rotor velocities into a single "true" flywheel + * velocity reading. Extracted as a static helper so it can be unit-tested without Phoenix6 sim. + * + *

The previous implementation returned {@code (leader + follower) / 2}, but the follower is + * set up via {@code Follower(..., MotorAlignmentValue.Opposed)} so its signed velocity is the + * negation of the leader's; averaging cancelled to ~0 at speed and prevented ShootCommand + * from ever tripping its at-speed threshold. The fix is to use only the leader. The follower + * parameter is retained in the signature so the call-site documents both signals and so a + * future regression to averaging is locally visible. See ShooterRollerVelocityTest. + */ + public static double aggregateRollerVelocity(double leaderVelocity, double followerVelocity) { + return leaderVelocity; + } + @Override public void periodic() { // Refresh status signals once per loop and cache the values to avoid // multiple blocking refresh() calls in other methods/commands which can // overload the CAN/communication bus and cause scheduler overruns. - //Made with great assistance by copilot try { - cachedRollerVelocity = (velocity_roller.refresh().getValueAsDouble() + velocity_roller_2.refresh().getValueAsDouble()) / 2.0; + cachedRollerVelocity = aggregateRollerVelocity( + velocity_roller.refresh().getValueAsDouble(), + velocity_roller_2.refresh().getValueAsDouble()); } catch (RuntimeException e) { // Be defensive: if refresh fails, keep last value and log to dashboard SmartDashboard.putString("Shooter/VelocityRefreshError", e.getMessage()); diff --git a/src/main/java/frc/robot/subsystem/Swerve.java b/src/main/java/frc/robot/subsystem/Swerve.java index 848d82a..1d18a34 100644 --- a/src/main/java/frc/robot/subsystem/Swerve.java +++ b/src/main/java/frc/robot/subsystem/Swerve.java @@ -17,7 +17,6 @@ import com.pathplanner.lib.path.PathConstraints; import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -35,12 +34,15 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; +import java.util.Set; import frc.robot.TunerConstants; import frc.robot.TunerConstants.TunerSwerveDrivetrain; import frc.robot.util.BaseCam.AprilTagResult; import frc.robot.util.Field; import frc.robot.util.LimeLightCam; +import frc.robot.util.VisionFilter; public final class Swerve extends TunerSwerveDrivetrain implements Subsystem, Sendable { private static Swerve m_Swerve; @@ -110,27 +112,15 @@ void cameraLocalization(){ } boolean acceptEstimate(AprilTagResult latestResult) { - if (latestResult.distToTag > 3.5) - return false; - SmartDashboard.putBoolean("Accepted", false); - if (latestResult.ambiguity > 0.7) - return false; // Too Ambiguous, Ignore - SmartDashboard.putBoolean("Accepted", false); - - if (getState().Speeds.omegaRadiansPerSecond > 1.5) - return false; // Rotating too fast, ignore - SmartDashboard.putBoolean("Accepted", false); - - if (latestResult.distToTag < 0.5) { - setVisionMeasurementStdDevs(VecBuilder.fill(0.3, .3, 50.0)); - } else { - setVisionMeasurementStdDevs( - VecBuilder.fill(latestResult.ambiguity * Math.pow(latestResult.distToTag, 2)*3.0, - latestResult.ambiguity * Math.pow(latestResult.distToTag, 2)*3.0, - latestResult.ambiguity * Math.pow(latestResult.distToTag, 2)*3.0)); - } - SmartDashboard.putBoolean("Accepted", true); - return true; + VisionFilter.Decision decision = VisionFilter.evaluate( + latestResult.distToTag, + latestResult.ambiguity, + getState().Speeds.omegaRadiansPerSecond); + if (decision.accept()) { + setVisionMeasurementStdDevs(decision.stdDevs()); + } + SmartDashboard.putBoolean("Accepted", decision.accept()); + return decision.accept(); } @@ -244,19 +234,21 @@ private void simulationInit() { - // Since we are using a holonomic drivetrain, the rotation component of this pose - // represents the goal holonomic rotation - Pose2d targetPose = Field.Alliance_Find.climb_tower; - + // Note: do NOT cache the climb target pose at construction time. Swerve is built before + // the FMS reports the alliance, so any cached Field.Alliance_Find.climb_tower would lock + // in the default (blue) value forever. alignToClimb() below defers the lookup until the + // command is scheduled (alliance known by then). See AlignToClimbTest. PathConstraints constraints = new PathConstraints( 0.5, 0.7, Units.degreesToRadians(5), Units.degreesToRadians(10)); public Command alignToClimb(){ - return AutoBuilder.pathfindToPose( - targetPose, - constraints, - 0.0); -} + return Commands.defer( + () -> AutoBuilder.pathfindToPose( + Field.Alliance_Find.climb_tower, + constraints, + 0.0), + Set.of(this)); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/util/Auto.java b/src/main/java/frc/robot/util/Auto.java index d889085..6440c1f 100644 --- a/src/main/java/frc/robot/util/Auto.java +++ b/src/main/java/frc/robot/util/Auto.java @@ -4,7 +4,6 @@ import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; -import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; @@ -30,12 +29,13 @@ public static void initialize(NamedCommand... Commands) { command.register(); if (configureAutoBuilder()) { + // buildAutoChooser() auto-loads every .auto file in + // src/main/deploy/pathplanner/autos/ by its on-disk name (including + // "Double Swipe Bravo Auto"). The previous explicit addOption calls + // added duplicate entries with mislabeled names (label "...Bravo..." + // silently loaded "...Alpha..."). See AutoChooserConsistencyTest. autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); - - autoChooser.addOption("Double Swipe Bravo Auto", new PathPlannerAuto("Double Swipe Alpha Auto", true)); - autoChooser.addOption("Single Swipe Bravo Auto", new PathPlannerAuto("Single Swipe Alpha Auto", true)); - autoChooser.addOption("Double Swipe Bravo Auto Reset Odometry", new PathPlannerAuto("Double Swipe Alpha Auto Reset Odometry", true)); } initialized = true; diff --git a/src/main/java/frc/robot/util/Field.java b/src/main/java/frc/robot/util/Field.java index 7f38831..0e23726 100644 --- a/src/main/java/frc/robot/util/Field.java +++ b/src/main/java/frc/robot/util/Field.java @@ -34,7 +34,7 @@ public class Field { public final static Translation2d hub_position_red = new Translation2d(11.924, 4.048); public final static Translation2d pass_position_red_1 = new Translation2d(15.48, 7.369); public final static Translation2d pass_position_red_2 = new Translation2d(15.715, 0.740); - public final static Pose2d climb_tower_red = new Pose2d(15.478,5201, Rotation2d.fromDegrees(0)); + public final static Pose2d climb_tower_red = new Pose2d(15.478, 5.201, Rotation2d.fromDegrees(0)); diff --git a/src/main/java/frc/robot/util/ShooterControlPolicy.java b/src/main/java/frc/robot/util/ShooterControlPolicy.java new file mode 100644 index 0000000..92b229a --- /dev/null +++ b/src/main/java/frc/robot/util/ShooterControlPolicy.java @@ -0,0 +1,51 @@ +package frc.robot.util; + +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; + +/** + * Pure-logic decision for the shooter: + * + *

+ * + *

Extracted from {@link frc.robot.commands.ShootCommand} so the latch logic is testable + * exhaustively with no HAL or mocks. The default enter/exit tolerances are exposed as constants + * so {@code ShootCommand} and the test stay in lock-step. + */ +public final class ShooterControlPolicy { + private ShooterControlPolicy() {} + + /** Default enter band — tight, matches the original 0.95 from ShootCommand. */ + public static final double DEFAULT_ENTER_TOL = 0.95; + + /** Default exit band — wider than enter so a brief sag doesn't drop the indexer. */ + public static final double DEFAULT_EXIT_TOL = 1.5; + + public static Decision compute( + double distMeters, + double measuredVel, + InterpolatingDoubleTreeMap velTable, + InterpolatingDoubleTreeMap indexerTable, + double enterTol, + double exitTol, + boolean wasFiring) { + double target = velTable.get(distMeters); + double indexer = indexerTable.get(distMeters); + double err = Math.abs(measuredVel - target); + boolean fire = wasFiring ? (err < exitTol) : (err < enterTol); + return new Decision(target, indexer, fire); + } + + /** + * @param flywheelTarget commanded flywheel velocity (rotations/second, signed) + * @param indexerSpeed commanded indexer velocity (only meaningful when {@code fireIndexer}) + * @param fireIndexer true → run the indexer + hopper; false → keep them stopped + */ + public record Decision(double flywheelTarget, double indexerSpeed, boolean fireIndexer) {} +} diff --git a/src/main/java/frc/robot/util/VisionFilter.java b/src/main/java/frc/robot/util/VisionFilter.java new file mode 100644 index 0000000..a5448f8 --- /dev/null +++ b/src/main/java/frc/robot/util/VisionFilter.java @@ -0,0 +1,60 @@ +package frc.robot.util; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; + +/** + * Pure-logic AprilTag vision-measurement filter, extracted from {@code Swerve.acceptEstimate} so + * it can be exhaustively unit-tested with no HAL and no mocks. + * + *

The original implementation: + * 

+ *   if (distToTag > 3.5)       return false;
+ *   if (ambiguity > 0.7)       return false;
+ *   if (omegaRadPerSec > 1.5)  return false;       // bug: not abs()-wrapped
+ *   if (distToTag < 0.5) setStdDevs(0.3, 0.3, 50)
+ *   else                 setStdDevs(amb * dist^2 * 3, ...)
+ *
+ * + *

This extraction preserves the original distance and ambiguity bounds exactly, and uses the + * absolute value of omega so a fast negative rotation is also rejected (the original + * comparison was a sign-sensitive {@code > 1.5}; this is the conservative fix). + */ +public final class VisionFilter { + private VisionFilter() {} + + public static final double MAX_DIST_M = 3.5; + public static final double MAX_AMBIGUITY = 0.7; + public static final double MAX_ABS_OMEGA_RAD_PER_SEC = 1.5; + public static final double CLOSE_RANGE_DIST_M = 0.5; + private static final Matrix CLOSE_RANGE_STDDEV = VecBuilder.fill(0.3, 0.3, 50.0); + + /** + * @param distToTag meters from camera to nearest tag + * @param ambiguity 0..1 ambiguity score from the pose estimator + * @param omegaRadPerSec robot angular velocity (sign-insensitive — abs() is taken internally) + */ + public static Decision evaluate(double distToTag, double ambiguity, double omegaRadPerSec) { + if (distToTag > MAX_DIST_M) return Decision.reject(); + if (ambiguity > MAX_AMBIGUITY) return Decision.reject(); + if (Math.abs(omegaRadPerSec) > MAX_ABS_OMEGA_RAD_PER_SEC) return Decision.reject(); + + Matrix stdDevs; + if (distToTag < CLOSE_RANGE_DIST_M) { + stdDevs = CLOSE_RANGE_STDDEV; + } else { + double s = ambiguity * distToTag * distToTag * 3.0; + stdDevs = VecBuilder.fill(s, s, s); + } + return new Decision(true, stdDevs); + } + + /** A decision returned by {@link #evaluate}: whether to accept, and (if so) what stddev to use. */ + public record Decision(boolean accept, Matrix stdDevs) { + public static Decision reject() { + return new Decision(false, null); + } + } +} diff --git a/src/main/java/frc/robot/util/constants.java b/src/main/java/frc/robot/util/constants.java deleted file mode 100644 index 0f5ce00..0000000 --- a/src/main/java/frc/robot/util/constants.java +++ /dev/null @@ -1,36 +0,0 @@ -package frc.robot.util; - -import static edu.wpi.first.units.Units.*; - - -import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import frc.robot.TunerConstants; - - -public class constants { - - public class FieldConstants{ - public static final Field2d SIM = new Field2d(); - - public static final Distance LENGTH = Feet.of(57.53); - public static final Distance WIDTH = Feet.of(26.75); - - // public final static Translation2d hub_position = new Translation2d(Units.inchesToMeters(182.11), - // Units.inchesToMeters(158.84)); - - } - - public class DriveConstants { - public final static double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed - - public final static double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity - - public static final double kToleranceDegree = 0.5; - public static final double kToleranceSpeed = 0.01; - - - - } - -} \ No newline at end of file diff --git a/src/test/java/frc/robot/NamedCommandConsistencyTest.java b/src/test/java/frc/robot/NamedCommandConsistencyTest.java new file mode 100644 index 0000000..c09570a --- /dev/null +++ b/src/test/java/frc/robot/NamedCommandConsistencyTest.java @@ -0,0 +1,111 @@ +package frc.robot; + +import static org.assertj.core.api.Assertions.assertThat; + +import com.fasterxml.jackson.databind.JsonNode; +import com.fasterxml.jackson.databind.ObjectMapper; +import java.io.IOException; +import java.nio.file.Files; +import java.nio.file.Path; +import java.util.HashSet; +import java.util.Set; +import java.util.TreeSet; +import org.junit.jupiter.api.Test; + +/** + * Layer B — static config validation. + * + *

For every {@code .auto} file under {@code src/main/deploy/pathplanner/autos}, parses the JSON + * and collects every {@code {"type":"named","data":{"name":"X"}}} reference. Asserts every + * referenced name appears in {@link NamedCommandRegistry#NAMES} — the single source of truth for + * which named commands {@link Robot#Robot()} registers with PathPlanner. + * + *

Catches the case where an auto JSON references a named command that nobody registered (or + * a typo'd name like {@code "Shooter"} instead of {@code "Shoot"}). Currently latent in this + * codebase: nothing fails at robot-init for unregistered named commands; they just silently no-op + * when the auto runs. + */ +class NamedCommandConsistencyTest { + private static final Path AUTOS_DIR = Path.of("src/main/deploy/pathplanner/autos"); + private static final Path PATHS_DIR = Path.of("src/main/deploy/pathplanner/paths"); + + @Test + void every_named_command_referenced_in_an_auto_is_registered() throws IOException { + Set referenced = new TreeSet<>(); + try (var stream = Files.list(AUTOS_DIR)) { + for (Path p : stream.filter(x -> x.toString().endsWith(".auto")).toList()) { + JsonNode root = new ObjectMapper().readTree(p.toFile()); + collectNamedCommandRefs(root, referenced); + } + } + assertThat(referenced) + .as("expected to find at least one named command reference across all autos") + .isNotEmpty(); + + Set missing = new TreeSet<>(referenced); + missing.removeAll(NamedCommandRegistry.NAMES); + assertThat(missing) + .as( + "named commands referenced in .auto JSON but never registered via" + + " NamedCommandRegistry / Robot.Robot()") + .isEmpty(); + } + + @Test + void every_path_referenced_in_an_auto_exists_on_disk() throws IOException { + Set referencedPaths = new TreeSet<>(); + try (var stream = Files.list(AUTOS_DIR)) { + for (Path p : stream.filter(x -> x.toString().endsWith(".auto")).toList()) { + JsonNode root = new ObjectMapper().readTree(p.toFile()); + collectPathRefs(root, referencedPaths); + } + } + + Set pathsOnDisk = new HashSet<>(); + try (var stream = Files.list(PATHS_DIR)) { + stream.filter(p -> p.toString().endsWith(".path")) + .forEach(p -> { + String name = p.getFileName().toString(); + pathsOnDisk.add(name.substring(0, name.length() - ".path".length())); + }); + } + + assertThat(referencedPaths) + .as("path names referenced in autos but missing .path file on disk") + .isSubsetOf(pathsOnDisk); + } + + private static void collectNamedCommandRefs(JsonNode node, Set sink) { + if (node == null || node.isNull()) return; + if (node.isObject()) { + JsonNode type = node.get("type"); + JsonNode data = node.get("data"); + if (type != null + && "named".equals(type.asText()) + && data != null + && data.has("name")) { + sink.add(data.get("name").asText()); + } + node.fields().forEachRemaining(e -> collectNamedCommandRefs(e.getValue(), sink)); + } else if (node.isArray()) { + node.forEach(child -> collectNamedCommandRefs(child, sink)); + } + } + + private static void collectPathRefs(JsonNode node, Set sink) { + if (node == null || node.isNull()) return; + if (node.isObject()) { + JsonNode type = node.get("type"); + JsonNode data = node.get("data"); + if (type != null + && "path".equals(type.asText()) + && data != null + && data.has("pathName")) { + sink.add(data.get("pathName").asText()); + } + node.fields().forEachRemaining(e -> collectPathRefs(e.getValue(), sink)); + } else if (node.isArray()) { + node.forEach(child -> collectPathRefs(child, sink)); + } + } +} diff --git a/src/test/java/frc/robot/commands/GyroResetTest.java b/src/test/java/frc/robot/commands/GyroResetTest.java new file mode 100644 index 0000000..dfe67a6 --- /dev/null +++ b/src/test/java/frc/robot/commands/GyroResetTest.java @@ -0,0 +1,60 @@ +package frc.robot.commands; + +import static org.mockito.ArgumentMatchers.eq; +import static org.mockito.Mockito.mock; +import static org.mockito.Mockito.verify; +import static org.mockito.Mockito.when; + +import com.ctre.phoenix6.hardware.Pigeon2; +import frc.robot.subsystem.Swerve; +import org.junit.jupiter.api.Test; +import org.mockito.Mockito; + +/** + * Layer D — Mockito-driven unit test for {@link GyroReset}. + * + *

{@code Swerve} is constructed only by its no-arg singleton getter (which spins up real + * Phoenix6 devices); we can't mock that cleanly. Instead we mock {@code Swerve} and + * {@code Pigeon2} and verify {@link GyroReset#initialize()} calls {@code pigeon.setYaw(angle)}. + * + *

Also verifies {@link GyroReset#isFinished()} is {@code true} (one-shot). + */ +class GyroResetTest { + + @Test + void initialize_calls_setYaw_with_default_zero_angle() { + Swerve swerve = mock(Swerve.class, Mockito.RETURNS_DEEP_STUBS); + Pigeon2 pigeon = mock(Pigeon2.class); + when(swerve.getPigeon2()).thenReturn(pigeon); + + GyroReset cmd = new GyroReset(swerve); + cmd.initialize(); + + verify(pigeon).setYaw(eq(0.0)); + } + + @Test + void initialize_calls_setYaw_with_specified_angle() { + Swerve swerve = mock(Swerve.class); + Pigeon2 pigeon = mock(Pigeon2.class); + when(swerve.getPigeon2()).thenReturn(pigeon); + + GyroReset cmd = new GyroReset(swerve, 90.0); + cmd.initialize(); + + verify(pigeon).setYaw(eq(90.0)); + } + + @Test + void isFinished_is_true_immediately() { + Swerve swerve = mock(Swerve.class); + GyroReset cmd = new GyroReset(swerve); + // No HAL init needed for isFinished — it's a constant return. + // (Construction adds requirements; that just registers with the CommandScheduler in + // production but doesn't need anything special here.) + // Skip directly to the assertion via the contract. + // We intentionally do NOT call initialize here. + // isFinished() should be true regardless of state. + org.assertj.core.api.Assertions.assertThat(cmd.isFinished()).isTrue(); + } +} diff --git a/src/test/java/frc/robot/subsystem/AlignToClimbTest.java b/src/test/java/frc/robot/subsystem/AlignToClimbTest.java new file mode 100644 index 0000000..cbb3a2b --- /dev/null +++ b/src/test/java/frc/robot/subsystem/AlignToClimbTest.java @@ -0,0 +1,75 @@ +package frc.robot.subsystem; + +import static org.assertj.core.api.Assertions.assertThat; + +import java.io.IOException; +import java.lang.reflect.Field; +import java.lang.reflect.Modifier; +import java.nio.file.Files; +import java.nio.file.Path; +import java.util.regex.Pattern; +import org.junit.jupiter.api.Test; + +/** + * Layer B — static contract test for {@link Swerve#alignToClimb()}. + * + *

The original bug: {@code Swerve} had a {@code Pose2d targetPose = Field.Alliance_Find + * .climb_tower} cached at class-construction time. Since {@code Swerve} is a singleton built + * before the FMS reports the alliance, this captured the default (blue) value. {@code + * alignToClimb} then passed that stale value to {@code AutoBuilder.pathfindToPose}, so on the red + * alliance the robot would drive to the blue climb tower. + * + *

The fix is to wrap {@code alignToClimb} in {@code Commands.defer} so the lookup happens at + * schedule-time (after alliance is known), and to delete the cached field. This test pins both: + * + *

    + *
  1. No field of type {@code Pose2d} is declared on {@code Swerve} that names "target" / + * "climb" (would imply someone re-added the cache). + *
  2. {@code Swerve.java} source contains {@code Commands.defer(}, indicating the deferred + * binding is in place. + *
+ */ +class AlignToClimbTest { + @Test + void swerve_does_not_cache_climb_target_pose_at_construction() { + for (Field f : Swerve.class.getDeclaredFields()) { + int mods = f.getModifiers(); + if (Modifier.isStatic(mods)) continue; + String type = f.getType().getSimpleName(); + if (!"Pose2d".equals(type)) continue; + String name = f.getName().toLowerCase(); + assertThat(name) + .as( + "Swerve.%s is a cached Pose2d — alignToClimb()'s climb_tower lookup" + + " must be deferred to schedule-time, not stored in a field" + + " at construction (alliance isn't known yet then). See" + + " AlignToClimbTest.", + f.getName()) + .doesNotContain("target") + .doesNotContain("climb") + .doesNotContain("tower"); + } + } + + @Test + void alignToClimb_uses_Commands_defer_for_lazy_alliance_lookup() throws IOException { + String src = Files.readString(Path.of("src/main/java/frc/robot/subsystem/Swerve.java")); + // Strip comments to reduce false positives. + String stripped = src.replaceAll("//[^\\n]*", ""); + // alignToClimb should call Commands.defer (which evaluates its supplier at schedule + // time) instead of returning a pre-built AutoBuilder.pathfindToPose(...) command. + Pattern alignToClimbBody = + Pattern.compile( + "public\\s+Command\\s+alignToClimb\\s*\\(\\s*\\)\\s*\\{([^}]+)\\}", + Pattern.DOTALL); + var m = alignToClimbBody.matcher(stripped); + assertThat(m.find()).as("expected to find alignToClimb() in Swerve.java").isTrue(); + String body = m.group(1); + assertThat(body) + .as( + "alignToClimb() body should call Commands.defer(...) so the alliance" + + " lookup happens at schedule-time, not at Swerve construction" + + " time. See AlignToClimbTest.") + .contains("Commands.defer("); + } +} diff --git a/src/test/java/frc/robot/subsystem/HopperStopContractTest.java b/src/test/java/frc/robot/subsystem/HopperStopContractTest.java new file mode 100644 index 0000000..ea8735f --- /dev/null +++ b/src/test/java/frc/robot/subsystem/HopperStopContractTest.java @@ -0,0 +1,66 @@ +package frc.robot.subsystem; + +import static org.assertj.core.api.Assertions.assertThat; + +import java.io.IOException; +import java.lang.reflect.Method; +import java.nio.file.Files; +import java.nio.file.Path; +import java.util.regex.Matcher; +import java.util.regex.Pattern; +import org.junit.jupiter.api.Test; + +/** + * Layer A + Layer B — locks down the {@link Hopper#stop()} contract and its callers. + * + * + * + *

Pure source-text + reflection inspection. No HAL. Sub-millisecond. + */ +class HopperStopContractTest { + @Test + void hopper_has_a_synchronous_void_stop_method() throws NoSuchMethodException { + Method m = Hopper.class.getDeclaredMethod("stop"); + assertThat(m.getReturnType()).isEqualTo(void.class); + } + + @Test + void no_call_site_outside_default_command_binding_uses_Command_returning_Stop() + throws IOException { + Pattern badPattern = Pattern.compile("Hopper\\.get\\(\\)\\.Stop\\(\\)"); + for (Path p : sourceFilesToCheck()) { + String src = Files.readString(p); + // Strip line comments to avoid false positives in commented-out code. + String stripped = src.replaceAll("//[^\\n]*", ""); + Matcher matcher = badPattern.matcher(stripped); + assertThat(matcher.find()) + .as( + "%s must not call Hopper.get().Stop() (Command-returning; result is" + + " discarded). Use Hopper.get().stop() (void) instead.", + p) + .isFalse(); + } + } + + private static java.util.List sourceFilesToCheck() throws IOException { + // Every .java file in the main source tree except Binds.java (which legitimately uses + // Hopper.get().Stop() as a default-command binding) and Hopper.java itself (which + // defines the method). + Path root = Path.of("src/main/java"); + try (var s = Files.walk(root)) { + return s.filter(p -> p.toString().endsWith(".java")) + .filter(p -> !p.endsWith("Binds.java")) + .filter(p -> !p.endsWith("Hopper.java")) + .sorted() + .toList(); + } + } +} diff --git a/src/test/java/frc/robot/subsystem/IntakeSimTest.java b/src/test/java/frc/robot/subsystem/IntakeSimTest.java new file mode 100644 index 0000000..3af31b5 --- /dev/null +++ b/src/test/java/frc/robot/subsystem/IntakeSimTest.java @@ -0,0 +1,47 @@ +package frc.robot.subsystem; + +import static org.assertj.core.api.Assertions.assertThat; + +import edu.wpi.first.wpilibj.simulation.SimHooks; +import frc.robot.testutil.WpilibTestBase; +import java.lang.reflect.Field; +import org.junit.jupiter.api.Test; + +/** + * Layer C — smoke test that {@link Intake#simulationPeriodic()} runs end-to-end without + * throwing, and that the cached pivot position is updated to a finite value after a few + * sim ticks. + * + *

We do not assert specific physics values — Phoenix6 status signals don't propagate sim + * state cleanly in headless JUnit (see ShooterRollerVelocityTest commit notes). What this + * test does protect against: + * + *

+ */ +class IntakeSimTest extends WpilibTestBase { + + @Test + void simulationPeriodic_runs_for_several_ticks_without_throwing() throws Exception { + Intake intake = Intake.get(); + for (int i = 0; i < 20; i++) { + intake.simulationPeriodic(); + intake.periodic(); + SimHooks.stepTiming(0.020); + } + double pivotPos = privateDouble(intake, "cachedIntakePivotPosition"); + double rollerVel = privateDouble(intake, "cachedRollerVelocity"); + assertThat(pivotPos).as("cached pivot position should be a finite number").isFinite(); + assertThat(rollerVel).as("cached roller velocity should be a finite number").isFinite(); + } + + private static double privateDouble(Object obj, String name) throws Exception { + Field f = obj.getClass().getDeclaredField(name); + f.setAccessible(true); + return f.getDouble(obj); + } +} diff --git a/src/test/java/frc/robot/subsystem/ShooterRollerVelocityTest.java b/src/test/java/frc/robot/subsystem/ShooterRollerVelocityTest.java new file mode 100644 index 0000000..5b76276 --- /dev/null +++ b/src/test/java/frc/robot/subsystem/ShooterRollerVelocityTest.java @@ -0,0 +1,53 @@ +package frc.robot.subsystem; + +import static org.assertj.core.api.Assertions.assertThat; + +import org.junit.jupiter.api.Test; + +/** + * Layer A — pure-logic test for {@link Shooter#aggregateRollerVelocity(double, double)}. + * + *

The aggregation must report the leader motor's signed velocity unmodified. The follower + * runs in {@code MotorAlignmentValue.Opposed} mode, so its signed rotor velocity is the negation + * of the leader's; averaging cancels to ~0 at speed. + * + *

(Originally written as a Phoenix6 sim integration test, but Phoenix6 status-signal updates + * don't propagate reliably in the headless JUnit sim environment. The behavior is small enough + * that a pure-logic test gives equivalent confidence at sub-millisecond cost.) + */ +class ShooterRollerVelocityTest { + + @Test + void at_speed_with_opposed_follower_reports_leader_velocity_not_zero() { + // Leader spinning -25 rps; opposed follower's signed rotor velocity is +25 rps. + // Buggy averaging: (-25 + 25)/2 = 0. Correct: -25. + double agg = Shooter.aggregateRollerVelocity(-25.0, +25.0); + assertThat(agg).isEqualTo(-25.0); + } + + @Test + void at_rest_returns_zero() { + assertThat(Shooter.aggregateRollerVelocity(0.0, 0.0)).isEqualTo(0.0); + } + + @Test + void positive_leader_with_opposed_follower_reports_positive_leader() { + // If the shooter were ever commanded forward, leader = +25, follower = -25. + assertThat(Shooter.aggregateRollerVelocity(+25.0, -25.0)).isEqualTo(+25.0); + } + + @Test + void ignores_follower_value_entirely() { + // Even if the follower reports a nonsense / desynced value, we trust the leader. + assertThat(Shooter.aggregateRollerVelocity(-25.0, 0.0)).isEqualTo(-25.0); + assertThat(Shooter.aggregateRollerVelocity(-25.0, +999.0)).isEqualTo(-25.0); + assertThat(Shooter.aggregateRollerVelocity(-25.0, -999.0)).isEqualTo(-25.0); + } + + @Test + void leader_velocity_is_signed() { + // Negative target velocities are the normal case; verify sign is preserved. + assertThat(Shooter.aggregateRollerVelocity(-50.0, +50.0)).isNegative(); + assertThat(Shooter.aggregateRollerVelocity(+50.0, -50.0)).isPositive(); + } +} diff --git a/src/test/java/frc/robot/testutil/WpilibTestBase.java b/src/test/java/frc/robot/testutil/WpilibTestBase.java new file mode 100644 index 0000000..9de1bfb --- /dev/null +++ b/src/test/java/frc/robot/testutil/WpilibTestBase.java @@ -0,0 +1,23 @@ +package frc.robot.testutil; + +import edu.wpi.first.hal.HAL; +import org.junit.jupiter.api.AfterAll; +import org.junit.jupiter.api.BeforeAll; + +/** + * Base class for tests that need the WPILib HAL initialized (DriverStation, Joystick, Pigeon2, + * any subsystem construction). Layer-A pure-logic tests must not extend this — HAL init + * adds ~1 second of JVM startup we don't want in the inner loop. + */ +public abstract class WpilibTestBase { + @BeforeAll + static void initHal() { + // 500ms timeout, mode 0 (none). Idempotent within a JVM. + HAL.initialize(500, 0); + } + + @AfterAll + static void shutdownHal() { + HAL.shutdown(); + } +} diff --git a/src/test/java/frc/robot/util/AllianceFindTest.java b/src/test/java/frc/robot/util/AllianceFindTest.java new file mode 100644 index 0000000..917779c --- /dev/null +++ b/src/test/java/frc/robot/util/AllianceFindTest.java @@ -0,0 +1,74 @@ +package frc.robot.util; + +import static org.assertj.core.api.Assertions.assertThat; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; +import frc.robot.testutil.WpilibTestBase; +import frc.robot.util.Field.Alliance_Find; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +/** + * Layer C — drives {@link Alliance_Find#setAlliance()} via {@link DriverStationSim}, asserting + * the four hub/pass/climb references switch in lock-step with the simulated alliance. + */ +class AllianceFindTest extends WpilibTestBase { + + @BeforeEach + void resetAllianceState() { + // Reset the static field state between tests so prev_alliance != alliance triggers the + // update branch on each setAlliance() call. + Alliance_Find.alliance = null; + Alliance_Find.prev_alliance = null; + DriverStationSim.setDsAttached(true); + } + + @AfterEach + void clearDsAlliance() { + DriverStationSim.setAllianceStationId( + edu.wpi.first.hal.AllianceStationID.Unknown); + DriverStationSim.notifyNewData(); + } + + @Test + void unknown_alliance_defaults_to_blue_references() { + DriverStationSim.setAllianceStationId(edu.wpi.first.hal.AllianceStationID.Unknown); + DriverStationSim.notifyNewData(); + Alliance_Find.setAlliance(); + assertThat(Alliance_Find.alliance).isEqualTo(DriverStation.Alliance.Blue); + // Initial state of class fields is already blue, so they should remain blue references. + assertThat(Alliance_Find.hub).isEqualTo(Field.hub_position_blue); + assertThat(Alliance_Find.climb_tower).isEqualTo(Field.climb_tower_blue); + } + + @Test + void red_alliance_switches_all_four_references_to_red() { + DriverStationSim.setAllianceStationId(edu.wpi.first.hal.AllianceStationID.Red1); + DriverStationSim.notifyNewData(); + Alliance_Find.setAlliance(); + assertThat(Alliance_Find.alliance).isEqualTo(DriverStation.Alliance.Red); + assertThat(Alliance_Find.hub).isEqualTo(Field.hub_position_red); + assertThat(Alliance_Find.Pass_1).isEqualTo(Field.pass_position_red_1); + assertThat(Alliance_Find.Pass_2).isEqualTo(Field.pass_position_red_2); + assertThat(Alliance_Find.climb_tower).isEqualTo(Field.climb_tower_red); + } + + @Test + void switching_back_from_red_to_blue_resets_all_four_references() { + DriverStationSim.setAllianceStationId(edu.wpi.first.hal.AllianceStationID.Red1); + DriverStationSim.notifyNewData(); + Alliance_Find.setAlliance(); + assertThat(Alliance_Find.alliance).isEqualTo(DriverStation.Alliance.Red); + + DriverStationSim.setAllianceStationId(edu.wpi.first.hal.AllianceStationID.Blue1); + DriverStationSim.notifyNewData(); + Alliance_Find.setAlliance(); + assertThat(Alliance_Find.alliance).isEqualTo(DriverStation.Alliance.Blue); + assertThat(Alliance_Find.hub).isEqualTo(Field.hub_position_blue); + assertThat(Alliance_Find.Pass_1).isEqualTo(Field.pass_position_blue_1); + assertThat(Alliance_Find.Pass_2).isEqualTo(Field.pass_position_blue_2); + assertThat(Alliance_Find.climb_tower).isEqualTo(Field.climb_tower_blue); + } +} diff --git a/src/test/java/frc/robot/util/AutoChooserConsistencyTest.java b/src/test/java/frc/robot/util/AutoChooserConsistencyTest.java new file mode 100644 index 0000000..adeb6b5 --- /dev/null +++ b/src/test/java/frc/robot/util/AutoChooserConsistencyTest.java @@ -0,0 +1,83 @@ +package frc.robot.util; + +import static org.assertj.core.api.Assertions.assertThat; + +import java.io.IOException; +import java.nio.file.Files; +import java.nio.file.Path; +import java.util.HashSet; +import java.util.Set; +import java.util.regex.Matcher; +import java.util.regex.Pattern; +import org.junit.jupiter.api.Test; + +/** + * Layer B — static config validation. + * + *

Parses the source of {@link frc.robot.util.Auto} and asserts two things: + * + *

    + *
  1. Every {@code new PathPlannerAuto("X", ...)} literal corresponds to an actual {@code X.auto} + * file in the {@code src/main/deploy/pathplanner/autos/} directory. + *
  2. Every {@code autoChooser.addOption("Label", new PathPlannerAuto("Name", ...))} has matching + * label and name — i.e. the chooser entry is honest about which auto it actually loads. This + * catches the historical bug where three entries labeled "...Bravo..." silently loaded + * "...Alpha..." auto files. + *
+ * + *

Source parsing (instead of reflection-on-chooser) keeps this Layer B — no HAL, no + * PathPlanner GUI config required. + */ +class AutoChooserConsistencyTest { + private static final Path AUTO_JAVA = Path.of("src/main/java/frc/robot/util/Auto.java"); + private static final Path AUTOS_DIR = Path.of("src/main/deploy/pathplanner/autos"); + private static final Pattern PATH_PLANNER_AUTO_CTOR = + Pattern.compile("new\\s+PathPlannerAuto\\s*\\(\\s*\"([^\"]+)\""); + // Matches: addOption("LABEL", new PathPlannerAuto("NAME", ...)) + private static final Pattern ADD_OPTION_WITH_AUTO = + Pattern.compile( + "addOption\\s*\\(\\s*\"([^\"]+)\"\\s*,\\s*" + + "new\\s+PathPlannerAuto\\s*\\(\\s*\"([^\"]+)\""); + + @Test + void every_explicit_PathPlannerAuto_name_has_a_matching_auto_file() throws IOException { + String source = Files.readString(AUTO_JAVA); + Set referencedAutoNames = new HashSet<>(); + Matcher m = PATH_PLANNER_AUTO_CTOR.matcher(source); + while (m.find()) { + referencedAutoNames.add(m.group(1)); + } + + Set autosOnDisk = new HashSet<>(); + try (var stream = Files.list(AUTOS_DIR)) { + stream.filter(p -> p.toString().endsWith(".auto")) + .forEach(p -> { + String name = p.getFileName().toString(); + autosOnDisk.add(name.substring(0, name.length() - ".auto".length())); + }); + } + + assertThat(autosOnDisk).as("should find some .auto files on disk").isNotEmpty(); + assertThat(referencedAutoNames) + .as("every new PathPlannerAuto(\"X\", ...) must have X.auto on disk") + .isSubsetOf(autosOnDisk); + } + + @Test + void addOption_label_matches_loaded_auto_name() throws IOException { + String source = Files.readString(AUTO_JAVA); + Matcher m = ADD_OPTION_WITH_AUTO.matcher(source); + int found = 0; + while (m.find()) { + found++; + String label = m.group(1); + String autoName = m.group(2); + assertThat(label) + .as("addOption label must match the underlying PathPlannerAuto name") + .isEqualTo(autoName); + } + // We don't require addOption calls to exist — buildAutoChooser auto-loads everything — + // but if any exist, they must be honest. The "found" counter is informational only. + assertThat(found).as("scanned addOption(...) calls (informational)").isGreaterThanOrEqualTo(0); + } +} diff --git a/src/test/java/frc/robot/util/BaseCamTest.java b/src/test/java/frc/robot/util/BaseCamTest.java new file mode 100644 index 0000000..356e19a --- /dev/null +++ b/src/test/java/frc/robot/util/BaseCamTest.java @@ -0,0 +1,123 @@ +package frc.robot.util; + +import static org.assertj.core.api.Assertions.assertThat; + +import edu.wpi.first.math.geometry.Pose2d; +import frc.robot.util.BaseCam.AprilTagResult; +import java.util.Optional; +import java.util.concurrent.atomic.AtomicBoolean; +import java.util.concurrent.atomic.AtomicInteger; +import org.junit.jupiter.api.Test; + +/** + * Layer A — locks down {@link BaseCam#addVisionEstimate} call-back semantics: + * + *

+ * + *

Uses a small in-test {@code FakeCam} fixture; no HAL, no mocks. + */ +class BaseCamTest { + + @Test + void no_estimate_means_no_callbacks_and_false_return() { + FakeCam cam = new FakeCam(Optional.empty()); + AtomicInteger added = new AtomicInteger(); + boolean ret = cam.addVisionEstimate((p, t, v) -> added.incrementAndGet(), r -> true); + assertThat(ret).isFalse(); + assertThat(added.get()).isEqualTo(0); + } + + @Test + void rejected_estimate_does_not_call_addVisionMeasurement() { + FakeCam cam = new FakeCam(Optional.of(makeResult(cam()))); + AtomicInteger added = new AtomicInteger(); + boolean ret = cam.addVisionEstimate((p, t, v) -> added.incrementAndGet(), r -> false); + assertThat(ret).isFalse(); + assertThat(added.get()).isEqualTo(0); + } + + @Test + void accepted_estimate_calls_addVisionMeasurement_exactly_once_with_pose_and_stddev() { + AtomicBoolean called = new AtomicBoolean(); + Pose2d expectedPose = new Pose2d(1, 2, new edu.wpi.first.math.geometry.Rotation2d(0)); + double expectedTime = 12.5; + FakeCam cam = new FakeCam(Optional.of(new FakeCam().new ResultWrapper( + expectedPose, expectedTime, 1.0, 1, 0.1).inner)); + cam.setStdDeviations(0.5, 0.5, 5.0); + + boolean ret = cam.addVisionEstimate( + (p, t, v) -> { + called.set(true); + assertThat(p).isEqualTo(expectedPose); + assertThat(t).isEqualTo(expectedTime); + assertThat(v.get(0, 0)).isEqualTo(0.5); + assertThat(v.get(1, 0)).isEqualTo(0.5); + assertThat(v.get(2, 0)).isEqualTo(5.0); + }, + r -> true); + + assertThat(ret).isTrue(); + assertThat(called.get()).isTrue(); + } + + @Test + void filter_callback_throwing_does_not_propagate_outside() { + FakeCam cam = new FakeCam(Optional.of(makeResult(cam()))); + AtomicInteger added = new AtomicInteger(); + // Filter throws — addVisionEstimate must catch and still call the measurement callback + // (this matches the current BaseCam contract: PLog.fatalException then fall through to + // accept and feed the measurement). + boolean ret = cam.addVisionEstimate( + (p, t, v) -> added.incrementAndGet(), + r -> { + throw new RuntimeException("filter blew up"); + }); + assertThat(ret).isTrue(); + assertThat(added.get()).isEqualTo(1); + } + + private static FakeCam cam() { + return new FakeCam(); + } + + private static AprilTagResult makeResult(FakeCam outer) { + return outer.new ResultWrapper( + new Pose2d(0, 0, new edu.wpi.first.math.geometry.Rotation2d(0)), + 0.0, + 2.0, + 1, + 0.1) + .inner; + } + + /** Concrete subclass that lets us hand it a precomputed Optional. */ + private static final class FakeCam extends BaseCam { + private Optional next = Optional.empty(); + + FakeCam() {} + + FakeCam(Optional next) { + this.next = next; + } + + @Override + public Optional getEstimate() { + return next; + } + + /** Helper: AprilTagResult is an inner class so we can only instantiate it via an outer. */ + final class ResultWrapper { + final AprilTagResult inner; + + ResultWrapper(Pose2d pose, double time, double distToTag, int tagCount, double ambiguity) { + inner = new AprilTagResult(pose, time, distToTag, tagCount, ambiguity); + } + } + } +} diff --git a/src/test/java/frc/robot/util/BotConstantsTest.java b/src/test/java/frc/robot/util/BotConstantsTest.java new file mode 100644 index 0000000..ef0266e --- /dev/null +++ b/src/test/java/frc/robot/util/BotConstantsTest.java @@ -0,0 +1,105 @@ +package frc.robot.util; + +import static org.assertj.core.api.Assertions.assertThat; + +import frc.robot.util.BotConstants.Hopper; +import frc.robot.util.BotConstants.Intake; +import frc.robot.util.BotConstants.Shooter; +import java.lang.reflect.Field; +import java.lang.reflect.Modifier; +import java.util.HashMap; +import java.util.Map; +import org.junit.jupiter.api.Test; + +/** + * Layer B — static config validation for the central {@link BotConstants} bag. Catches CAN-ID + * collisions, interpolation tables out of order or out of plausible bounds, and unsafe current + * limits. + */ +class BotConstantsTest { + + @Test + void every_can_id_is_unique_across_all_subsystems() throws IllegalAccessException { + Map seen = new HashMap<>(); + for (Class sub : new Class[] {Intake.class, Hopper.class, Shooter.class}) { + for (Field f : sub.getDeclaredFields()) { + if (!Modifier.isStatic(f.getModifiers())) continue; + if (f.getType() != int.class) continue; + String fqName = sub.getSimpleName() + "." + f.getName(); + if (!looksLikeCanId(f.getName())) continue; + int id = f.getInt(null); + String prev = seen.put(id, fqName); + assertThat(prev) + .as("CAN ID %d is declared by both %s and %s", id, prev, fqName) + .isNull(); + } + } + } + + @Test + void every_can_id_is_in_valid_range() throws IllegalAccessException { + for (Class sub : new Class[] {Intake.class, Hopper.class, Shooter.class}) { + for (Field f : sub.getDeclaredFields()) { + if (!Modifier.isStatic(f.getModifiers())) continue; + if (f.getType() != int.class) continue; + if (!looksLikeCanId(f.getName())) continue; + int id = f.getInt(null); + assertThat(id) + .as("%s.%s = %d must be a valid CAN ID in [0, 62]", + sub.getSimpleName(), f.getName(), id) + .isBetween(0, 62); + } + } + } + + @Test + void shooter_velocity_table_is_in_a_plausible_range() { + // All keyed distances should be between 0.5 and 10 meters. + // All velocities should be between -100 and 0 (the shooter spins backwards). + double[] testDistances = {0.5, 1.0, 1.607, 2.0, 2.206, 2.5, 2.744, 3.060, 5.0, 10.0}; + for (double d : testDistances) { + double v = Shooter.ShooterTable.get(d); + assertThat(v) + .as("Shooter velocity at distance %.3f is implausible", d) + .isBetween(-100.0, 0.0); + } + } + + @Test + void backSpin_table_is_in_a_plausible_range() { + double[] testDistances = {1.607, 2.0, 2.206, 2.744, 3.060}; + for (double d : testDistances) { + double v = Shooter.backSpinTable.get(d); + assertThat(v) + .as("Backspin velocity at distance %.3f is implausible", d) + .isBetween(-100.0, 0.0); + } + } + + @Test + void shooter_and_backspin_tables_have_matching_distance_keys() { + // This isn't strictly required, but they should generally be sampled at the same + // distances so the indexer/flywheel speeds are coordinated. + for (double d : new double[] {1.607, 2.206, 2.257, 2.744, 3.060}) { + double shooterV = Shooter.ShooterTable.get(d); + double backV = Shooter.backSpinTable.get(d); + assertThat(shooterV).as("shooter table missing key %.3f", d).isNotEqualTo(0.0); + assertThat(backV).as("backspin table missing key %.3f", d).isNotEqualTo(0.0); + } + } + + @Test + void stator_current_limits_are_safe() { + // Stator current limits should never exceed 200A (mechanical/electrical safety), + // and should never be 0 (which would disable the limit and effectively be unbounded). + assertThat(Intake.cfg_Roller.CurrentLimits.StatorCurrentLimit).isBetween(1.0, 200.0); + assertThat(Intake.cfg_Pivot.CurrentLimits.StatorCurrentLimit).isBetween(1.0, 200.0); + assertThat(Hopper.cfg_Hopper.CurrentLimits.StatorCurrentLimit).isBetween(1.0, 200.0); + assertThat(Shooter.cfg_shooter.CurrentLimits.StatorCurrentLimit).isBetween(1.0, 200.0); + } + + private static boolean looksLikeCanId(String fieldName) { + String n = fieldName.toLowerCase(); + return n.endsWith("id") || n.endsWith("_id") || n.endsWith("id_2"); + } +} diff --git a/src/test/java/frc/robot/util/FieldBoundsTest.java b/src/test/java/frc/robot/util/FieldBoundsTest.java new file mode 100644 index 0000000..8a61c8c --- /dev/null +++ b/src/test/java/frc/robot/util/FieldBoundsTest.java @@ -0,0 +1,58 @@ +package frc.robot.util; + +import static edu.wpi.first.units.Units.Meters; +import static org.assertj.core.api.Assertions.assertThat; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import java.lang.reflect.Field; +import java.lang.reflect.Modifier; +import java.util.ArrayList; +import java.util.List; +import org.junit.jupiter.api.DynamicTest; +import org.junit.jupiter.api.TestFactory; + +/** + * Layer B — static config validation. No HAL needed. + * + *

Walks every public static Translation2d / Pose2d declared on {@link frc.robot.util.Field} + * and asserts the coordinates are on the field. Catches typos like {@code 5201} (should have been + * {@code 5.201}). + */ +class FieldBoundsTest { + private static final double FIELD_LENGTH_M = + frc.robot.util.Field.LENGTH.in(Meters); + private static final double FIELD_WIDTH_M = + frc.robot.util.Field.WIDTH.in(Meters); + + @TestFactory + List every_published_pose_is_inside_the_field() throws IllegalAccessException { + List tests = new ArrayList<>(); + for (Field f : frc.robot.util.Field.class.getDeclaredFields()) { + int mods = f.getModifiers(); + if (!Modifier.isStatic(mods) || !Modifier.isPublic(mods)) continue; + Object value = f.get(null); + if (value instanceof Translation2d t) { + tests.add(DynamicTest.dynamicTest( + f.getName() + " inside field", () -> assertOnField(f.getName(), t))); + } else if (value instanceof Pose2d p) { + tests.add(DynamicTest.dynamicTest( + f.getName() + " inside field", + () -> assertOnField(f.getName(), p.getTranslation()))); + } + } + // Make sure we actually found some — guards against the test silently passing if Field + // were refactored to private fields. + assertThat(tests).as("no public Translation2d/Pose2d fields found on Field").isNotEmpty(); + return tests; + } + + private static void assertOnField(String name, Translation2d t) { + assertThat(t.getX()) + .as(name + ".x must be within field length [0, %.3f]", FIELD_LENGTH_M) + .isBetween(0.0, FIELD_LENGTH_M); + assertThat(t.getY()) + .as(name + ".y must be within field width [0, %.3f]", FIELD_WIDTH_M) + .isBetween(0.0, FIELD_WIDTH_M); + } +} diff --git a/src/test/java/frc/robot/util/PLogTest.java b/src/test/java/frc/robot/util/PLogTest.java new file mode 100644 index 0000000..b2557fc --- /dev/null +++ b/src/test/java/frc/robot/util/PLogTest.java @@ -0,0 +1,74 @@ +package frc.robot.util; + +import static org.assertj.core.api.Assertions.assertThat; + +import java.io.ByteArrayOutputStream; +import java.io.PrintStream; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +/** + * Layer A — tests {@link PLog}'s severity/category/message format by capturing System.out. + * + *

Catches regressions if someone changes the bracket format or drops one of the three fields. + */ +class PLogTest { + private PrintStream originalOut; + private ByteArrayOutputStream captured; + + @BeforeEach + void captureStdout() { + originalOut = System.out; + captured = new ByteArrayOutputStream(); + System.setOut(new PrintStream(captured)); + } + + @AfterEach + void restoreStdout() { + System.setOut(originalOut); + } + + @Test + void debug_writes_severity_category_message_in_canonical_format() { + PLog.debug("Cat", "msg"); + assertThat(captured.toString()).contains("[Debug] [Cat] msg"); + } + + @Test + void info_writes_severity_category_message() { + PLog.info("Cat", "hello"); + assertThat(captured.toString()).contains("[Info] [Cat] hello"); + } + + @Test + void unusual_writes_severity_category_message() { + PLog.unusual("Cat", "huh"); + assertThat(captured.toString()).contains("[Unusual] [Cat] huh"); + } + + @Test + void fatalException_includes_exception_class_simple_name_and_stack_trace() { + Exception e = new IllegalStateException("boom"); + PLog.fatalException("Cat", "user said something", e); + String out = captured.toString(); + assertThat(out).contains("[Fatal]"); + assertThat(out).contains("[Cat]"); + assertThat(out).contains("user said something"); + assertThat(out).contains("IllegalStateException"); + assertThat(out).contains("boom"); + // At least one stacktrace element with 4-space indent. + assertThat(out).contains(" "); + } + + @Test + void log_format_is_stable_across_severities() { + PLog.debug("X", "a"); + PLog.info("Y", "b"); + PLog.unusual("Z", "c"); + String out = captured.toString(); + assertThat(out) + .containsSubsequence( + "[Debug] [X] a", "[Info] [Y] b", "[Unusual] [Z] c"); + } +} diff --git a/src/test/java/frc/robot/util/ShooterControlPolicyTest.java b/src/test/java/frc/robot/util/ShooterControlPolicyTest.java new file mode 100644 index 0000000..9e04fe9 --- /dev/null +++ b/src/test/java/frc/robot/util/ShooterControlPolicyTest.java @@ -0,0 +1,124 @@ +package frc.robot.util; + +import static org.assertj.core.api.Assertions.assertThat; + +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import org.junit.jupiter.api.Test; + +/** + * Layer A — pure-logic tests of the shooter "should we fire the indexer?" decision, extracted + * from {@link frc.robot.commands.ShootCommand} so it can be exhaustively tested with no HAL and + * no mocks. + * + *

The contract: + *

+ */ +class ShooterControlPolicyTest { + // Tables roughly matching BotConstants.Shooter.* but small and self-contained. + private static final InterpolatingDoubleTreeMap VEL = treeMap(1.5, -25.0, 3.0, -30.0); + private static final InterpolatingDoubleTreeMap IDX = treeMap(1.5, -20.0, 3.0, -28.0); + private static final double ENTER = 0.95; + private static final double EXIT = 1.5; + + @Test + void flywheel_target_is_interpolated_from_distance() { + var d = ShooterControlPolicy.compute(2.25, 0.0, VEL, IDX, ENTER, EXIT, false); + assertThat(d.flywheelTarget()).isCloseTo(-27.5, within(1e-9)); + assertThat(d.indexerSpeed()).isCloseTo(-24.0, within(1e-9)); + } + + @Test + void does_not_fire_when_far_from_target_and_not_currently_firing() { + var d = ShooterControlPolicy.compute(1.5, 0.0, VEL, IDX, ENTER, EXIT, false); + // target is -25, measured is 0, |err| = 25 -> not within enter band + assertThat(d.fireIndexer()).isFalse(); + } + + @Test + void fires_once_within_enter_band() { + // target is -25, measured -24.5 -> |err| = 0.5 < 0.95 + var d = ShooterControlPolicy.compute(1.5, -24.5, VEL, IDX, ENTER, EXIT, false); + assertThat(d.fireIndexer()).isTrue(); + } + + @Test + void keeps_firing_while_inside_exit_band() { + // target -25, measured -23.7 -> |err| = 1.3, between enter (0.95) and exit (1.5) + var d = ShooterControlPolicy.compute(1.5, -23.7, VEL, IDX, ENTER, EXIT, /*wasFiring*/ true); + assertThat(d.fireIndexer()).isTrue(); + } + + @Test + void stops_firing_when_drifting_past_exit_band() { + // target -25, measured -22 -> |err| = 3 > 1.5 + var d = ShooterControlPolicy.compute(1.5, -22.0, VEL, IDX, ENTER, EXIT, /*wasFiring*/ true); + assertThat(d.fireIndexer()).isFalse(); + } + + @Test + void exit_band_is_wider_than_enter_band_so_short_dips_do_not_drop_indexer() { + // Enter -> firing + var d1 = ShooterControlPolicy.compute(1.5, -24.5, VEL, IDX, ENTER, EXIT, false); + assertThat(d1.fireIndexer()).isTrue(); + + // Small dip: |err|=1.2, outside enter (0.95) but inside exit (1.5) -> keep firing + var d2 = ShooterControlPolicy.compute(1.5, -23.8, VEL, IDX, ENTER, EXIT, true); + assertThat(d2.fireIndexer()).isTrue(); + } + + @Test + void enter_just_inside_band_fires() { + // target -25, measured -24.1 -> err = 0.9 (clearly inside enter band of 0.95) + var d = ShooterControlPolicy.compute(1.5, -24.1, VEL, IDX, ENTER, EXIT, false); + assertThat(d.fireIndexer()).isTrue(); + } + + @Test + void enter_just_outside_band_does_not_fire() { + // target -25, measured -24 -> err = 1.0 (just outside enter band) + var d = ShooterControlPolicy.compute(1.5, -24.0, VEL, IDX, ENTER, EXIT, false); + assertThat(d.fireIndexer()).isFalse(); + } + + @Test + void exit_just_inside_band_keeps_firing() { + // target -25, measured -23.6 -> err = 1.4 (clearly inside exit band of 1.5) + var d = ShooterControlPolicy.compute(1.5, -23.6, VEL, IDX, ENTER, EXIT, true); + assertThat(d.fireIndexer()).isTrue(); + } + + @Test + void exit_just_outside_band_stops_firing() { + // target -25, measured -23.4 -> err = 1.6 (just outside exit band) + var d = ShooterControlPolicy.compute(1.5, -23.4, VEL, IDX, ENTER, EXIT, true); + assertThat(d.fireIndexer()).isFalse(); + } + + @Test + void distance_below_table_min_clamps_to_lowest_velocity() { + var d = ShooterControlPolicy.compute(0.5, 0.0, VEL, IDX, ENTER, EXIT, false); + assertThat(d.flywheelTarget()).isEqualTo(-25.0); + } + + @Test + void distance_above_table_max_clamps_to_highest_velocity() { + var d = ShooterControlPolicy.compute(99.0, 0.0, VEL, IDX, ENTER, EXIT, false); + assertThat(d.flywheelTarget()).isEqualTo(-30.0); + } + + private static InterpolatingDoubleTreeMap treeMap(double k1, double v1, double k2, double v2) { + var m = new InterpolatingDoubleTreeMap(); + m.put(k1, v1); + m.put(k2, v2); + return m; + } + + private static org.assertj.core.data.Offset within(double e) { + return org.assertj.core.data.Offset.offset(e); + } +} diff --git a/src/test/java/frc/robot/util/VisionFilterTest.java b/src/test/java/frc/robot/util/VisionFilterTest.java new file mode 100644 index 0000000..d4735bc --- /dev/null +++ b/src/test/java/frc/robot/util/VisionFilterTest.java @@ -0,0 +1,110 @@ +package frc.robot.util; + +import static edu.wpi.first.units.Units.*; +import static org.assertj.core.api.Assertions.assertThat; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import org.junit.jupiter.api.Test; + +/** + * Layer A — pure-logic tests of the AprilTag vision filter that lives inside + * {@code Swerve.acceptEstimate}. Asserts every threshold individually and locks down the standard- + * deviation calculation. + * + *

No HAL, no mocks. Sub-millisecond per test. + */ +class VisionFilterTest { + + @Test + void rejects_when_dist_to_tag_above_3_5_m() { + var d = VisionFilter.evaluate(/*dist*/ 3.6, /*amb*/ 0.1, /*omega*/ 0.0); + assertThat(d.accept()).isFalse(); + } + + @Test + void rejects_when_ambiguity_above_0_7() { + var d = VisionFilter.evaluate(2.0, 0.71, 0.0); + assertThat(d.accept()).isFalse(); + } + + @Test + void rejects_when_rotating_too_fast() { + var d = VisionFilter.evaluate(2.0, 0.1, 1.6); + assertThat(d.accept()).isFalse(); + // Symmetric: a fast negative rotation is also rejected. + var d2 = VisionFilter.evaluate(2.0, 0.1, -1.6); + assertThat(d2.accept()).isFalse(); + } + + @Test + void accepts_close_target_within_thresholds() { + var d = VisionFilter.evaluate(2.0, 0.2, 0.5); + assertThat(d.accept()).isTrue(); + } + + @Test + void uses_fixed_stddev_when_dist_under_half_meter() { + var d = VisionFilter.evaluate(0.4, 0.3, 0.0); + assertThat(d.accept()).isTrue(); + assertVec(d.stdDevs(), 0.3, 0.3, 50.0); + } + + @Test + void scales_stddev_quadratically_with_distance_outside_half_meter() { + double dist = 2.0; + double amb = 0.5; + double expected = amb * dist * dist * 3.0; // == 0.5 * 4 * 3 = 6.0 + var d = VisionFilter.evaluate(dist, amb, 0.0); + assertThat(d.accept()).isTrue(); + assertVec(d.stdDevs(), expected, expected, expected); + } + + @Test + void boundary_dist_3_5_exact_is_accepted() { + // The original code uses strict inequality (> 3.5 rejects), so exactly 3.5 is in. + var d = VisionFilter.evaluate(3.5, 0.1, 0.0); + assertThat(d.accept()).isTrue(); + } + + @Test + void boundary_ambiguity_0_7_exact_is_accepted() { + var d = VisionFilter.evaluate(2.0, 0.7, 0.0); + assertThat(d.accept()).isTrue(); + } + + @Test + void boundary_omega_1_5_exact_is_accepted() { + var d = VisionFilter.evaluate(2.0, 0.1, 1.5); + assertThat(d.accept()).isTrue(); + } + + @Test + void boundary_dist_0_5_exact_uses_quadratic_branch() { + // Code path is `dist < 0.5 -> fixed`, so 0.5 falls through to quadratic. + double dist = 0.5; + double amb = 0.4; + double expected = amb * dist * dist * 3.0; + var d = VisionFilter.evaluate(dist, amb, 0.0); + assertThat(d.accept()).isTrue(); + assertVec(d.stdDevs(), expected, expected, expected); + } + + private static void assertVec(Matrix v, double x, double y, double r) { + assertThat(v).isNotNull(); + assertThat(v.get(0, 0)).isCloseTo(x, within(1e-9)); + assertThat(v.get(1, 0)).isCloseTo(y, within(1e-9)); + assertThat(v.get(2, 0)).isCloseTo(r, within(1e-9)); + } + + private static org.assertj.core.data.Offset within(double e) { + return org.assertj.core.data.Offset.offset(e); + } + + @SuppressWarnings("unused") + private static Matrix vec(double x, double y, double r) { + return VecBuilder.fill(x, y, r); + } +}