diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bb6e5e5..a4542a8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -27,6 +27,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Robot.RobotRunType; import frc.robot.commands.WaitSupplierCommand; +import frc.robot.localization.DrivetrainState; import frc.robot.sim.FuelSim; import frc.robot.sim.SimulatedRobotState; import frc.robot.subsystems.LEDs; @@ -84,8 +85,9 @@ public final class RobotContainer { private final AutoCommandFactory autoCommandFactory; /* Subsystems */ - private final LEDs leds = new LEDs(); + private final DrivetrainState drivetrainState; private final Swerve swerve; + private final LEDs leds = new LEDs(); private final Vision vision; private final AdjustableHood adjustableHood; private final Turret turret; @@ -112,7 +114,11 @@ public RobotContainer(RobotRunType runtimeType) { switch (runtimeType) { case kReal: sim = null; - swerve = new Swerve(SwerveReal::new, GyroNavX2::new, SwerveModuleReal::new); + Swerve.Bundle realBundle = + Swerve.create(SwerveReal::new, GyroNavX2::new, SwerveModuleReal::new); + this.drivetrainState = realBundle.drivetrainState(); + this.swerve = realBundle.swerve(); + vision = new Vision(swerve.state, new VisionReal()); adjustableHood = new AdjustableHood(new AdjustableHoodReal()); turret = new Turret(new TurretReal(), swerve.state); @@ -120,7 +126,7 @@ public RobotContainer(RobotRunType runtimeType) { intake = new Intake(new IntakeReal()); climber = new Climber(new ClimberIOEmpty()); indexer = new Indexer(new IndexerReal()); - + break; case kSimulation: SimulatedArena.overrideInstance(new Arena2026Rebuilt(false)); @@ -138,8 +144,11 @@ public RobotContainer(RobotRunType runtimeType) { sim.indexer.addFuel(); }); FuelSim.getInstance().start(); - swerve = new Swerve(sim.swerveDrive::simProvider, sim.swerveDrive::gyroProvider, - sim.swerveDrive::moduleProvider); + Swerve.Bundle simBundle = Swerve.create(sim.swerveDrive::simProvider, + sim.swerveDrive::gyroProvider, sim.swerveDrive::moduleProvider); + this.drivetrainState = simBundle.drivetrainState(); + this.swerve = simBundle.swerve(); + vision = new Vision(swerve.state, sim.visionSim); adjustableHood = new AdjustableHood(sim.adjustableHood); turret = new Turret(sim.turret, swerve.state); @@ -150,13 +159,17 @@ public RobotContainer(RobotRunType runtimeType) { SmartDashboard.putNumber("VisionFudge", 0.0); - + FuelSim.getInstance().spawnStartingFuel(); break; default: sim = null; - swerve = new Swerve(SwerveIOEmpty::new, GyroIOEmpty::new, SwerveModuleIOEmpty::new); + Swerve.Bundle defaultBundle = + Swerve.create(SwerveIOEmpty::new, GyroIOEmpty::new, SwerveModuleIOEmpty::new); + this.drivetrainState = defaultBundle.drivetrainState(); + this.swerve = defaultBundle.swerve(); + vision = new Vision(swerve.state, new VisionIOEmpty()); adjustableHood = new AdjustableHood(new AdjustableHoodIOEmpty()); turret = new Turret(new TurretIOEmpty(), swerve.state); @@ -164,14 +177,14 @@ public RobotContainer(RobotRunType runtimeType) { intake = new Intake(new IntakeIOEmpty()); climber = new Climber(new ClimberSim()); indexer = new Indexer(new IndexerIOEmpty()); - + break; - + } targetingState = new TargetingState(() -> swerve.state.getGlobalPoseEstimate(), - () -> swerve.state.getFieldRelativeSpeeds(), shooter.getFlyWheelVeloRPS()); + () -> swerve.state.getFieldRelativeSpeeds(), shooter.getFlyWheelVeloRPS()); // DASHBOARD STUFF SmartDashboard.putData(Constants.DashboardValues.autoChooser, autoChooser); SmartDashboard.putNumber(Constants.DashboardValues.shootX, diff --git a/src/main/java/frc/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java index 97aa3c3..bda6f42 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/robot/subsystems/swerve/Swerve.java @@ -73,13 +73,13 @@ @NullMarked public final class Swerve extends SubsystemBase { - private final Lock odometryLock = new ReentrantLock(); + private final Lock odometryLock; private final PhoenixOdometryThread odometryThread; public final SwerveModule[] modules; private final GyroIO gyro; - private final GyroInputsAutoLogged gyroInputs = new GyroInputsAutoLogged(); + private final GyroInputsAutoLogged gyroInputs; private final SwerveIO io; - private final SwerveInputsAutoLogged inputs = new SwerveInputsAutoLogged(); + private final SwerveInputsAutoLogged inputs; private final SwerveRateLimiter limiter = new SwerveRateLimiter(); @@ -106,34 +106,69 @@ public final class Swerve extends SubsystemBase { * @param gyroIo factory for creating the gyro IO implementation * @param moduleIoFn factory for creating per-module IO implementations */ - public Swerve(Function swerveIo, + public static record Bundle(Swerve swerve, DrivetrainState drivetrainState) { + } + + public static Bundle create(Function swerveIo, Function gyroIo, BiFunction moduleIoFn) { - super("Swerve"); - this.odometryThread = new PhoenixOdometryThread(this.odometryLock); - this.gyro = gyroIo.apply(this.odometryThread); - this.modules = IntStream.range(0, Constants.Swerve.modulesConstants.length) - .mapToObj(i -> new SwerveModule(i, moduleIoFn.apply(i, this.odometryThread))) + Lock localLock = new ReentrantLock(); + PhoenixOdometryThread localOdometryThread = new PhoenixOdometryThread(localLock); + + GyroIO localGyro = gyroIo.apply(localOdometryThread); + GyroInputsAutoLogged localGyroInputs = new GyroInputsAutoLogged(); + SwerveIO localIo = swerveIo.apply(localOdometryThread); + + SwerveInputsAutoLogged localInputs = new SwerveInputsAutoLogged(); + + SwerveModule[] localModules = IntStream.range(0, Constants.Swerve.modulesConstants.length) + .mapToObj(i -> new SwerveModule(i, moduleIoFn.apply(i, localOdometryThread))) .toArray(SwerveModule[]::new); - this.io = swerveIo.apply(odometryThread); - this.odometryThread.start(); - this.odometryLock.lock(); - SwerveModulePosition[] initPositions = new SwerveModulePosition[modules.length]; + + localOdometryThread.start(); + + localLock.lock(); + SwerveModulePosition[] initPositions = new SwerveModulePosition[localModules.length]; try { - Arrays.stream(modules).map(mod -> { + Arrays.stream(localModules).map(mod -> { mod.updateInputs(); return mod.getPosition(); }).toArray(_i -> initPositions); - this.gyro.updateInputs(this.gyroInputs); - Logger.processInputs("Swerve/Gyro", this.gyroInputs); + + localGyro.updateInputs(localGyroInputs); + Logger.processInputs("Swerve/Gyro", localGyroInputs); } finally { - this.odometryLock.unlock(); + localLock.unlock(); } - this.state = new DrivetrainState(initPositions, this.gyroInputs.yaw); - autoFactory = new AutoFactory(state::getGlobalPoseEstimate, state::resetPose, + + DrivetrainState instantiatedState = new DrivetrainState(initPositions, localGyroInputs.yaw); + + Swerve instantiatedSwerve = new Swerve(localLock, localOdometryThread, localModules, + localGyro, localGyroInputs, localIo, localInputs, instantiatedState); + + return new Bundle(instantiatedSwerve, instantiatedState); + } + + + private Swerve(Lock odometryLock, PhoenixOdometryThread odometryThread, SwerveModule[] modules, + GyroIO gyro, GyroInputsAutoLogged gyroInputs, SwerveIO io, SwerveInputsAutoLogged inputs, + DrivetrainState state) { + super("Swerve"); + + this.odometryLock = odometryLock; + this.odometryThread = odometryThread; + this.modules = modules; + this.gyro = gyro; + this.gyroInputs = gyroInputs; + this.io = io; + this.inputs = inputs; // B. Assign it here to fix the error! + this.state = state; + + this.autoFactory = new AutoFactory(state::getGlobalPoseEstimate, state::resetPose, this::followTrajectory, true, this); } + /** * Set this to true to flip trajectories about the y axis (left/right) for auto paths. */