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.geometry.Translation3d; 025import edu.wpi.first.math.util.Units; 026import edu.wpi.first.units.measure.Angle; 027import edu.wpi.first.units.measure.AngularVelocity; 028import edu.wpi.first.units.measure.Distance; 029import edu.wpi.first.units.measure.Mass; 030import edu.wpi.first.units.measure.Time; 031 032/** 033 * <h2>SimulationConstants</h2> 034 * <p>A collection of constants used within our MapleSim code and other related sim classes. 035 * <p>This includes things like component offsets, physical constants, and any other values 036 * that are relevant to the simulation</p> 037 */ 038public interface SimulationConstants { 039 040 /** 041 * 042 * 043 * <h2>Record that holds CAD Offsets</h2> 044 * 045 * <p> 046 * Sourced from CAD exports to hold a component's positional offsets from their 047 * position in CAD 048 * to their position in sim 049 * 050 * <pre>{@code 051 * Offsets SHOOTER = new Offsets(-0.1016, 0.2032, 0.3255, 052 * Degrees.of(90), Degrees.of(0), Degrees.of(-90)); 053 * 054 * Pose3d shooterPose = SHOOTER.withRotation( 055 * new Rotation3d(0, 0, turretSim.getAngle().getRadians())); 056 * }</pre> 057 * 058 * @param x X translation from the robot origin (meters) 059 * @param y Y translation from the robot origin (meters) 060 * @param z Z translation from the robot origin (meters) 061 * @param roll Rotation about the X axis 062 * @param pitch Rotation about the Y axis 063 * @param yaw Rotation about the Z axis 064 */ 065 public static record Offsets( 066 Distance x, Distance y, Distance z, Angle roll, Angle pitch, Angle yaw) { 067 068 /** 069 * 070 * 071 * <h4>Constructs an Offsets instance with no rotation</h4> 072 * 073 * @param x X translation from the robot origin (meters) 074 * @param y Y translation from the robot origin (meters) 075 * @param z Z translation from the robot origin (meters) 076 */ 077 public Offsets(double x, double y, double z) { 078 this(Meters.of(x), Meters.of(y), Meters.of(z), Radians.of(0), Radians.of(0), Radians.of(0)); 079 } 080 081 /** 082 * 083 * 084 * <h4>Constructs an Offsets instance with no rotation, using your unit of 085 * choice</h4> 086 * 087 * @param x X translation from the robot origin 088 * @param y Y translation from the robot origin 089 * @param z Z translation from the robot origin 090 */ 091 public Offsets(Distance x, Distance y, Distance z) { 092 this(x.in(Meters), y.in(Meters), z.in(Meters)); 093 } 094 095 /** 096 * 097 * 098 * <h4>Constructs an Offsets instance with no translation</h4> 099 * 100 * @param roll Rotation about the X axis 101 * @param pitch Rotation about the Y axis 102 * @param yaw Rotation about the Z axis 103 */ 104 public Offsets(Angle roll, Angle pitch, Angle yaw) { 105 this(Meters.of(0), Meters.of(0), Meters.of(0), roll, pitch, yaw); 106 } 107 108 /** 109 * 110 * 111 * <h4>Constructs an Offsets instance with no rotation</h4> 112 * 113 * @param x X translation from the robot origin (meters) 114 * @param y Y translation from the robot origin (meters) 115 * @param z Z translation from the robot origin (meters) 116 * @param roll Rotation about the X axis 117 * @param pitch Rotation about the Y axis 118 * @param yaw Rotation about the Z axis 119 */ 120 public Offsets(double x, double y, double z, Angle roll, Angle pitch, Angle yaw) { 121 this(Meters.of(x), Meters.of(y), Meters.of(z), roll, pitch, yaw); 122 } 123 124 /** 125 * 126 * 127 * <h4>Translational component as a {@link Translation3d}</h4> 128 * 129 * @return translation from the robot origin 130 */ 131 public Translation3d toTranslation3d() { 132 return new Translation3d(x.in(Meters), y.in(Meters), z.in(Meters)); 133 } 134 135 /** 136 * 137 * 138 * <h4>Rotational component as a {@link Rotation3d}</h4> 139 * 140 * @return Rotation3d of the roll, pitch, and yaw components of the offset 141 */ 142 public Rotation3d toRotation3d() { 143 return new Rotation3d(roll, pitch, yaw); 144 } 145 146 /** 147 * 148 * 149 * <h4>This offset as a {@link Pose3d}.</h4> 150 * 151 * <p> 152 * Useful for components whose pose is fully static and requires no adjustments 153 * 154 * @return pose at this offset's position and orientation 155 */ 156 public Pose3d toPose3d() { 157 return new Pose3d(toTranslation3d(), toRotation3d()); 158 } 159 160 /** 161 * 162 * 163 * <h4>Applies this offset's translation and rotation onto an existing 164 * {@link Pose3d}</h4> 165 * 166 * @param pose the base pose to offset 167 * @return a new pose with this offset applied to both translation and rotation 168 */ 169 public Pose3d applyToPose3d(Pose3d pose) { 170 return new Pose3d( 171 pose.getMeasureX().plus(x), 172 pose.getMeasureY().plus(y), 173 pose.getMeasureZ().plus(z), 174 applyToRotation3d(pose.getRotation())); 175 } 176 177 /** 178 * 179 * 180 * <h4>Applies this offset's translation and rotation onto an existing 181 * {@link Pose3d}</h4> 182 * 183 * <p> 184 * Translation is applied, but it is <b>robot robot relative</b> 185 * 186 * @param pose the base pose to offset 187 * @return a new pose with this offset applied to both translation and rotation 188 */ 189 public Pose3d applyToPose3dRobotRelative(Pose3d pose) { 190 Translation3d rotatedOffset = toTranslation3d().rotateBy(pose.getRotation()); 191 return new Pose3d( 192 pose.getTranslation().plus(rotatedOffset), applyToRotation3d(pose.getRotation())); 193 } 194 195 /** 196 * 197 * 198 * <h4>Composes this offset's rotation onto an existing {@link Rotation3d}</h4> 199 * 200 * @param rotation the base rotation to offset 201 * @return a new rotation with this offset's rotation applied on top 202 */ 203 public Rotation3d applyToRotation3d(Rotation3d rotation) { 204 return new Rotation3d( 205 rotation.getX() + roll.in(Radians), 206 rotation.getY() + pitch.in(Radians), 207 rotation.getZ() + yaw.in(Radians)); 208 } 209 210 /** 211 * 212 * 213 * <h4>Pose at this offset's translation with an additional rotation</h4> 214 * 215 * <p> 216 * Applies a rotation to this offsets rotation 217 * 218 * <pre>{@code 219 * SHOOTER_OFFSETS.withRotation( 220 * new Rotation3d(0, 0, turretSim.getAngle().getRadians())); 221 * }</pre> 222 * 223 * @param rotation the additional rotation to add to this offset's rotation 224 * @return a new pose at this offset's translation with the combined rotation 225 */ 226 public Pose3d withRotation(Rotation3d rotation) { 227 return new Pose3d(toTranslation3d(), toRotation3d().plus(rotation)); 228 } 229 230 /** 231 * 232 * 233 * <h4>Applies this offset's X/Y translation and yaw onto an existing 234 * {@link Pose2d}</h4> 235 * 236 * <p> 237 * Roll and pitch are ignored because they have no 2D equivalent 238 * 239 * @param pose the base 2D pose to offset 240 * @return a new pose of this offset's X, Y, and yaw 241 */ 242 public Pose2d applyToPose2d(Pose2d pose) { 243 return new Pose2d( 244 pose.getMeasureX().plus(x), 245 pose.getMeasureY().plus(y), 246 applyToRotation2d(pose.getRotation())); 247 } 248 249 /** 250 * 251 * 252 * <h4>Applies this offset's X/Y translation and yaw onto an existing 253 * {@link Pose2d}</h4> 254 * 255 * <p> 256 * Translation is applied, but it is <b>robot robot relative</b> 257 * 258 * @param pose the base 2D pose to offset 259 * @return a new pose with this offset applied 260 */ 261 public Pose2d applyToPose2dRobotRelative(Pose2d pose) { 262 Translation2d rotatedOffset = new Translation2d(x, y).rotateBy(pose.getRotation()); 263 return new Pose2d( 264 pose.getTranslation().plus(rotatedOffset), applyToRotation2d(pose.getRotation())); 265 } 266 267 /** 268 * 269 * 270 * <h4>Applies this offset's yaw onto an existing {@link Rotation2d}</h4> 271 * 272 * <p> 273 * Roll and pitch are ignored because they have no 2D equivalent 274 * 275 * @param rotation the base 2D rotation to offset 276 * @return a new rotation with this offset's yaw applied on top 277 */ 278 public Rotation2d applyToRotation2d(Rotation2d rotation) { 279 return new Rotation2d(rotation.getRadians() + yaw.in(Radians)); 280 } 281 } 282 283 /** 284 * Contains simulation constants related to the intake subsystem 285 */ 286 public interface Intake { 287 288 double INTAKE_WIDTH = 0.5; 289 290 double INTAKE_LENGTH = 0.15; 291 292 double PIVOT_END_X = 0; 293 294 public Offsets PIVOT_OFFSETS = new Offsets( 295 0.2393388152, 296 0.0, // CAD zero angle offset degrees 297 0.19685, 298 Degrees.of(-40), 299 Degrees.of(0), 300 Degrees.of(90)); 301 302 public Offsets ROLLER_OFFSETS = new Offsets(0.022, 0, 0.2152848, Degrees.of(90), Degrees.of(0), Degrees.of(90)); 303 304 public Offsets OUTTAKE_OFFSETS = new Offsets(0.4, 0, 0); 305 } 306 307 /** 308 * Contains simulation constants related to our physical hopper. 309 * <p>The hopper is not necessarily its own subsystem, but it has some important properties. 310 */ 311 public interface Hopper { 312 313 int FUEL_CAPACITY = 54; 314 315 public Offsets OFFSETS = new Offsets(-0.06, 0, 0.25, Degrees.of(90), Degrees.of(0), Degrees.of(90)); 316 317 int FUEL_LAYERS = 4; 318 319 Pose3d VISIBLE_POSE = new Pose3d(0, 0, 0, new Rotation3d(1.55, 0, 1.5)); 320 321 Pose3d HIDDEN_POSE = new Pose3d(1000, 1000, 1000, new Rotation3d()); 322 } 323 324 /** 325 * Contains simulation constants related to the shooter subsystem 326 */ 327 public interface Shooter { 328 329 double BPS = 8; 330 331 double COMPRESSION_METRES = Units.inchesToMeters(1.379342); 332 333 public static double angularVelocityToMps(AngularVelocity angularVelocity) { 334 return ((Settings.Shooter.WHEEL_RADIUS.in(Meters) * 2 - COMPRESSION_METRES) 335 * (angularVelocity.in(RPM)) 336 * Math.PI) 337 / 60.0; 338 } 339 340 public Offsets OFFSETS = new Offsets(Units.inchesToMeters(-7.836), 0, 0.7); 341 } 342 343 /** 344 * Contains simulation constants related to our drivetrain and its components 345 */ 346 public interface Drivetrain { 347 348 // alignment 349 PIDConstants XY = new PIDConstants(2.2, 0, 0.0); 350 351 PIDConstants THETA = new PIDConstants(3, 0, 0.0); 352 353 Distance LENGTH = Inches.of(27); 354 355 Distance WIDTH = Inches.of(25.5); 356 357 // TODO: Get actual 358 double WHEEL_COF = 1.2; 359 360 Time SIMULATION_STEP_TIME = Seconds.of(0.005); 361 362 Mass ROBOT_WEIGHT = Pounds.of(65); 363 364 Mass RED_BUMPER_WEIGHT = Pounds.of(16.8); 365 366 Mass BLUE_BUMPER_WEIGHT = Pounds.of(15.4); 367 368 Supplier<Mass> TOTAL_WEIGHT = () -> { 369 if (Robot.isBlue()) { 370 return ROBOT_WEIGHT.plus(BLUE_BUMPER_WEIGHT); 371 } else { 372 return ROBOT_WEIGHT.plus(RED_BUMPER_WEIGHT); 373 } 374 }; 375 376 @SuppressWarnings("unchecked") 377 public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>[] MODULE_CONSTANTS = new SwerveModuleConstants[] { 378 TunerConstants.FrontLeft, 379 TunerConstants.FrontRight, 380 TunerConstants.BackLeft, 381 TunerConstants.BackRight 382 }; 383 384 public static final Translation2d[] MODULE_TRANSLATIONS = new Translation2d[] { 385 new Translation2d(MODULE_CONSTANTS[0].LocationX, MODULE_CONSTANTS[0].LocationY), 386 new Translation2d(MODULE_CONSTANTS[1].LocationX, MODULE_CONSTANTS[1].LocationY), 387 new Translation2d(MODULE_CONSTANTS[2].LocationX, MODULE_CONSTANTS[2].LocationY), 388 new Translation2d(MODULE_CONSTANTS[3].LocationX, MODULE_CONSTANTS[3].LocationY) 389 }; 390 } 391 392 /** 393 * Starting positions for the robots in the simulation. 394 */ 395 public static final Pose2d[] ROBOTS_STARTING_POSITIONS = new Pose2d[] { 396 new Pose2d(12.5, 0.5, Rotation2d.fromDegrees(90)), // depot side trench facing hub 397 new Pose2d(12.5, 7.777, Rotation2d.fromDegrees(270)), 398 new Pose2d(15, 2, Rotation2d.fromDegrees(180)), 399 new Pose2d(1.6, 6, new Rotation2d()), 400 new Pose2d(1.6, 4, new Rotation2d()) 401 }; 402 403 /** 404 * Boolean for whether {@link org.ironmaple.simulation.seasonspecific.rebuilt2026.Arena2026Rebuilt} efficiency mode is enabled. When true, less game pieces will be spawned. 405 * If false, the normal amount will spawn. 406 */ 407 public static final Boolean SPAWN_GAMEPIECES_SPARSELY = true; // whether to spawn a decreased set of gamepieces to conserve processing power 408}