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