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 java.util.function.Supplier;
011
012import com.ctre.phoenix6.configs.CANcoderConfiguration;
013import com.ctre.phoenix6.configs.TalonFXConfiguration;
014import com.ctre.phoenix6.swerve.SwerveModuleConstants;
015import com.pathplanner.lib.config.PIDConstants;
016import com.stuypulse.robot.Robot;
017import com.stuypulse.robot.constants.Settings;
018import com.stuypulse.robot.subsystems.swerve.TunerConstants;
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.math.util.Units;
025import edu.wpi.first.units.measure.AngularVelocity;
026import edu.wpi.first.units.measure.Distance;
027import edu.wpi.first.units.measure.Mass;
028import edu.wpi.first.units.measure.Time;
029
030/**
031 * <h2>SimulationConstants</h2>
032 * <p>A collection of constants used within our MapleSim code and other related sim classes.
033 * <p>This includes things like component offsets, physical constants, and any other values 
034 * that are relevant to the simulation</p>
035 */
036public interface SimulationConstants {
037    /**
038     * Contains simulation constants related to the intake subsystem
039     */
040    public interface Intake {
041
042        double INTAKE_WIDTH = 0.5;
043
044        double INTAKE_LENGTH = 0.15;
045
046        double PIVOT_END_X = 0;
047
048        public Offsets PIVOT_OFFSETS = new Offsets(
049                0.2393388152,
050                0.0, // CAD zero angle offset degrees
051                0.19685,
052                Degrees.of(-40),
053                Degrees.of(0),
054                Degrees.of(90));
055
056        public Offsets ROLLER_OFFSETS = new Offsets(0.022, 0, 0.2152848, Degrees.of(90), Degrees.of(0), Degrees.of(90));
057
058        public Offsets OUTTAKE_OFFSETS = new Offsets(0.4, 0, 0);
059    }
060
061    /**
062     * Contains simulation constants related to our physical hopper.
063     * <p>The hopper is not necessarily its own subsystem, but it has some important properties.
064     */
065    public interface Hopper {
066
067        int FUEL_CAPACITY = 54;
068
069        public Offsets OFFSETS = new Offsets(-0.06, 0, 0.25, Degrees.of(90), Degrees.of(0), Degrees.of(90));
070
071        int FUEL_LAYERS = 4;
072
073        Pose3d VISIBLE_POSE = new Pose3d(0, 0, 0, new Rotation3d(1.55, 0, 1.5));
074
075        Pose3d HIDDEN_POSE = new Pose3d(1000, 1000, 1000, new Rotation3d());
076    }
077
078    /**
079     * Contains simulation constants related to the shooter subsystem
080     */
081    public interface Shooter {
082
083        double BPS = 8;
084
085        double COMPRESSION_METRES = Units.inchesToMeters(1.379342);
086
087        public static double angularVelocityToMps(AngularVelocity angularVelocity) {
088            return ((Settings.Shooter.WHEEL_RADIUS.in(Meters) * 2 - COMPRESSION_METRES)
089                    * (angularVelocity.in(RPM))
090                    * Math.PI)
091                    / 60.0;
092        }
093
094        public Offsets OFFSETS = new Offsets(Units.inchesToMeters(-7.836), 0, 0.7);
095    }
096
097    /**
098     * Contains simulation constants related to our drivetrain and its components
099     */
100    public interface Drivetrain {
101
102        // alignment
103        PIDConstants XY = new PIDConstants(2.2, 0, 0.0);
104
105        PIDConstants THETA = new PIDConstants(3, 0, 0.0);
106
107        Distance LENGTH = Inches.of(27);
108
109        Distance WIDTH = Inches.of(25.5);
110
111        // TODO: Get actual
112        double WHEEL_COF = 1.2;
113
114        Time SIMULATION_STEP_TIME = Seconds.of(0.005);
115
116        Mass ROBOT_WEIGHT = Pounds.of(65);
117
118        Mass RED_BUMPER_WEIGHT = Pounds.of(16.8);
119
120        Mass BLUE_BUMPER_WEIGHT = Pounds.of(15.4);
121
122        Supplier<Mass> TOTAL_WEIGHT = () -> {
123            if (Robot.isBlue()) {
124                return ROBOT_WEIGHT.plus(BLUE_BUMPER_WEIGHT);
125            } else {
126                return ROBOT_WEIGHT.plus(RED_BUMPER_WEIGHT);
127            }
128        };
129
130        @SuppressWarnings("unchecked")
131        public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>[] MODULE_CONSTANTS = new SwerveModuleConstants[] {
132                TunerConstants.FrontLeft,
133                TunerConstants.FrontRight,
134                TunerConstants.BackLeft,
135                TunerConstants.BackRight
136        };
137
138        public static final Translation2d[] MODULE_TRANSLATIONS = new Translation2d[] {
139                new Translation2d(MODULE_CONSTANTS[0].LocationX, MODULE_CONSTANTS[0].LocationY),
140                new Translation2d(MODULE_CONSTANTS[1].LocationX, MODULE_CONSTANTS[1].LocationY),
141                new Translation2d(MODULE_CONSTANTS[2].LocationX, MODULE_CONSTANTS[2].LocationY),
142                new Translation2d(MODULE_CONSTANTS[3].LocationX, MODULE_CONSTANTS[3].LocationY)
143        };
144    }
145
146    /**
147     * Starting positions for the robots in the simulation.
148     */
149        public static final Pose2d[] ROBOTS_STARTING_POSITIONS = new Pose2d[] {
150                        new Pose2d(12.5, 0.5, Rotation2d.fromDegrees(90)), // depot side trench facing hub
151                        new Pose2d(12.5, 7.777, Rotation2d.fromDegrees(270)),
152                        new Pose2d(15, 2, Rotation2d.fromDegrees(180)),
153                        new Pose2d(1.6, 6, new Rotation2d()),
154                        new Pose2d(1.6, 4, new Rotation2d())
155        };
156
157    /**
158     * Boolean for whether {@link org.ironmaple.simulation.seasonspecific.rebuilt2026.Arena2026Rebuilt} efficiency mode is enabled. When true, less game pieces will be spawned.
159     * If false, the normal amount will spawn.
160     */
161    public static final Boolean SPAWN_GAMEPIECES_SPARSELY = true; // whether to spawn a decreased set of gamepieces to conserve processing power
162}