Source code

001/************************* PROJECT RON *************************/
002/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */
003/* Use of this source code is governed by an MIT-style license */
004/* that can be found in the repository LICENSE file.           */
005/***************************************************************/
006package com.stuypulse.robot.util.simulation;
007
008import static edu.wpi.first.units.Units.*;
009
010import com.stuypulse.robot.constants.Settings;
011import com.stuypulse.robot.subsystems.handoff.Handoff;
012import com.stuypulse.robot.subsystems.handoff.Handoff.HandoffState;
013import com.stuypulse.robot.subsystems.intake.Intake;
014import com.stuypulse.robot.subsystems.intake.Intake.IntakeState;
015import com.stuypulse.robot.subsystems.shooter.Shooter;
016import com.stuypulse.robot.subsystems.shooter.Shooter.ShooterState;
017import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain;
018import dev.doglog.DogLog;
019import edu.wpi.first.math.geometry.Pose2d;
020import edu.wpi.first.math.geometry.Pose3d;
021import edu.wpi.first.math.geometry.Rotation2d;
022import edu.wpi.first.math.geometry.Rotation3d;
023import edu.wpi.first.math.geometry.Translation2d;
024import edu.wpi.first.networktables.NetworkTableInstance;
025import edu.wpi.first.networktables.StructArrayPublisher;
026import edu.wpi.first.networktables.StructPublisher;
027import edu.wpi.first.units.measure.Angle;
028import edu.wpi.first.units.measure.Distance;
029import edu.wpi.first.units.measure.LinearVelocity;
030import edu.wpi.first.wpilibj.Notifier;
031import org.ironmaple.simulation.IntakeSimulation;
032import org.ironmaple.simulation.SimulatedArena;
033import org.ironmaple.simulation.drivesims.SwerveDriveSimulation;
034import org.ironmaple.simulation.seasonspecific.rebuilt2026.Arena2026Rebuilt;
035import org.ironmaple.simulation.seasonspecific.rebuilt2026.RebuiltFuelOnFly;
036
037public class Simulation {
038
039    private static final Simulation instance;
040
041    public final Arena2026Rebuilt arenaInstance;
042
043    private final Notifier shotLoop;
044
045    private final SwerveDriveSimulation swerveMSim;
046
047    private final IntakeSimulation intakeMSim;
048
049    private final Intake intakeSim;
050
051    private final Shooter shooterSim;
052
053    private final Handoff handoffSim;
054
055    private final StructArrayPublisher<Pose3d> fuel;
056
057    private final StructPublisher<Pose3d> intakePivot;
058
059    private final StructPublisher<Pose3d> hopper;
060
061    private final StructArrayPublisher<Pose3d> fuelLayers;
062
063    private final StructPublisher<Pose3d> shooter;
064
065    static {
066        if (CommandSwerveDrivetrain.getInstance().getMapleSimDrive() != null)
067            instance = new Simulation();
068        else
069            // extra safeguarding to ensure NO overlap between sim code and actual code
070            instance = null;
071    }
072
073    public static Simulation getInstance() {
074        return instance;
075    }
076
077    private Simulation() {
078        intakeSim = Intake.getInstance();
079        shooterSim = Shooter.getInstance();
080        handoffSim = Handoff.getInstance();
081        swerveMSim = CommandSwerveDrivetrain.getInstance().getMapleSimDrive();
082        arenaInstance = new Arena2026Rebuilt(false);
083        configureArena(arenaInstance, swerveMSim);
084
085        intakeMSim = createIntakeSimulation();
086        intakeMSim.addGamePiecesToIntake(SimulationConstants.Hopper.FUEL_CAPACITY);
087
088        shotLoop = new Notifier(this::updateSubsystemsBPSLoop);
089        shotLoop.startPeriodic(1.0 / SimulationConstants.Shooter.BPS);
090
091        NetworkTableInstance table = NetworkTableInstance.getDefault();
092        fuel = table.getStructArrayTopic("AdvScope/FuelPoses", Pose3d.struct).publish();
093        intakePivot = table.getStructTopic("AdvScope/IntakePose", Pose3d.struct).publish();
094        hopper = table.getStructTopic("AdvScope/HopperPose", Pose3d.struct).publish();
095        fuelLayers = table.getStructArrayTopic("AdvScope/FuelLayers", Pose3d.struct).publish();
096        shooter = table.getStructTopic("AdvScope/ShooterPose", Pose3d.struct).publish();
097    }
098
099    private void configureArena(Arena2026Rebuilt arena, SwerveDriveSimulation drivetrain) {
100        arena.setEfficiencyMode(SimulationConstants.SPAWN_GAMEPIECES_SPARSELY);
101        arena.resetFieldForAuto();
102        arena.addDriveTrainSimulation(drivetrain);
103        SimulatedArena.overrideInstance(arena);
104    }
105
106    private IntakeSimulation createIntakeSimulation() {
107        return IntakeSimulation.OverTheBumperIntake(
108                "Fuel",
109                swerveMSim,
110                Meters.of(SimulationConstants.Intake.INTAKE_WIDTH),
111                Meters.of(SimulationConstants.Intake.INTAKE_LENGTH),
112                IntakeSimulation.IntakeSide.FRONT,
113                SimulationConstants.Hopper.FUEL_CAPACITY);
114    }
115
116    private Pose3d getIntakePivotPose() {
117        return SimulationConstants.Intake.PIVOT_OFFSETS.withRotation(
118                new Rotation3d(
119                        0, // inverts the angle
120                        intakeSim.getRelativePosition().in(Radians),
121                        0));
122    }
123
124    private double getIntakeArmEndX() {
125        return SimulationConstants.Intake.PIVOT_END_X
126                + // sin works because we're zeroed at horizontal
127                Settings.Intake.Pivot.PIVOT_ARM_LENGTH.in(Meters)
128                        * Math.sin(
129                                intakeSim.getRelativePosition().in(Radians)
130                                        + SimulationConstants.Intake.PIVOT_OFFSETS.toRotation3d().getX());
131    }
132
133    private void updateIntakeEnabled(boolean enabled) {
134        if (enabled) {
135            intakeMSim.startIntake();
136        } else {
137            intakeMSim.stopIntake();
138        }
139    }
140
141    private void updateIntake() {
142        boolean intakeEnabled = intakeSim.atTargetAngle()
143                && (intakeSim.getState() == IntakeState.DOWN)
144                && Settings.EnabledSubsystems.INTAKE.get();
145        updateIntakeEnabled(intakeEnabled);
146    }
147
148    private void updateHopperFuel() {
149        final double hopperPercentage = (double) intakeMSim.getGamePiecesAmount()
150                / (double) SimulationConstants.Hopper.FUEL_CAPACITY;
151        final int layers = SimulationConstants.Hopper.FUEL_LAYERS;
152        final Pose3d visiblePose = SimulationConstants.Hopper.VISIBLE_POSE;
153        final Pose3d hiddenPose = SimulationConstants.Hopper.HIDDEN_POSE;
154        final Pose3d[] poses = new Pose3d[layers];
155        for (int i = 0; i < layers; i++) {
156            poses[i] = hopperPercentage >= (1 / (double) layers) * (i + 1) ? visiblePose : hiddenPose;
157        }
158        fuelLayers.set(poses);
159        DogLog.log("Intake/hopperpercentage", hopperPercentage);
160    }
161
162    /**
163     * <h4>Extension of {@link Arena2026Rebuilt#addPieceWithVariance} that uses
164     * chassis speeds</h4>
165     *
166     * <p>
167     * Adds a gamepiece too the arena with a certain random variance.
168     *
169     * @param piecePose the field relative position at which to spawn the gamepiece
170     * @param yaw the initial yaw of the gamepiece
171     * @param height the initial height of the gamepiece above the field
172     * @param speed the initial speed of the gamepiece
173     * @param pitch the initial pitch of the gamepiece
174     * @param xVariance the maximum random offset applied to the x coordinate
175     * @param yVariance the maximum random offset applied to the y coordinate
176     * @param yawVariance the maximum random offset applied to the yaw, in degrees
177     * @param speedVariance the maximum random offset applied to the speed, in m/s
178     * @param pitchVariance the maximum random offset applied to the pitch, in degrees
179     */
180    private void robotRelativeAddPieceWithVariance(
181            Translation2d piecePose,
182            Rotation2d yaw,
183            Distance height,
184            LinearVelocity speed,
185            Angle pitch,
186            double xVariance,
187            double yVariance,
188            double yawVariance,
189            double speedVariance,
190            double pitchVariance) {
191        arenaInstance.addGamePieceProjectile(
192                new RebuiltFuelOnFly(
193                        piecePose.plus(
194                                new Translation2d(
195                                        Arena2026Rebuilt.randomInRange(xVariance),
196                                        Arena2026Rebuilt.randomInRange(yVariance))),
197                        new Translation2d(),
198                        swerveMSim.getDriveTrainSimulatedChassisSpeedsFieldRelative(),
199                        yaw.plus(Rotation2d.fromDegrees(Arena2026Rebuilt.randomInRange(yawVariance))),
200                        height,
201                        speed.plus(MetersPerSecond.of(Arena2026Rebuilt.randomInRange(speedVariance))),
202                        Degrees.of(pitch.in(Degrees) + Arena2026Rebuilt.randomInRange(pitchVariance))));
203    }
204
205    private void summonFuelAtIntake() {
206        robotRelativeAddPieceWithVariance(
207                swerveMSim
208                        .getSimulatedDriveTrainPose()
209                        .getTranslation()
210                        .plus(
211                                SimulationConstants.Intake.OUTTAKE_OFFSETS
212                                        .applyToPose3dRobotRelative(
213                                                new Pose3d(
214                                                        getIntakeArmEndX(),
215                                                        0,
216                                                        0,
217                                                        new Rotation3d(
218                                                                swerveMSim.getSimulatedDriveTrainPose().getRotation())))
219                                        .getTranslation()
220                                        .toTranslation2d()),
221                swerveMSim.getSimulatedDriveTrainPose().getRotation(),
222                Meters.of(0),
223                MetersPerSecond.of(2),
224                Radians.of(0), // x
225                SimulationConstants.Intake.INTAKE_WIDTH,
226                0.0,
227                0.0, // speed
228                1.0,
229                0.0);
230    }
231
232    private boolean canShoot() {
233        // final ShooterState shooterState = shooterSim.getState();
234        // final boolean shooterEnabled = (shooterState == ShooterState.SHOOT || shooterState == ShooterState.MANUAL_HUB || shooterState == ShooterState.FERRY) && Settings.EnabledSubsystems.SHOOTER.get();
235        return handoffSim.getState() == HandoffState.FORWARD && intakeMSim.obtainGamePieceFromIntake();
236    }
237
238    /**
239     * <h4>Custom interval periodic function</h4>
240     * <p>Runs at the speed of 1 over the balls per second constant {@link SimulationConstants.Shooter#BPS}</p>
241     */
242    private void updateSubsystemsBPSLoop() {
243        if (intakeSim.getState() == IntakeState.OUTTAKE
244                && Settings.EnabledSubsystems.INTAKE.get()
245                && intakeMSim.obtainGamePieceFromIntake()) {
246            summonFuelAtIntake();
247        }
248        if (this.canShoot()) {
249            final Pose2d shooterPose = SimulationConstants.Shooter.OFFSETS.applyToPose2d(
250                    swerveMSim.getSimulatedDriveTrainPose());
251            final double launchAngle = 67.67; // random hood exit angle?
252            this.robotRelativeAddPieceWithVariance(
253                    shooterPose.getTranslation(),
254                    swerveMSim.getSimulatedDriveTrainPose().getRotation(),
255                    Meters.of(SimulationConstants.Shooter.OFFSETS.toPose3d().getZ()),
256                    MetersPerSecond.of(
257                            SimulationConstants.Shooter.angularVelocityToMps(
258                                    shooterSim.getCurrentAngularVelocity())),
259                    Degrees.of(launchAngle),
260                    SimulationConstants.Intake.INTAKE_WIDTH,
261                    0,
262                    0,
263                    0.5,
264                    0);
265        }
266    }
267
268    public synchronized void update() {
269        if (swerveMSim == null)
270            return;
271        fuel.set(arenaInstance.getGamePiecesArrayByType("Fuel"));
272        updateIntake();
273        updateHopperFuel();
274        double armEndX = getIntakeArmEndX();
275        intakePivot.set(getIntakePivotPose());
276        hopper.set(
277                SimulationConstants.Hopper.OFFSETS.applyToPose3d(
278                        new Pose3d(armEndX, 0, 0, new Rotation3d())));
279        // Translation2d outtakeTranslationRobotRelative =
280        // swerveMSim.getSimulatedDriveTrainPose().getTranslation().plus(
281        // SimulationConstants.Intake.OUTTAKE_OFFSETS.applyToPose3dRobotRelative(
282        // new Pose3d(getIntakeArmEndX(), 0, 0, new
283        // Rotation3d(swerveMSim.getSimulatedDriveTrainPose().getRotation()))).getTranslation().toTranslation2d());
284        // shooter.set(new Pose3d(tra.getX(), tra.getY(), 0, new Rotation3d()));
285        // shooter.set(SimulationConstants.Shooter.OFFSETS.applyToPose3dRobotRelative(new
286        // Pose3d(swerveMSim.getSimulatedDriveTrainPose())));
287    }
288}