customIntakeCondition = gp -> true;
+
+ public enum IntakeSide {
+ FRONT,
+ LEFT,
+ RIGHT,
+ BACK
+ }
+
+ /**
+ *
+ *
+ * Creates an Intake Simulation that Tightly Attaches to One Side of the Chassis.
+ *
+ * This typically represents an In-The-Frame (ITF) Intake.
+ *
+ * @param targetedGamePieceType the type of game pieces that this intake can collect
+ * @param driveTrainSimulation the chassis to which this intake is attached
+ * @param width the width of the intake
+ * @param side the side of the chassis where the intake is attached
+ * @param capacity the maximum number of game pieces that the intake can hold
+ */
+ public static IntakeSimulation InTheFrameIntake(
+ String targetedGamePieceType,
+ AbstractDriveTrainSimulation driveTrainSimulation,
+ Distance width,
+ IntakeSide side,
+ int capacity) {
+ return OverTheBumperIntake(targetedGamePieceType, driveTrainSimulation, width, Meters.of(0.02), side, capacity);
+ }
+
+ /**
+ *
+ *
+ *
Creates an Intake Simulation that Extends Out of the Chassis Frame.
+ *
+ * This typically represents an Over-The-Bumper (OTB) Intake.
+ *
+ * @param targetedGamePieceType the type of game pieces that this intake can collect
+ * @param driveTrainSimulation the chassis to which this intake is attached
+ * @param width the valid width of the intake
+ * @param lengthExtended the length the intake extends out from the chassis when activated
+ * @param side the side of the chassis where the intake is attached
+ * @param capacity the maximum number of game pieces that the intake can hold
+ * @return a new instance of {@link IntakeSimulation} that extends over the bumper
+ */
+ public static IntakeSimulation OverTheBumperIntake(
+ String targetedGamePieceType,
+ AbstractDriveTrainSimulation driveTrainSimulation,
+ Distance width,
+ Distance lengthExtended,
+ IntakeSide side,
+ int capacity) {
+ return new IntakeSimulation(
+ targetedGamePieceType,
+ driveTrainSimulation,
+ getIntakeRectangle(driveTrainSimulation, width.in(Meters), lengthExtended.in(Meters), side),
+ capacity);
+ }
+
+ private static Rectangle getIntakeRectangle(
+ AbstractDriveTrainSimulation driveTrainSimulation, double width, double lengthExtended, IntakeSide side) {
+ final Rectangle intakeRectangle = new Rectangle(width, lengthExtended);
+ intakeRectangle.rotate(
+ switch (side) {
+ case LEFT, RIGHT -> 0;
+ case FRONT, BACK -> Math.toRadians(90);
+ });
+ final double distanceTransformed = lengthExtended / 2 - 0.01;
+ intakeRectangle.translate(
+ switch (side) {
+ case LEFT -> new Vector2(
+ 0, driveTrainSimulation.config.bumperWidthY.in(Meters) / 2 + distanceTransformed);
+ case RIGHT -> new Vector2(
+ 0, -driveTrainSimulation.config.bumperWidthY.in(Meters) / 2 - distanceTransformed);
+ case FRONT -> new Vector2(
+ driveTrainSimulation.config.bumperLengthX.in(Meters) / 2 + distanceTransformed, 0);
+ case BACK -> new Vector2(
+ -driveTrainSimulation.config.bumperLengthX.in(Meters) / 2 - distanceTransformed / 2, 0);
+ });
+
+ return intakeRectangle;
+ }
+
+ /**
+ *
+ *
+ *
Creates an Intake Simulation with a Specific Shape.
+ *
+ * This constructor initializes an intake with a custom shape that is used when the intake is fully extended.
+ *
+ * @param targetedGamePieceType the type of game pieces that this intake can collect
+ * @param driveTrainSimulation the chassis to which this intake is attached
+ * @param shape the shape of the intake when fully extended, represented as a {@link Convex} object
+ * @param capacity the maximum number of game pieces that the intake can hold
+ */
+ public IntakeSimulation(
+ String targetedGamePieceType,
+ AbstractDriveTrainSimulation driveTrainSimulation,
+ Convex shape,
+ int capacity) {
+ super(shape);
+ super.setDensity(0);
+
+ this.targetedGamePieceType = targetedGamePieceType;
+ this.gamePiecesInIntakeCount = 0;
+
+ if (capacity > 100) throw new IllegalArgumentException("capacity too large, max is 100");
+ this.capacity = capacity;
+
+ this.gamePiecesToRemove = new ArrayDeque<>(capacity);
+
+ this.intakeRunning = false;
+ this.driveTrainSimulation = driveTrainSimulation;
+
+ register();
+ }
+
+ /**
+ *
+ *
+ *
Turns the Intake On.
+ *
+ * Extends the intake out from the chassis, making it part of the chassis's collision space.
+ *
+ *
Once activated, the intake is considered running and will listen for contact with
+ * {@link GamePieceOnFieldSimulation} instances, allowing it to collect game pieces.
+ */
+ public void startIntake() {
+ if (intakeRunning) return;
+
+ driveTrainSimulation.addFixture(this);
+ this.intakeRunning = true;
+ }
+
+ /**
+ *
+ *
+ *
Turns the Intake Off.
+ *
+ * Retracts the intake into the chassis, removing it from the chassis's collision space.
+ *
+ *
Once turned off, the intake will no longer listen for or respond to contacts with
+ * {@link GamePieceOnFieldSimulation} instances.
+ */
+ public void stopIntake() {
+ if (!intakeRunning) return;
+
+ driveTrainSimulation.removeFixture(this);
+ this.intakeRunning = false;
+ }
+
+ /**
+ *
+ *
+ *
Get the amount of game pieces in the intake.
+ *
+ * @return the amount of game pieces stored in the intake
+ */
+ public int getGamePiecesAmount() {
+ return gamePiecesInIntakeCount;
+ }
+
+ /**
+ *
+ *
+ * Removes 1 game piece from the intake.
+ *
+ * Deducts the {@link #getGamePiecesAmount()}} by 1, if there is any remaining.
+ *
+ *
This is used to obtain a game piece from the intake and move it a feeder/shooter.
+ *
+ * @return if there is game piece(s) remaining, and therefore retrieved
+ */
+ public boolean obtainGamePieceFromIntake() {
+ if (gamePiecesInIntakeCount < 1) return false;
+ gamePiecesInIntakeCount--;
+ return true;
+ }
+
+ /**
+ *
+ *
+ *
Adds 1 game piece from the intake.
+ *
+ * Increases the {@link #getGamePiecesAmount()}} by 1, if there is still space.
+ *
+ * @return if there is still space in the intake to perform this action
+ */
+ public boolean addGamePieceToIntake() {
+ boolean toReturn = gamePiecesInIntakeCount < capacity;
+ if (toReturn) gamePiecesInIntakeCount++;
+
+ return toReturn;
+ }
+ /**
+ *
+ *
+ *
Adds a number of game pieces to the intake.
+ *
+ * If the number of pieces added would drive the intake above capacity the intake will only add pieces up to max.
+ *
+ * @param piecesToAdd The number of pieces to add too the intake.
+ * @return Wether or not all game pieces could be added to the intake. Just because this returns false does not mean
+ * that no pieces were added.
+ */
+ public boolean addGamePiecesToIntake(int piecesToAdd) {
+ boolean toReturn = gamePiecesInIntakeCount + piecesToAdd <= capacity;
+ gamePiecesInIntakeCount = Math.min(gamePiecesInIntakeCount + piecesToAdd, capacity);
+ return toReturn;
+ }
+
+ /**
+ *
+ *
+ * Sets the amount of game pieces in the intake.
+ *
+ * Sets the {@link #getGamePiecesAmount()}} to a given amount.
+ *
+ *
Will make sure that the amount is non-negative and does not exceed the capacity
+ *
+ * @return the actual (clamped) game piece count after performing this action
+ */
+ public int setGamePiecesCount(int gamePiecesInIntakeCount) {
+ return this.gamePiecesInIntakeCount = MathUtil.clamp(gamePiecesInIntakeCount, 0, capacity);
+ }
+
+ /**
+ *
+ *
+ *
The {@link ContactListener} for the Intake Simulation.
+ *
+ * This class can be added to the simulation world to detect and manage contacts between the intake and
+ * {@link GamePieceOnFieldSimulation} instances of the type {@link #targetedGamePieceType}.
+ *
+ *
If contact is detected and the intake is running, the {@link GamePieceOnFieldSimulation} will be marked for
+ * removal from the field.
+ */
+ public final class GamePieceContactListener implements ContactListener
{
+ @Override
+ public void begin(ContactCollisionData collision, Contact contact) {
+ if (!intakeRunning) return;
+ if (gamePiecesInIntakeCount >= capacity) return;
+
+ final CollisionBody collisionBody1 = collision.getBody1(), collisionBody2 = collision.getBody2();
+ final Fixture fixture1 = collision.getFixture1(), fixture2 = collision.getFixture2();
+
+ if (collisionBody1 instanceof GamePieceOnFieldSimulation gamePiece
+ && Objects.equals(gamePiece.type, targetedGamePieceType)
+ && fixture2 == IntakeSimulation.this) flagGamePieceForRemoval(gamePiece);
+ else if (collisionBody2 instanceof GamePieceOnFieldSimulation gamePiece
+ && Objects.equals(gamePiece.type, targetedGamePieceType)
+ && fixture1 == IntakeSimulation.this) flagGamePieceForRemoval(gamePiece);
+
+ boolean coralOrAlgaeIntake = "Coral".equals(IntakeSimulation.this.targetedGamePieceType)
+ || "Algae".equals(IntakeSimulation.this.targetedGamePieceType);
+ if (collisionBody1 instanceof ReefscapeCoralAlgaeStack stack
+ && coralOrAlgaeIntake
+ && fixture2 == IntakeSimulation.this) flagGamePieceForRemoval(stack);
+ else if (collisionBody2 instanceof ReefscapeCoralAlgaeStack stack
+ && coralOrAlgaeIntake
+ && fixture1 == IntakeSimulation.this) flagGamePieceForRemoval(stack);
+ }
+
+ private void flagGamePieceForRemoval(GamePieceOnFieldSimulation gamePiece) {
+ if (!customIntakeCondition.test(gamePiece)) return;
+ gamePiecesToRemove.add(gamePiece);
+ gamePiecesInIntakeCount++;
+ }
+
+ /* functions not used */
+ @Override
+ public void persist(ContactCollisionData collision, Contact oldContact, Contact newContact) {}
+
+ @Override
+ public void end(ContactCollisionData collision, Contact contact) {}
+
+ @Override
+ public void destroyed(ContactCollisionData collision, Contact contact) {}
+
+ @Override
+ public void collision(ContactCollisionData collision) {}
+
+ @Override
+ public void preSolve(ContactCollisionData collision, Contact contact) {}
+
+ @Override
+ public void postSolve(ContactCollisionData collision, SolvedContact contact) {}
+ }
+
+ /**
+ *
+ *
+ * Obtains a New Instance of the {@link GamePieceContactListener} for This Intake.
+ *
+ * @return a new {@link GamePieceContactListener} for this intake
+ */
+ public GamePieceContactListener getGamePieceContactListener() {
+ return new GamePieceContactListener();
+ }
+
+ /**
+ *
+ *
+ * Clears the game pieces that have been obtained by the intake.
+ *
+ * This method is called from {@link SimulatedArena#simulationPeriodic()} to remove the
+ * {@link GamePieceOnFieldSimulation} instances that have been obtained by the intake from the field.
+ *
+ *
Game pieces are marked for removal if they have come into contact with the intake during the last
+ * {@link SimulatedArena#getSimulationSubTicksIn1Period()} sub-ticks. These game pieces should be removed from the
+ * field to reflect their interaction with the intake.
+ */
+ public void removeObtainedGamePieces(SimulatedArena arena) {
+ while (!gamePiecesToRemove.isEmpty()) {
+ GamePieceOnFieldSimulation gamePiece = gamePiecesToRemove.poll();
+ gamePiece.onIntake(this.targetedGamePieceType);
+ arena.removeGamePiece(gamePiece);
+ }
+ }
+
+ public void register() {
+ register(SimulatedArena.getInstance());
+ }
+
+ public void register(SimulatedArena arena) {
+ arena.addIntakeSimulation(this);
+ }
+
+ /**
+ *
+ *
+ *
Returns wether or not this intake is currently running
+ */
+ public boolean isRunning() {
+ return intakeRunning;
+ }
+
+ /**
+ *
+ *
+ * Sets a Custom Intake Condition.
+ *
+ * This method allows the user to define a custom condition for determining whether a game piece on the field can
+ * be collected by the intake. The condition is specified as a {@link Predicate} that takes a
+ * {@link GamePieceOnFieldSimulation} object as input and returns a boolean value.
+ *
+ *
The condition is used to determine whether a game piece should be collected by the intake. If the predicate
+ * returns true, the game piece will be collected by the intake. If the predicate returns false, the game piece will
+ * not be collected by the intake. Note how this predicate is only called if the game piece is in contact with the
+ * intake, and the intake is turned on, and it is the target game piece.
+ *
+ * @param customIntakeCondition a {@link Predicate} representing the custom condition for intake eligibility of game
+ * pieces on the field
+ */
+ public void setCustomIntakeCondition(Predicate customIntakeCondition) {
+ this.customIntakeCondition = customIntakeCondition;
+ }
+}
diff --git a/yagsl/java/swervelib/simulation/ironmaple/simulation/SimulatedArena.java b/yagsl/java/swervelib/simulation/ironmaple/simulation/SimulatedArena.java
new file mode 100644
index 0000000..5796f2e
--- /dev/null
+++ b/yagsl/java/swervelib/simulation/ironmaple/simulation/SimulatedArena.java
@@ -0,0 +1,786 @@
+package swervelib.simulation.ironmaple.simulation;
+
+import static edu.wpi.first.units.Units.Seconds;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.networktables.BooleanPublisher;
+import edu.wpi.first.networktables.BooleanSubscriber;
+import edu.wpi.first.networktables.DoublePublisher;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.units.measure.Time;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import java.util.ArrayList;
+import java.util.HashSet;
+import java.util.Hashtable;
+import java.util.List;
+import java.util.Map;
+import java.util.Objects;
+import java.util.Set;
+import org.dyn4j.dynamics.Body;
+import org.dyn4j.dynamics.BodyFixture;
+import org.dyn4j.geometry.Convex;
+import org.dyn4j.geometry.Geometry;
+import org.dyn4j.geometry.MassType;
+import org.dyn4j.world.PhysicsWorld;
+import org.dyn4j.world.World;
+import swervelib.simulation.ironmaple.simulation.drivesims.AbstractDriveTrainSimulation;
+import swervelib.simulation.ironmaple.simulation.gamepieces.GamePiece;
+import swervelib.simulation.ironmaple.simulation.gamepieces.GamePieceOnFieldSimulation;
+import swervelib.simulation.ironmaple.simulation.gamepieces.GamePieceProjectile;
+import swervelib.simulation.ironmaple.simulation.motorsims.SimulatedBattery;
+import swervelib.simulation.ironmaple.simulation.opponentsim.OpponentManager;
+import swervelib.simulation.ironmaple.simulation.seasonspecific.rebuilt2026.Arena2026Rebuilt;
+import swervelib.simulation.ironmaple.utils.mathutils.GeometryConvertor;
+
+/**
+ *
+ *
+ * Abstract Simulation World
+ *
+ * Check Online
+ * Documentation
+ *
+ *
The heart of the simulator.
+ *
+ * This class cannot be instantiated directly; it must be created as a specific season's arena.
+ *
+ *
The default instance can be obtained using the {@link #getInstance()} method.
+ *
+ *
Simulates all interactions within the arena field.
+ *
+ *
The following objects can be added to the simulation world and will interact with each other:
+ *
+ *
+ * - {@link AbstractDriveTrainSimulation}: Represents abstract drivetrain simulations with collision detection.
+ *
- {@link GamePieceOnFieldSimulation}: Represents abstract game pieces with collision detection.
+ *
- {@link IntakeSimulation}: Represents an intake simulation that responds to contact with
+ * {@link GamePieceOnFieldSimulation}.
+ *
+ */
+public abstract class SimulatedArena {
+ /** Whether to allow the simulation to run a real robot This feature is HIGHLY RECOMMENDED to be turned OFF */
+ public static boolean ALLOW_CREATION_ON_REAL_ROBOT = false;
+
+ protected int redScore = 0;
+ protected int blueScore = 0;
+ protected double matchClock = 0;
+ protected double lastMeasuredTimestamp=System.currentTimeMillis();
+
+ public Map redScoringBreakdown = new Hashtable();
+ public Map blueScoringBreakdown = new Hashtable();
+ protected Map redPublishers = new Hashtable();
+ protected Map bluePublishers = new Hashtable();
+
+ public NetworkTable redTable =
+ NetworkTableInstance.getDefault().getTable("SmartDashboard/MapleSim/MatchData/Breakdown/Red Alliance");
+ public NetworkTable blueTable =
+ NetworkTableInstance.getDefault().getTable("SmartDashboard/MapleSim/MatchData/Breakdown/blue Alliance");
+ public NetworkTable genericInfoTable =
+ NetworkTableInstance.getDefault().getTable("SmartDashboard/MapleSim/MatchData/Breakdown");
+
+ public DoublePublisher matchClockPublisher =
+ genericInfoTable.getDoubleTopic("Match Clock").publish();
+
+ public static BooleanPublisher resetFieldPublisher = NetworkTableInstance.getDefault()
+ .getTable("SmartDashboard/MapleSim/MatchData")
+ .getBooleanTopic("Reset Field")
+ .publish();
+
+ public static BooleanSubscriber resetFieldSubscriber =
+ resetFieldPublisher.getTopic().subscribe(false);
+
+ Boolean shouldPublishMatchBreakdown = true;
+
+ private static SimulatedArena instance = null;
+ protected OpponentManager opponentManager;
+
+ /**
+ *
+ *
+ * Gets/Creates the Default Simulation World
+ *
+ * Multiple instances of {@link SimulatedArena} can exist elsewhere.
+ *
+ * @return the main simulation arena instance
+ * @throws IllegalStateException if the method is call when running on a real robot
+ */
+ public static SimulatedArena getInstance() {
+ if (RobotBase.isReal() && (!ALLOW_CREATION_ON_REAL_ROBOT))
+ throw new IllegalStateException(
+ "MapleSim is running on a real robot! (If you would actually want that, set SimulatedArena.ALLOW_CREATION_ON_REAL_ROBOT to true).");
+
+ if (instance == null) instance = new Arena2026Rebuilt(false);
+
+ return instance;
+ }
+
+ /**
+ *
+ *
+ *
Overrides the Default Simulation World
+ *
+ * Overrides the return value of {@link #getInstance()}
+ *
+ *
This method allows simulating an arena from a different year or a custom field.
+ *
+ *
Currently, only the 2024 arena is supported, so avoid calling this method for now.
+ *
+ * @param newInstance the new simulation arena instance to override the current one
+ */
+ public static void overrideInstance(SimulatedArena newInstance) {
+ if (instance != null) instance = new Arena2026Rebuilt();
+ instance = newInstance;
+ }
+
+ /** The number of sub-ticks the simulator will run in each robot period. */
+ private static int SIMULATION_SUB_TICKS_IN_1_PERIOD = 5;
+
+ public static int getSimulationSubTicksIn1Period() {
+ return SIMULATION_SUB_TICKS_IN_1_PERIOD;
+ }
+ /** The period length of each sub-tick, in seconds. */
+ private static Time SIMULATION_DT = Seconds.of(TimedRobot.kDefaultPeriod / SIMULATION_SUB_TICKS_IN_1_PERIOD);
+
+ public static Time getSimulationDt() {
+ return SIMULATION_DT;
+ }
+
+ /**
+ *
+ *
+ *
Returns the score of the specified team.
+ *
+ * @param isBlue The team to return the score of as a bool.
+ * @return The score of the specified team.
+ */
+ public int getScore(boolean isBlue) {
+ return isBlue ? blueScore : redScore;
+ }
+
+ /**
+ *
+ *
+ * Returns the score of the specified team.
+ *
+ * @param allianceColor The team to return the score of as a Alliance enum.
+ * @return The score of the specified team.
+ */
+ public int getScore(Alliance allianceColor) {
+ return getScore(allianceColor == Alliance.Blue);
+ }
+
+
+ /**
+ * Adds an OpponentManager to the SimulatedArena.
+ *
+ * @param opponentManager the OpponentManager to use.
+ */
+ protected void withOpponentManager(OpponentManager opponentManager) {
+ this.opponentManager = opponentManager;
+ }
+
+ /**
+ * Gets the current {@link OpponentManager}. Use casting for your Arena, some arenas may override with castless
+ * methods.
+ *
+ * @return the {@link OpponentManager} in use.
+ */
+ public OpponentManager getOpponentManager() {
+ return opponentManager;
+ }
+
+
+ /**
+ *
+ *
+ * Adds to the score of the specified team
+ *
+ * @param isBlue Wether to add to the blue or red team score.
+ * @param toAdd How many points to add.
+ */
+ public void addToScore(boolean isBlue, int toAdd) {
+ if (isBlue) blueScore += toAdd;
+ else redScore += toAdd;
+ addValueToMatchBreakdown(isBlue, DriverStation.isAutonomous() ? "Auto/AutoScore" : "TeleopScore", toAdd);
+ }
+
+ /**
+ *
+ *
+ * Overrides the Timing Configurations of the Simulations.
+ *
+ * If Using Advantage-Kit: DO NOT CHANGE THE
+ * DEFAULT TIMINGS
+ *
+ * Changes apply to every instance of {@link SimulatedArena}.
+ *
+ *
The new configuration will take effect the next time {@link SimulatedArena#simulationPeriodic()} is called on
+ * an instance.
+ *
+ *
It is recommended to call this method before the first call to {@link SimulatedArena#simulationPeriodic()} of
+ * any instance.
+ *
+ *
It is also recommended to keep the simulation frequency above 200 Hz for accurate simulation results.
+ *
+ * @param robotPeriod the time between two calls of {@link #simulationPeriodic()}, usually obtained from
+ * {@link TimedRobot#getPeriod()}
+ * @param simulationSubTicksPerPeriod the number of Iterations, or {@link #simulationSubTick(int)} that the
+ * simulation runs per each call to {@link #simulationPeriodic()}
+ */
+ public static synchronized void overrideSimulationTimings(Time robotPeriod, int simulationSubTicksPerPeriod) {
+ SIMULATION_SUB_TICKS_IN_1_PERIOD = simulationSubTicksPerPeriod;
+ SIMULATION_DT = robotPeriod.div(SIMULATION_SUB_TICKS_IN_1_PERIOD);
+ }
+
+ protected final World
physicsWorld;
+ protected final Set driveTrainSimulations;
+
+ protected final Set gamePieces;
+ protected final List customSimulations;
+
+ private final List intakeSimulations;
+
+ /**
+ *
+ *
+ * Constructs a new simulation arena with the specified field map of obstacles.
+ *
+ * This constructor initializes a physics world with zero gravity and adds the provided obstacles to the world.
+ *
+ *
It also sets up the collections for drivetrain simulations, game pieces, projectiles, and intake simulations.
+ *
+ * @param obstaclesMap the season-specific field map containing the layout of obstacles for the simulation
+ */
+ protected SimulatedArena(FieldMap obstaclesMap) {
+ this.physicsWorld = new World<>();
+ this.physicsWorld.setGravity(PhysicsWorld.ZERO_GRAVITY);
+ for (Body obstacle : obstaclesMap.obstacles) this.physicsWorld.addBody(obstacle);
+ this.driveTrainSimulations = new HashSet<>();
+ customSimulations = new ArrayList<>();
+ this.gamePieces = new HashSet<>();
+ this.intakeSimulations = new ArrayList<>();
+ setupValueForMatchBreakdown("TotalScore");
+ setupValueForMatchBreakdown("TeleopScore");
+ setupValueForMatchBreakdown("Auto/AutoScore");
+ resetFieldPublisher.set(false);
+ }
+
+ /**
+ *
+ *
+ *
Represents a custom simulation to be updated during each simulation sub-tick.
+ *
+ * This allows you to register custom actions that will be executed at a high frequency during each simulation
+ * sub-tick. This is useful for tasks that need to be updated multiple times per simulation cycle.
+ *
+ *
Examples of how this method is used:
+ *
+ *
+ * - Pulling encoder values for high-frequency odometry updates.
+ *
- Adding custom simulation objects or handling events in the simulated arena.
+ *
+ */
+ public interface Simulatable {
+ /**
+ * Called in {@link #simulationSubTick(int)}.
+ *
+ * @param subTickNum the number of this sub-tick (counting from 0 in each robot period)
+ */
+ void simulationSubTick(int subTickNum);
+ }
+
+ /**
+ *
+ *
+ * Registers a custom simulation.
+ *
+ * @param simulatable the custom simulation to register
+ */
+ public synchronized void addCustomSimulation(Simulatable simulatable) {
+ this.customSimulations.add(simulatable);
+ }
+
+ /**
+ *
+ *
+ * Registers an {@link IntakeSimulation}.
+ *
+ * NOTE: This method is automatically called in the constructor of {@link IntakeSimulation}, so
+ * you don't need to call it manually.
+ *
+ *
The intake simulation should be bound to an {@link AbstractDriveTrainSimulation} and becomes part of its
+ * collision space.
+ *
+ *
This method immediately starts the {@link org.ironmaple.simulation.IntakeSimulation.GamePieceContactListener},
+ * which listens for contact between the intake and any game piece.
+ *
+ * @param intakeSimulation the intake simulation to be registered
+ */
+ protected synchronized void addIntakeSimulation(IntakeSimulation intakeSimulation) {
+ this.intakeSimulations.add(intakeSimulation);
+ this.physicsWorld.addContactListener(intakeSimulation.getGamePieceContactListener());
+ }
+
+ /**
+ *
+ *
+ *
Registers an {@link AbstractDriveTrainSimulation}.
+ *
+ * The collision space of the drive train is immediately added to the simulation world.
+ *
+ *
Starting from the next call to {@link #simulationPeriodic()}, the
+ * {@link AbstractDriveTrainSimulation#simulationSubTick()} method will be called during each sub-tick of the
+ * simulator.
+ *
+ * @param driveTrainSimulation the drivetrain simulation to be registered
+ */
+ public synchronized void addDriveTrainSimulation(AbstractDriveTrainSimulation driveTrainSimulation) {
+ this.physicsWorld.addBody(driveTrainSimulation);
+
+ this.driveTrainSimulations.add(driveTrainSimulation);
+ }
+
+ /**
+ *
+ *
+ *
Registers a {@link GamePieceOnFieldSimulation} to the Simulation.
+ *
+ * The collision space of the game piece is immediately added to the simulation world.
+ *
+ *
{@link IntakeSimulation}s will be able to interact with this game piece during the next call to
+ * {@link SimulatedArena#simulationPeriodic()}.
+ *
+ * @param gamePiece the game piece to be registered in the simulation
+ */
+ public synchronized void addGamePiece(GamePieceOnFieldSimulation gamePiece) {
+ this.physicsWorld.addBody(gamePiece);
+ this.gamePieces.add(gamePiece);
+ }
+
+ /**
+ *
+ *
+ *
Tells the arena to start publishing the match breakdown data to network tables
+ */
+ public void enableBreakdownPublishing() {
+ shouldPublishMatchBreakdown = true;
+ }
+
+ /**
+ *
+ *
+ * Tells the arena to stop publishing the match breakdown data to network tables
+ */
+ public void disableBreakdownPublishing() {
+ shouldPublishMatchBreakdown = false;
+ }
+
+ /**
+ *
+ *
+ * Publishes the match breakdown data to network tables
+ */
+ protected void publishBreakdown() {
+
+ for (String key : redScoringBreakdown.keySet()) {
+ if (!redPublishers.containsKey(key))
+ redPublishers.put(key, redTable.getDoubleTopic(key).publish());
+
+ redPublishers.get(key).set(redScoringBreakdown.get(key));
+ }
+ for (String key : blueScoringBreakdown.keySet()) {
+ if (!bluePublishers.containsKey(key))
+ bluePublishers.put(key, blueTable.getDoubleTopic(key).publish());
+
+ bluePublishers.get(key).set(blueScoringBreakdown.get(key));
+ }
+
+ // genericInfoTable.getDoubleTopic("currentMatchTime").publish().set(blueScore);
+ }
+
+ /**
+ *
+ *
+ * replaces or adds a value to the match scoring breakdown published to network tables
+ *
+ * @param isBlueTeam Wether to add to the blue teams match breakdown or the red teams match breakdown
+ * @param valueKey The name of the value to be added
+ * @param value The value to be added
+ */
+ public void replaceValueInMatchBreakDown(boolean isBlueTeam, String valueKey, Double value) {
+ if (isBlueTeam) blueScoringBreakdown.put(valueKey, value);
+ else redScoringBreakdown.put(valueKey, value);
+ }
+
+ /**
+ *
+ *
+ * Defaults a value to 0 and creates it in the match breakdown. This is useful on startup to make sure all match
+ * breakdown values display before they are first updated
+ *
+ * @param valueKey The key to add to match breakdown
+ */
+ public void setupValueForMatchBreakdown(String valueKey) {
+ replaceValueInMatchBreakDown(true, valueKey, 0);
+ replaceValueInMatchBreakDown(false, valueKey, 0);
+ }
+
+ /**
+ *
+ *
+ * replaces or adds a value to the match scoring breakdown published to network tables
+ *
+ * @param isBlueTeam Wether to add to the blue teams match breakdown or the red teams match breakdown
+ * @param valueKey The name of the value to be added
+ * @param value The value to be added
+ */
+ public void replaceValueInMatchBreakDown(boolean isBlueTeam, String valueKey, Integer value) {
+ replaceValueInMatchBreakDown(isBlueTeam, valueKey, (double) value);
+ }
+
+ /**
+ *
+ *
+ * Adds too a value in the scoring breakdown. If value does not already exist in the scoring breakdown it will
+ * be defaulted to 0 and then added too
+ *
+ * @param isBlueTeam Wether to add to the blue teams match breakdown or the red teams match breakdown
+ * @param ValueKey The name of the value to be added too
+ * @param toAdd how much to be added to specified value
+ */
+ public void addValueToMatchBreakdown(boolean isBlueTeam, String ValueKey, Double toAdd) {
+ if (isBlueTeam) {
+ if (blueScoringBreakdown.get(ValueKey) == null) blueScoringBreakdown.put(ValueKey, toAdd);
+ else blueScoringBreakdown.put(ValueKey, blueScoringBreakdown.get(ValueKey) + toAdd);
+ } else {
+ if (redScoringBreakdown.get(ValueKey) == null) redScoringBreakdown.put(ValueKey, toAdd);
+ else redScoringBreakdown.put(ValueKey, redScoringBreakdown.get(ValueKey) + toAdd);
+ }
+ }
+
+ /**
+ *
+ *
+ * Adds too a value in the scoring breakdown. If value does not already exist in the scoring breakdown it will
+ * be defaulted to 0 and then added too
+ *
+ * @param isBlueTeam Wether to add to the blue teams match breakdown or the red teams match breakdown
+ * @param valueKey The name of the value to be added too
+ * @param toAdd how much to be added to specified value
+ */
+ public void addValueToMatchBreakdown(boolean isBlueTeam, String valueKey, int toAdd) {
+ addValueToMatchBreakdown(isBlueTeam, valueKey, (double) toAdd);
+ }
+
+ /**
+ *
+ *
+ * Registers a {@link GamePieceProjectile} to the Simulation and Launches It.
+ *
+ *
Calls to {@link GamePieceProjectile#launch()}, which will launch the game piece immediately.
+ *
+ * @param gamePieceProjectile the projectile to be registered and launched in the simulation
+ */
+ public synchronized void addGamePieceProjectile(GamePieceProjectile gamePieceProjectile) {
+ this.gamePieces.add(gamePieceProjectile);
+ gamePieceProjectile.launch();
+ }
+
+ /**
+ *
+ *
+ *
Removes a {@link GamePieceOnFieldSimulation} from the Simulation.
+ *
+ * Removes the game piece from the physics world and the simulation's game piece collection.
+ *
+ * @param gamePiece the game piece to be removed from the simulation
+ * @return true if this set contained the specified element
+ */
+ public synchronized boolean removeGamePiece(GamePieceOnFieldSimulation gamePiece) {
+ this.physicsWorld.removeBody(gamePiece);
+ return this.gamePieces.remove(gamePiece);
+ }
+
+ public synchronized boolean removePiece(GamePiece toRemove) {
+ if (toRemove.isGrounded()) {
+ return removeGamePiece((GamePieceOnFieldSimulation) toRemove);
+ }
+ return removeProjectile((GamePieceProjectile) toRemove);
+ }
+
+ /**
+ *
+ *
+ *
Removes a {@link GamePieceProjectile} from the Simulation.
+ *
+ * Removes the game piece projectile from the simulation.
+ *
+ * @param gamePieceLaunched the game piece projectile to be removed from the simulation
+ * @return true if this set contained the specified element
+ */
+ public synchronized boolean removeProjectile(GamePieceProjectile gamePieceLaunched) {
+ return this.gamePieces.remove(gamePieceLaunched);
+ }
+
+ /**
+ *
+ *
+ *
Removes All {@link GamePieceOnFieldSimulation} Objects from the Simulation.
+ *
+ * This method clears all game pieces from the physics world and the simulation's game piece collection.
+ */
+ public synchronized void clearGamePieces() {
+ for (GamePieceOnFieldSimulation gamePiece : this.gamePiecesOnField()) this.physicsWorld.removeBody(gamePiece);
+
+ this.gamePieces.clear();
+ this.blueScore = 0;
+ this.redScore = 0;
+ }
+
+ /**
+ *
+ *
+ *
Shuts down the current SimulatedArena and stops all simulation so that simulatable objects may be added to a
+ * new arena
+ */
+ public synchronized void shutDown() {
+ this.physicsWorld.removeAllBodies();
+ }
+
+ /**
+ *
+ *
+ * Update the simulation world.
+ *
+ * This method should be called ONCE in {@link TimedRobot#simulationPeriodic()} (or
+ * LoggedRobot.simulationPeriodic() if using Advantage-Kit)
+ *
+ *
If not configured through {@link SimulatedArena#overrideSimulationTimings(Time, int)} , the simulator will
+ * iterate through 5 Sub-ticks by default.
+ *
+ *
The amount of CPU Time that the Dyn4j engine uses in displayed in
+ * SmartDashboard/MapleArenaSimulation/Dyn4jEngineCPUTimeMS, usually performance is not a concern
+ */
+ public synchronized void simulationPeriodic() {
+ /* obtain lock to the simulated arena class to block any calls to overrideTimings() */
+ synchronized (SimulatedArena.class) {
+ final long t0 = System.nanoTime();
+ // move through a few sub-periods in each update
+ for (int i = 0; i < SIMULATION_SUB_TICKS_IN_1_PERIOD; i++) simulationSubTick(i);
+
+ matchClock += (System.currentTimeMillis() - lastMeasuredTimestamp)/1000.0;
+ lastMeasuredTimestamp = System.currentTimeMillis();
+
+ SmartDashboard.putNumber("MapleArenaSimulation/Dyn4jEngineCPUTimeMS", (System.nanoTime() - t0) / 1000000.0);
+
+ if (resetFieldSubscriber.get()) {
+ SimulatedArena.getInstance().resetFieldForAuto();
+ resetFieldPublisher.set(false);
+ matchClock = 0;
+ }
+ }
+ }
+
+ /**
+ *
+ *
+ *
Processes a Single Simulation Sub-Tick.
+ *
+ * This method performs the actions for each sub-tick of the simulation, including:
+ *
+ *
+ * - Updating all registered {@link AbstractDriveTrainSimulation} objects.
+ *
- Updating all {@link GamePieceProjectile} objects in the simulation.
+ *
- Stepping the physics world with the specified sub-tick duration.
+ *
- Removing any game pieces as detected by the {@link IntakeSimulation} objects.
+ *
- Executing any additional sub-tick actions registered via
+ * {@link SimulatedArena#addCustomSimulation(Simulatable)} .
+ *
+ */
+ protected void simulationSubTick(int subTickNum) {
+ SimulatedBattery.simulationSubTick();
+ driveTrainSimulations.forEach(AbstractDriveTrainSimulation::simulationSubTick);
+
+ GamePieceProjectile.updateGamePieceProjectiles(this, this.gamePieceLaunched());
+
+ this.physicsWorld.step(1, SIMULATION_DT.in(Seconds));
+
+ intakeSimulations.forEach(intake -> intake.removeObtainedGamePieces(this));
+ customSimulations.forEach(sim -> sim.simulationSubTick(subTickNum));
+
+ replaceValueInMatchBreakDown(true, "TotalScore", blueScore);
+ replaceValueInMatchBreakDown(false, "TotalScore", redScore);
+
+ if (shouldPublishMatchBreakdown) {
+ publishBreakdown();
+ matchClockPublisher.set(matchClock);
+ }
+ }
+
+ /**
+ *
+ *
+ * Returns a list of all grounded pieces on the field
+ *
+ * @return all grounded (aka not projectile) pieces on the field as a set of GamePieceOnFieldSimulation objects
+ */
+ public synchronized Set gamePiecesOnField() {
+ Set returnList = new HashSet();
+ for (GamePiece gamePiece : this.gamePieces) {
+ if (gamePiece.isGrounded()) {
+ returnList.add((GamePieceOnFieldSimulation) gamePiece);
+ }
+ }
+
+ return returnList;
+ }
+
+ /**
+ *
+ *
+ * Returns a list of all projectile pieces on the field
+ *
+ * @return all projectile pieces on the field as a set of GamePieceProjectile objects
+ */
+ public synchronized Set gamePieceLaunched() {
+ Set returnList = new HashSet();
+ for (GamePiece gamePiece : this.gamePieces) {
+ if (!gamePiece.isGrounded()) {
+ returnList.add((GamePieceProjectile) gamePiece);
+ }
+ }
+
+ return returnList;
+ }
+
+ /**
+ *
+ *
+ * Obtains the 3D Poses of a Specific Type of Game Piece.
+ *
+ * This method is used to visualize the positions of game pieces
+ *
+ *
Also, if you have a game-piece detection vision system (wow!), this is the how you can
+ * simulate it.
+ *
+ *
Both {@link GamePieceOnFieldSimulation} and {@link GamePieceProjectile} of the specified type will be
+ * included.
+ *
+ *
+ * - The type is determined in the constructor of {@link GamePieceOnFieldSimulation}.
+ *
- For example, {@link org.ironmaple.simulation.seasonspecific.crescendo2024.CrescendoNoteOnField} has the
+ * type "Note".
+ *
+ *
+ * @param type the type of game piece, as determined by the constructor of {@link GamePieceOnFieldSimulation}
+ * @return a {@link List} of {@link Pose3d} objects representing the 3D positions of the game pieces
+ */
+ public synchronized List getGamePiecesPosesByType(String type) {
+ final List gamePiecesPoses = new ArrayList<>();
+ for (GamePiece gamePiece : gamePieces)
+ if (Objects.equals(gamePiece.getType(), type)) gamePiecesPoses.add(gamePiece.getPose3d());
+
+ return gamePiecesPoses;
+ }
+
+ /**
+ *
+ *
+ * Obtains the 3D Poses of a Specific Type of Game Piece as an array.
+ *
+ * @see #getGamePiecesPosesByType(String)
+ */
+ public synchronized Pose3d[] getGamePiecesArrayByType(String type) {
+ return getGamePiecesPosesByType(type).toArray(Pose3d[]::new);
+ }
+
+ /**
+ *
+ *
+ * Returns all game pieces on the field of the specified type as a list
+ *
+ * @param type The string type to be selected.
+ * @return The game pieces as a list of {@link GamePiece}
+ */
+ public synchronized List getGamePiecesByType(String type) {
+ final List gamePiecesPoses = new ArrayList<>(this.gamePieces);
+ return gamePiecesPoses.stream().filter(gamePiece -> !Objects.equals(gamePiece.getType(), type)).toList();
+ }
+
+ /**
+ *
+ *
+ * Resets the Field for Autonomous Mode.
+ *
+ * This method clears all current game pieces from the field and places new game pieces in their starting
+ * positions for the autonomous mode.
+ */
+ public synchronized void resetFieldForAuto() {
+ clearGamePieces();
+ matchClock = 0;
+ placeGamePiecesOnField();
+ }
+
+ /**
+ *
+ *
+ *
Places Game Pieces on the Field for Autonomous Mode.
+ *
+ * This method sets up the game pieces on the field, typically in their starting positions for autonomous mode.
+ *
+ *
It should be implemented differently for each season-specific subclass of {@link SimulatedArena} to reflect
+ * the unique game piece placements for that season's game.
+ */
+ public abstract void placeGamePiecesOnField();
+
+ /**
+ *
+ *
+ *
Represents an Abstract Field Map
+ *
+ * Stores the layout of obstacles and game pieces.
+ *
+ *
For each season-specific subclass of {@link SimulatedArena}, there should be a corresponding subclass of this
+ * class to store the field map for that specific season's game.
+ */
+ public abstract static class FieldMap {
+ private final List
obstacles = new ArrayList<>();
+
+ protected void addBorderLine(Translation2d startingPoint, Translation2d endingPoint) {
+ addCustomObstacle(
+ Geometry.createSegment(
+ GeometryConvertor.toDyn4jVector2(startingPoint),
+ GeometryConvertor.toDyn4jVector2(endingPoint)),
+ new Pose2d());
+ }
+
+ protected void addRectangularObstacle(double width, double height, Pose2d absolutePositionOnField) {
+ addCustomObstacle(Geometry.createRectangle(width, height), absolutePositionOnField);
+ }
+
+ protected void addCustomObstacle(Convex shape, Pose2d absolutePositionOnField) {
+ final Body obstacle = createObstacle(shape);
+
+ obstacle.getTransform().set(GeometryConvertor.toDyn4jTransform(absolutePositionOnField));
+
+ obstacles.add(obstacle);
+ }
+
+ private static Body createObstacle(Convex shape) {
+ final Body obstacle = new Body();
+ obstacle.setMass(MassType.INFINITE);
+ final BodyFixture fixture = obstacle.addFixture(shape);
+ fixture.setFriction(0.6);
+ fixture.setRestitution(0.3);
+ return obstacle;
+ }
+ }
+}
diff --git a/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/AbstractDriveTrainSimulation.java b/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/AbstractDriveTrainSimulation.java
new file mode 100644
index 0000000..93e2471
--- /dev/null
+++ b/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/AbstractDriveTrainSimulation.java
@@ -0,0 +1,169 @@
+package swervelib.simulation.ironmaple.simulation.drivesims;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import org.dyn4j.dynamics.Body;
+import org.dyn4j.geometry.Geometry;
+import org.dyn4j.geometry.MassType;
+import swervelib.simulation.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig;
+import swervelib.simulation.ironmaple.utils.mathutils.GeometryConvertor;
+
+import static edu.wpi.first.units.Units.Meters;
+
+/**
+ *
+ *
+ * Represents an Abstract Drivetrain Simulation.
+ *
+ * Simulates the Mass, Collision Space, and Friction of the Drivetrain.
+ *
+ * This class models the physical properties of a drivetrain, including mass and collision space.
+ *
+ *
It also provides APIs to obtain the status (position, velocity etc.) in WPILib geometry classes.
+ *
+ *
The propelling forces generated by motors are simulated in its subclass, or {@link SwerveDriveSimulation}.
+ */
+public abstract class AbstractDriveTrainSimulation extends Body {
+ public static final double
+ BUMPER_COEFFICIENT_OF_FRICTION = 0.65, // https://en.wikipedia.org/wiki/Friction#Coefficient_of_friction
+ BUMPER_COEFFICIENT_OF_RESTITUTION = 0.08; // https://simple.wikipedia.org/wiki/Coefficient_of_restitution
+
+ public final DriveTrainSimulationConfig config;
+
+ /**
+ *
+ *
+ *
Creates a Simulation of a Drivetrain.
+ *
+ * Sets Up the Collision Space and Mass of the Chassis.
+ *
+ * Since this is an abstract class, the constructor must be called from a subclass.
+ *
+ *
Note that the chassis does not appear on the simulation field upon creation. Refer to
+ * {@link swervelib.simulation.ironmaple.simulation.SimulatedArena} for instructions on how to add it to the simulation world.
+ *
+ * @param config a {@link DriveTrainSimulationConfig} instance containing the configurations of this drivetrain
+ * @param initialPoseOnField the initial pose of the drivetrain in the simulation world
+ */
+ protected AbstractDriveTrainSimulation(DriveTrainSimulationConfig config, Pose2d initialPoseOnField) {
+
+ this.config = config;
+ /* width and height in world reference is flipped */
+ final double WIDTH_IN_WORLD_REFERENCE = config.bumperLengthX.in(Meters),
+ HEIGHT_IN_WORLD_REFERENCE = config.bumperWidthY.in(Meters);
+
+ super.addFixture(
+ Geometry.createRectangle(WIDTH_IN_WORLD_REFERENCE, HEIGHT_IN_WORLD_REFERENCE),
+ config.getDensityKgPerSquaredMeters(),
+ BUMPER_COEFFICIENT_OF_FRICTION,
+ BUMPER_COEFFICIENT_OF_RESTITUTION);
+
+ super.setMass(MassType.NORMAL);
+ super.setLinearDamping(0.1);
+ super.setAngularDamping(0.1);
+ setSimulationWorldPose(initialPoseOnField);
+ }
+
+ /**
+ *
+ *
+ *
Sets the Robot's Current Pose in the Simulation World.
+ *
+ * This method instantly teleports the robot to the specified pose in the simulation world. The robot does not
+ * drive to the new pose; it is moved directly.
+ *
+ * @param robotPose the desired robot pose, represented as a {@link Pose2d}
+ */
+ public void setSimulationWorldPose(Pose2d robotPose) {
+ super.transform.set(GeometryConvertor.toDyn4jTransform(robotPose));
+ super.linearVelocity.set(0, 0);
+ }
+
+ /**
+ *
+ *
+ *
Sets the Robot's Speeds to the Given Chassis Speeds.
+ *
+ * This method sets the robot's current velocity to the specified chassis speeds.
+ *
+ *
The robot does not accelerate smoothly to these speeds; instead, it jumps to the velocity
+ * Instantaneously.
+ *
+ * @param givenSpeeds the desired chassis speeds, represented as a {@link ChassisSpeeds} object
+ */
+ public void setRobotSpeeds(ChassisSpeeds givenSpeeds) {
+ super.setLinearVelocity(GeometryConvertor.toDyn4jLinearVelocity(givenSpeeds));
+ super.setAngularVelocity(givenSpeeds.omegaRadiansPerSecond);
+ }
+
+ /**
+ *
+ *
+ *
Abstract Simulation Sub-Tick Method.
+ *
+ * This method is called every time the simulation world is updated.
+ *
+ *
It is implemented in the sub-classes of {@link AbstractDriveTrainSimulation}.
+ *
+ *
It is responsible for applying the propelling forces to the robot during each sub-tick of the simulation.
+ */
+ public abstract void simulationSubTick();
+
+ /**
+ *
+ *
+ *
Gets the Actual Pose of the Drivetrain in the Simulation World.
+ *
+ * This method is used to display the robot on AdvantageScope Field3d or to update
+ * vision simulations.
+ *
+ *
Note: Do not use this method to simulate odometry! For a more realistic odometry simulation,
+ * use a {@link SwerveDriveSimulation} together with a
+ * {@link edu.wpi.first.math.estimator.SwerveDrivePoseEstimator}.
+ *
+ * @return a {@link Pose2d} object yielding the current world pose of the robot in the simulation
+ */
+ public Pose2d getSimulatedDriveTrainPose() {
+ return GeometryConvertor.toWpilibPose2d(getTransform());
+ }
+
+ /**
+ *
+ *
+ *
Gets the Actual Robot-Relative Chassis Speeds from the Simulation.
+ *
+ * This method returns the actual chassis speeds of the drivetrain in the simulation, relative to the robot.
+ *
+ *
To simulate the chassis speeds calculated by encoders, use a {@link SwerveDriveSimulation} together with
+ * {@link edu.wpi.first.math.kinematics.SwerveDriveKinematics#toChassisSpeeds(SwerveModuleState...)} for a more
+ * realistic simulation.
+ *
+ * @return the actual chassis speeds in the simulation world, Robot-Relative
+ */
+ public ChassisSpeeds getDriveTrainSimulatedChassisSpeedsRobotRelative() {
+ ChassisSpeeds speeds = getDriveTrainSimulatedChassisSpeedsFieldRelative();
+ speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
+ speeds, getSimulatedDriveTrainPose().getRotation());
+ return speeds;
+ }
+
+ /**
+ *
+ *
+ *
Gets the Actual Field-Relative Chassis Speeds from the Simulation.
+ *
+ * This method returns the actual chassis speeds of the drivetrain in the simulation, relative to the robot.
+ *
+ *
To simulate the chassis speeds calculated by encoders, use a {@link SwerveDriveSimulation} together with
+ * {@link edu.wpi.first.math.kinematics.SwerveDriveKinematics#toChassisSpeeds(SwerveModuleState...)} for a more
+ * realistic simulation.
+ *
+ * @return the actual chassis speeds in the simulation world, Field-Relative
+ */
+ public ChassisSpeeds getDriveTrainSimulatedChassisSpeedsFieldRelative() {
+ return GeometryConvertor.toWpilibChassisSpeeds(getLinearVelocity(), getAngularVelocity());
+ }
+}
diff --git a/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/COTS.java b/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/COTS.java
new file mode 100644
index 0000000..c687c43
--- /dev/null
+++ b/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/COTS.java
@@ -0,0 +1,408 @@
+package swervelib.simulation.ironmaple.simulation.drivesims;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import swervelib.simulation.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig;
+
+import java.util.function.Supplier;
+
+import static edu.wpi.first.units.Units.*;
+
+public class COTS {
+ /**
+ *
+ *
+ *
Stores the coefficient of friction of some common used wheels.
+ *
+ * Data comes from Spectrum
+ * 3847's Build Blog.
+ */
+ public enum WHEELS {
+ /**
+ * Colsons Wheels.
+ */
+ COLSONS(0.899),
+ /**
+ * Default Neoprene Treads for Mark4 Modules
+ */
+ DEFAULT_NEOPRENE_TREAD(1.426),
+ /**
+ * Blue Nitrile
+ * Tread from AndyMark.
+ */
+ BLUE_NITRILE_TREAD(1.542),
+ /**
+ * Vex Grip V2 Wheel.
+ */
+ VEX_GRIP_V2(1.916),
+ /**
+ * Team 88's TPU90A Grippy Tire
+ */
+ SLS_PRINTED_WHEELS(2.106);
+
+ public final double cof;
+
+ WHEELS(double cof) {
+ this.cof = cof;
+ }
+ }
+
+ /**
+ * creates a SDS Mark4
+ * Swerve Module for simulation
+ */
+ public static SwerveModuleSimulationConfig ofMark4(
+ DCMotor driveMotor, DCMotor steerMotor, double wheelCOF, int gearRatioLevel) {
+ return new SwerveModuleSimulationConfig(
+ driveMotor,
+ steerMotor,
+ switch (gearRatioLevel) {
+ case 1 -> 8.14;
+ case 2 -> 6.75;
+ case 3 -> 6.12;
+ case 4 -> 5.14;
+ default -> throw new IllegalStateException("Unknown gearing level: " + gearRatioLevel);
+ },
+ 12.8,
+ Volts.of(0.1),
+ Volts.of(0.2),
+ Inches.of(2),
+ KilogramSquareMeters.of(0.03),
+ wheelCOF);
+ }
+
+ /**
+ * creates a SDS
+ * Mark4-i Swerve Module for simulation
+ */
+ public static SwerveModuleSimulationConfig ofMark4i(
+ DCMotor driveMotor, DCMotor steerMotor, double wheelCOF, int gearRatioLevel) {
+ return new SwerveModuleSimulationConfig(
+ driveMotor,
+ steerMotor,
+ switch (gearRatioLevel) {
+ case 1 -> 8.14;
+ case 2 -> 6.75;
+ case 3 -> 6.12;
+ case 4 -> 5.15;
+ default -> throw new IllegalStateException("Unknown gearing level: " + gearRatioLevel);
+ },
+ 150.0 / 7.0,
+ Volts.of(0.1),
+ Volts.of(0.2),
+ Inches.of(2),
+ KilogramSquareMeters.of(0.03),
+ wheelCOF);
+ }
+
+ /**
+ * creates a SDS Mark4-n Swerve
+ * Module for simulation
+ */
+ public static SwerveModuleSimulationConfig ofMark4n(
+ DCMotor driveMotor, DCMotor steerMotor, double wheelCOF, int gearRatioLevel) {
+ return new SwerveModuleSimulationConfig(
+ driveMotor,
+ steerMotor,
+ switch (gearRatioLevel) {
+ case 1 -> 7.13;
+ case 2 -> 5.9;
+ case 3 -> 5.36;
+ default -> throw new IllegalStateException("Unknown gearing level: " + gearRatioLevel);
+ },
+ 18.75,
+ Volts.of(0.1),
+ Volts.of(0.2),
+ Inches.of(2),
+ KilogramSquareMeters.of(0.03),
+ wheelCOF);
+ }
+
+ /**
+ * creates a WCP SwerveX Swerve Module
+ * for simulation
+ */
+ public static SwerveModuleSimulationConfig ofSwerveX(
+ DCMotor driveMotor, DCMotor steerMotor, double wheelCOF, int gearRatioLevel, double firstStageRatio) {
+ double secondStageRatio =
+ switch (gearRatioLevel) {
+ case 1 -> 26.0 / 20.0;
+ case 2, 3 -> 28.0 / 18.0;
+ default -> throw new IllegalStateException("Unknown gearing level: " + gearRatioLevel);
+ };
+ return new SwerveModuleSimulationConfig(
+ driveMotor,
+ steerMotor,
+ firstStageRatio * secondStageRatio,
+ 11.3142,
+ Volts.of(0.1),
+ Volts.of(0.2),
+ Inches.of(2),
+ KilogramSquareMeters.of(0.03),
+ wheelCOF);
+ }
+
+ /**
+ * creates a WCP SwerveX Flipped
+ * Swerve Module for simulation
+ *
+ *
X1 Ratios are gearRatioLevel 1-3
+ * X2 Ratios are gearRatioLevel 4-6
+ * X3 Ratios are gearRatioLevel 7-9
+ */
+ public static SwerveModuleSimulationConfig ofSwerveXFlipped(
+ DCMotor driveMotor, DCMotor steerMotor, double wheelCOF, int gearRatioLevel, int pinionSize) {
+ var unknownPinionErr = new IllegalStateException("Unknown pinion size: " + pinionSize);
+ var unknownLevelErr = new IllegalStateException("Unknown gearing level: " + gearRatioLevel);
+ return new SwerveModuleSimulationConfig(
+ driveMotor,
+ steerMotor,
+ switch (gearRatioLevel) {
+ case 1 -> switch (pinionSize) {
+ case 10 -> 8.1;
+ case 11 -> 7.36;
+ case 12 -> 6.75;
+ default -> throw unknownPinionErr;
+ };
+ case 2 -> switch (pinionSize) {
+ case 10 -> 6.72;
+ case 11 -> 6.11;
+ case 12 -> 5.6;
+ default -> throw unknownPinionErr;
+ };
+ case 3 -> switch (pinionSize) {
+ case 10 -> 5.51;
+ case 11 -> 5.01;
+ case 12 -> 4.59;
+ default -> throw unknownPinionErr;
+ };
+ default -> throw unknownLevelErr;
+ },
+ 13.3714,
+ Volts.of(0.1),
+ Volts.of(0.2),
+ Inches.of(2),
+ KilogramSquareMeters.of(0.03),
+ wheelCOF);
+ }
+
+ /**
+ * creates a WCP SwerveXS Swerve
+ * Module for simulation
+ */
+ public static SwerveModuleSimulationConfig ofSwerveXS(
+ DCMotor driveMotor, DCMotor steerMotor, double wheelCOF, int gearRatioLevel, int pinionSize) {
+ var unknownPinionErr = new IllegalStateException("Unknown pinion size: " + pinionSize);
+ var unknownLevelErr = new IllegalStateException("Unknown gearing level: " + gearRatioLevel);
+ return new SwerveModuleSimulationConfig(
+ driveMotor,
+ steerMotor,
+ switch (gearRatioLevel) {
+ case 1 -> switch (pinionSize) {
+ case 12 -> 6;
+ case 13 -> 5.54;
+ case 14 -> 5.14;
+ default -> throw unknownPinionErr;
+ };
+ case 2 -> switch (pinionSize) {
+ case 12 -> 4.71;
+ case 13 -> 4.4;
+ case 14 -> 4.13;
+ default -> throw unknownPinionErr;
+ };
+ default -> throw unknownLevelErr;
+ },
+ 41.25,
+ Volts.of(0.1),
+ Volts.of(0.2),
+ Inches.of(2),
+ KilogramSquareMeters.of(0.03),
+ wheelCOF);
+ }
+
+ /**
+ * creates a WCP SwerveX2 Swerve
+ * Module for simulation
+ *
+ *
X1 Ratios are gearRatioLevel 1-3
+ * X2 Ratios are gearRatioLevel 4-6
+ * X3 Ratios are gearRatioLevel 7-9
+ * X4 Ratios are gearRatioLevel 10-12
+ */
+ public static SwerveModuleSimulationConfig ofSwerveX2(
+ DCMotor driveMotor, DCMotor steerMotor, double wheelCOF, int gearRatioLevel, int pinionSize) {
+ var unknownPinionErr = new IllegalStateException("Unknown pinion size: " + pinionSize);
+ var unknownLevelErr = new IllegalStateException("Unknown gearing level: " + gearRatioLevel);
+ return new SwerveModuleSimulationConfig(
+ driveMotor,
+ steerMotor,
+ switch (gearRatioLevel) {
+ case 1 -> switch (pinionSize) {
+ case 10 -> 7.67;
+ case 11 -> 6.98;
+ case 12 -> 6.39;
+ default -> throw unknownPinionErr;
+ };
+ case 2 -> switch (pinionSize) {
+ case 10 -> 6.82;
+ case 11 -> 6.2;
+ case 12 -> 5.68;
+ default -> throw unknownPinionErr;
+ };
+ case 3 -> switch (pinionSize) {
+ case 10 -> 6.48;
+ case 11 -> 5.89;
+ case 12 -> 5.4;
+ default -> throw unknownPinionErr;
+ };
+ case 4 -> switch (pinionSize) {
+ case 10 -> 5.67;
+ case 11 -> 5.15;
+ case 12 -> 4.73;
+ default -> throw unknownPinionErr;
+ };
+ default -> throw unknownLevelErr;
+ },
+ 12.1,
+ Volts.of(0.1),
+ Volts.of(0.2),
+ Inches.of(2),
+ KilogramSquareMeters.of(0.03),
+ wheelCOF);
+ }
+
+ /**
+ * creates a WCP SwerveX2S Swerve
+ * Module for simulation
+ */
+ public static SwerveModuleSimulationConfig ofSwerveX2S(
+ DCMotor driveMotor, DCMotor steerMotor, double wheelCOF, int gearRatioLevel, int pinionSize) {
+ var unknownPinionErr = new IllegalStateException("Unknown pinion size: " + pinionSize);
+ var unknownLevelErr = new IllegalStateException("Unknown gearing level: " + gearRatioLevel);
+ return new SwerveModuleSimulationConfig(
+ driveMotor,
+ steerMotor,
+ switch (gearRatioLevel) {
+ case 1 -> switch (pinionSize) {
+ case 15 -> 6.0;
+ case 16 -> 5.63;
+ case 17 -> 5.29;
+ default -> throw unknownPinionErr;
+ };
+ case 2 -> switch (pinionSize) {
+ case 17 -> 4.94;
+ case 18 -> 4.67;
+ case 19 -> 4.42;
+ default -> throw unknownPinionErr;
+ };
+ case 3 -> switch (pinionSize) {
+ case 19 -> 4.11;
+ case 20 -> 3.9;
+ case 21 -> 3.71;
+ default -> throw unknownPinionErr;
+ };
+ default -> throw unknownLevelErr;
+ },
+ 25.9,
+ Volts.of(0.1),
+ Volts.of(0.2),
+ Inches.of(2),
+ KilogramSquareMeters.of(0.03),
+ wheelCOF);
+ }
+
+ /**
+ * Creates a REV MAXSwerve swerve module for simulation
+ *
+ *
Base Kit ratios are gearRatioLevel 1-3
+ * Gear Ratio Upgrade Kit ratios are gearRatioLevel 4-8
+ */
+ public static SwerveModuleSimulationConfig ofMAXSwerve(
+ DCMotor driveMotor, DCMotor steerMotor, double wheelCOF, int gearRatioLevel) {
+ return new SwerveModuleSimulationConfig(
+ driveMotor,
+ steerMotor,
+ switch (gearRatioLevel) {
+ case 1 -> 5.5;
+ case 2 -> 5.08;
+ case 3 -> 4.71;
+ case 4 -> 4.50;
+ case 5 -> 4.29;
+ case 6 -> 4;
+ case 7 -> 3.75;
+ case 8 -> 3.56;
+ default -> throw new IllegalStateException("Unknown gearing level: " + gearRatioLevel);
+ },
+ 9424.0 / 203.0,
+ Volts.of(0.1),
+ Volts.of(0.1),
+ Inches.of(1.5),
+ KilogramSquareMeters.of(0.02),
+ wheelCOF);
+ }
+
+ /**
+ * Creates a TTB Thrifty Swerve swerve module
+ * for simulation
+ */
+ public static SwerveModuleSimulationConfig ofThriftySwerve(
+ DCMotor driveMotor, DCMotor steerMotor, double wheelCOF, int gearRatioLevel) {
+ return new SwerveModuleSimulationConfig(
+ driveMotor,
+ steerMotor,
+ switch (gearRatioLevel) {
+ case 1 -> 6.75;
+ case 2 -> 6.23;
+ case 3 -> 5.79;
+ case 4 -> 6;
+ case 5 -> 5.54;
+ case 6 -> 5.14;
+ default -> throw new IllegalStateException("Unknown gearing level: " + gearRatioLevel);
+ },
+ 25,
+ Volts.of(0.1),
+ Volts.of(0.2),
+ Inches.of(2),
+ KilogramSquareMeters.of(0.03),
+ wheelCOF);
+ }
+
+ /**
+ *
+ *
+ *
+ *
+ * @return a gyro simulation factory configured for the Pigeon 2 IMU
+ */
+ public static Supplier ofPigeon2() {
+ /*
+ * user manual of pigeon 2:
+ * https://store.ctr-electronics.com/content/user-manual/Pigeon2%20User's%20Guide.pdf
+ * */
+ return () -> new GyroSimulation(0.5, 0.02);
+ }
+
+ /**
+ *
+ *
+ * Creates the Simulation for a navX2-MXP IMU.
+ *
+ * @return a gyro simulation factory configured for the navX2-MXP IMU
+ */
+ public static Supplier ofNav2X() {
+ return () -> new GyroSimulation(2, 0.04);
+ }
+
+ /**
+ *
+ *
+ * Creates the Simulation for a Generic, Low-Accuracy IMU.
+ *
+ * @return a gyro simulation factory configured for a generic low-accuracy IMU
+ */
+ public static Supplier ofGenericGyro() {
+ return () -> new GyroSimulation(5, 0.06);
+ }
+}
diff --git a/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/GyroSimulation.java b/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/GyroSimulation.java
new file mode 100644
index 0000000..ffb477a
--- /dev/null
+++ b/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/GyroSimulation.java
@@ -0,0 +1,213 @@
+package swervelib.simulation.ironmaple.simulation.drivesims;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Time;
+import swervelib.simulation.ironmaple.simulation.SimulatedArena;
+import swervelib.simulation.ironmaple.utils.mathutils.MapleCommonMath;
+
+import java.util.Queue;
+import java.util.concurrent.ConcurrentLinkedQueue;
+
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.Seconds;
+
+/**
+ * Simulation for a IMU module used as gyro.
+ *
+ * The Simulation is basically an indefinite integral of the angular velocity during each simulation sub ticks. Above
+ * that, it also musicales the measurement inaccuracy of the gyro, drifting in no-motion and drifting due to impacts.
+ */
+public class GyroSimulation {
+ private static final double
+ /* The threshold of instantaneous angular acceleration at which the chassis is considered to experience an "impact." */
+ ANGULAR_ACCELERATION_THRESHOLD_START_DRIFTING = 500,
+ /* The amount of drift, in radians, that the gyro experiences as a result of each multiple of the angular acceleration threshold. */
+ DRIFT_DUE_TO_IMPACT_COEFFICIENT = Math.toRadians(1);
+ private final double AVERAGE_DRIFTING_IN_30_SECS_MOTIONLESS_DEG, VELOCITY_MEASUREMENT_STANDARD_DEVIATION_PERCENT;
+
+ private Rotation2d gyroReading;
+ private double measuredAngularVelocityRadPerSec, previousAngularVelocityRadPerSec;
+ private final Queue cachedRotations;
+
+ /**
+ *
+ *
+ * Creates a Gyro Simulation.
+ *
+ * @param AVERAGE_DRIFTING_IN_30_SECS_MOTIONLESS_DEG the average amount of drift, in degrees, the gyro experiences
+ * if it remains motionless for 30 seconds on a vibrating platform. This value can often be found in the user
+ * manual.
+ * @param VELOCITY_MEASUREMENT_STANDARD_DEVIATION_PERCENT the standard deviation of the velocity measurement,
+ * typically around 0.05
+ */
+ public GyroSimulation(
+ double AVERAGE_DRIFTING_IN_30_SECS_MOTIONLESS_DEG, double VELOCITY_MEASUREMENT_STANDARD_DEVIATION_PERCENT) {
+ this.AVERAGE_DRIFTING_IN_30_SECS_MOTIONLESS_DEG = AVERAGE_DRIFTING_IN_30_SECS_MOTIONLESS_DEG;
+ this.VELOCITY_MEASUREMENT_STANDARD_DEVIATION_PERCENT = VELOCITY_MEASUREMENT_STANDARD_DEVIATION_PERCENT;
+
+ gyroReading = new Rotation2d();
+ this.previousAngularVelocityRadPerSec = this.measuredAngularVelocityRadPerSec = 0;
+ this.cachedRotations = new ConcurrentLinkedQueue<>();
+ for (int i = 0; i < SimulatedArena.getSimulationSubTicksIn1Period(); i++) cachedRotations.offer(gyroReading);
+ }
+
+ /**
+ *
+ *
+ * Calibrates the Gyro to a Given Rotation.
+ *
+ * This method sets the current rotation of the gyro, similar to Pigeon2().setYaw()
+ * .
+ *
+ *
After setting the rotation, the gyro will continue to estimate the rotation by integrating the angular
+ * velocity, adding it to the specified rotation.
+ *
+ * @param currentRotation the current rotation of the robot, represented as a {@link Rotation2d}
+ */
+ public void setRotation(Rotation2d currentRotation) {
+ this.gyroReading = currentRotation;
+ }
+
+ /**
+ *
+ *
+ *
Obtains the Estimated Rotation of the Gyro.
+ *
+ * This method returns the estimated rotation of the gyro, which includes measurement errors due to drifting and
+ * other factors.
+ *
+ * @return the current reading of the gyro, represented as a {@link Rotation2d}
+ */
+ public Rotation2d getGyroReading() {
+ return gyroReading;
+ }
+
+ /**
+ *
+ *
+ *
Gets the Measured Angular Velocity of the Gyro.
+ *
+ * This method returns the angular velocity measured by the gyro, in radians per second.
+ *
+ *
The measurement includes random errors based on the configured settings of the gyro.
+ *
+ * @return the measured angular velocity
+ */
+ public AngularVelocity getMeasuredAngularVelocity() {
+ return RadiansPerSecond.of(measuredAngularVelocityRadPerSec);
+ }
+
+ /**
+ * gyro readings for high-frequency
+ * odometers.
+ *
+ * @return the readings of the gyro during the last 5 simulation sub ticks
+ */
+ public Rotation2d[] getCachedGyroReadings() {
+ return cachedRotations.toArray(Rotation2d[]::new);
+ }
+
+ /**
+ *
+ *
+ *
Updates the Gyro Simulation for Each Sub-Tick.
+ *
+ * This method updates the gyro simulation and should be called during every sub-tick of the simulation.
+ *
+ *
If you are using this class outside of {@link swervelib.simulation.ironmaple.simulation.SimulatedArena}: make sure to call it 5
+ * times in each robot period (if using default timings), or refer to
+ * {@link swervelib.simulation.ironmaple.simulation.SimulatedArena#overrideSimulationTimings(Time, int)}.
+ *
+ * @param actualAngularVelocityRadPerSec the actual angular velocity in radians per second, usually obtained from
+ * {@link AbstractDriveTrainSimulation#getAngularVelocity()}
+ */
+ public void updateSimulationSubTick(double actualAngularVelocityRadPerSec) {
+ final Rotation2d driftingDueToImpact = getDriftingDueToImpact(actualAngularVelocityRadPerSec);
+ gyroReading = gyroReading.plus(driftingDueToImpact);
+
+ final Rotation2d dTheta = getGyroDTheta(actualAngularVelocityRadPerSec);
+ gyroReading = gyroReading.plus(dTheta);
+
+ final Rotation2d noMotionDrifting = getNoMotionDrifting();
+ gyroReading = gyroReading.plus(noMotionDrifting);
+
+ cachedRotations.poll();
+ cachedRotations.offer(gyroReading);
+ }
+
+ /**
+ *
+ *
+ *
Simulates IMU Drifting Due to Robot Impacts.
+ *
+ * This method generates a random amount of drifting for the IMU if the instantaneous angular acceleration
+ * exceeds a threshold, simulating the effects of impacts on the robot.
+ *
+ * @param actualAngularVelocityRadPerSec the actual angular velocity in radians per second, used to determine if an
+ * impact is detected
+ * @return the amount of drifting the IMU will experience if an impact is detected, or
+ * Rotation2d.fromRadians(0) if no impact is detected
+ */
+ private Rotation2d getDriftingDueToImpact(double actualAngularVelocityRadPerSec) {
+ final double
+ angularAccelerationRadPerSecSq =
+ (actualAngularVelocityRadPerSec - previousAngularVelocityRadPerSec)
+ / SimulatedArena.getSimulationDt().in(Seconds),
+ driftingDueToImpactDegAbsVal =
+ Math.abs(angularAccelerationRadPerSecSq) > ANGULAR_ACCELERATION_THRESHOLD_START_DRIFTING
+ ? Math.abs(angularAccelerationRadPerSecSq)
+ / ANGULAR_ACCELERATION_THRESHOLD_START_DRIFTING
+ * DRIFT_DUE_TO_IMPACT_COEFFICIENT
+ : 0,
+ driftingDueToImpactDeg = Math.copySign(driftingDueToImpactDegAbsVal, -angularAccelerationRadPerSecSq);
+
+ previousAngularVelocityRadPerSec = actualAngularVelocityRadPerSec;
+
+ return Rotation2d.fromRadians(driftingDueToImpactDeg);
+ }
+
+ /**
+ *
+ *
+ *
Gets the Measured ΔTheta of the Gyro.
+ *
+ * This method simulates the change in the robot's angle (ΔTheta) since the last sub-tick, as measured by the
+ * gyro.
+ *
+ *
The measurement includes random errors based on the configuration of the gyro.
+ *
+ * @param actualAngularVelocityRadPerSec the actual angular velocity in radians per second, used to calculate the
+ * ΔTheta
+ * @return the measured ΔTheta, including any measurement errors
+ */
+ private Rotation2d getGyroDTheta(double actualAngularVelocityRadPerSec) {
+ this.measuredAngularVelocityRadPerSec = MapleCommonMath.generateRandomNormal(
+ actualAngularVelocityRadPerSec,
+ VELOCITY_MEASUREMENT_STANDARD_DEVIATION_PERCENT * Math.abs(actualAngularVelocityRadPerSec));
+ return Rotation2d.fromRadians(measuredAngularVelocityRadPerSec
+ * SimulatedArena.getSimulationDt().in(Seconds));
+ }
+
+ /**
+ *
+ *
+ *
Generates the No-Motion Gyro Drifting.
+ *
+ * This method simulates the minor drifting of the gyro that occurs regardless of whether the robot is moving or
+ * not.
+ *
+ * @return the amount of drifting generated while the robot is not moving
+ */
+ private Rotation2d getNoMotionDrifting() {
+ final double
+ AVERAGE_DRIFTING_1_PERIOD =
+ this.AVERAGE_DRIFTING_IN_30_SECS_MOTIONLESS_DEG
+ / 30
+ * SimulatedArena.getSimulationDt().in(Seconds),
+ driftingInThisPeriod = MapleCommonMath.generateRandomNormal(0, AVERAGE_DRIFTING_1_PERIOD);
+
+ return Rotation2d.fromDegrees(driftingInThisPeriod);
+ }
+}
diff --git a/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/SelfControlledSwerveDriveSimulation.java b/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/SelfControlledSwerveDriveSimulation.java
new file mode 100644
index 0000000..d0d4344
--- /dev/null
+++ b/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/SelfControlledSwerveDriveSimulation.java
@@ -0,0 +1,598 @@
+package swervelib.simulation.ironmaple.simulation.drivesims;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.units.measure.*;
+import edu.wpi.first.wpilibj.Timer;
+import swervelib.simulation.ironmaple.simulation.SimulatedArena;
+import swervelib.simulation.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig;
+import swervelib.simulation.ironmaple.simulation.motorsims.SimulatedMotorController;
+import swervelib.simulation.ironmaple.utils.mathutils.SwerveStateProjection;
+
+import java.util.Arrays;
+
+import static edu.wpi.first.units.Units.*;
+
+/**
+ *
+ *
+ *
An easier way to simulate swerve drive.
+ *
+ * Check Online Documentation
+ *
+ *
This class owns and controls a {@link SwerveDriveSimulation}, running closed loops/open loops on the simulated
+ * motors.
+ *
+ *
It works identically to how the real swerve simulation code.
+ *
+ *
Note: The order for swerve modules is: front-left, front-right, back-left, back-right.
+ */
+public class SelfControlledSwerveDriveSimulation {
+ private final SwerveDriveSimulation swerveDriveSimulation;
+ private final SelfControlledModuleSimulation[] moduleSimulations;
+ private final SwerveDriveKinematics kinematics;
+ private final SwerveDrivePoseEstimator poseEstimator;
+ private final SwerveModuleState[] setPointsOptimized;
+
+ /**
+ *
+ *
+ *
Default Constructor.
+ *
+ * Constructs a simplified swerve simulation with default standard deviations for odometry & vision pose
+ * estimates.
+ *
+ *
Odometry is simulated as high-frequency odometry, assuming a robot period of 0.02 seconds and an odometry
+ * frequency of 250 Hz.
+ *
+ * @param swerveDriveSimulation the {@link SwerveDriveSimulation} to control.
+ */
+ public SelfControlledSwerveDriveSimulation(SwerveDriveSimulation swerveDriveSimulation) {
+ this(swerveDriveSimulation, VecBuilder.fill(0.1, 0.1, 0.1), VecBuilder.fill(0.9, 0.9, 0.9));
+ }
+
+ /**
+ *
+ *
+ *
Constructs an instance with given odometry standard deviations.
+ *
+ * Constructs a simplified swerve simulation with specified standard deviations for odometry & vision pose
+ * estimates.
+ *
+ * @param swerveDriveSimulation the {@link SwerveDriveSimulation} to control.
+ * @param stateStdDevs the standard deviations for odometry encoders.
+ * @param visionMeasurementStdDevs the standard deviations for vision pose estimates.
+ */
+ public SelfControlledSwerveDriveSimulation(
+ SwerveDriveSimulation swerveDriveSimulation,
+ Matrix stateStdDevs,
+ Matrix visionMeasurementStdDevs) {
+ this.swerveDriveSimulation = swerveDriveSimulation;
+ this.moduleSimulations = Arrays.stream(swerveDriveSimulation.getModules())
+ .map(SelfControlledModuleSimulation::new)
+ .toArray(SelfControlledModuleSimulation[]::new);
+
+ this.kinematics = swerveDriveSimulation.kinematics;
+
+ this.poseEstimator = new SwerveDrivePoseEstimator(
+ kinematics,
+ getRawGyroAngle(),
+ getLatestModulePositions(),
+ getActualPoseInSimulationWorld(),
+ stateStdDevs,
+ visionMeasurementStdDevs);
+
+ this.setPointsOptimized = new SwerveModuleState[moduleSimulations.length];
+ Arrays.fill(setPointsOptimized, new SwerveModuleState());
+ }
+
+ /**
+ *
+ *
+ * Periodic Method for Simplified Swerve Sim.
+ *
+ * Call this method in the {@link edu.wpi.first.wpilibj2.command.Subsystem#periodic()} of your swerve subsystem.
+ *
+ *
Updates the odometry by fetching cached inputs.
+ */
+ public void periodic() {
+ final SwerveModulePosition[][] cachedModulePositions = getCachedModulePositions();
+ for (int i = 0; i < SimulatedArena.getSimulationSubTicksIn1Period(); i++)
+ poseEstimator.updateWithTime(
+ Timer.getFPGATimestamp()
+ - SimulatedArena.getSimulationDt().in(Seconds)
+ * (SimulatedArena.getSimulationDt().in(Seconds) - i),
+ swerveDriveSimulation.gyroSimulation.getCachedGyroReadings()[i],
+ cachedModulePositions[i]);
+ }
+
+ /**
+ *
+ *
+ *
Obtains the LATEST module positions measured by the encoders.
+ *
+ * The order for swerve modules is: front-left, front-right, back-left, back-right.
+ *
+ * @return the module positions
+ */
+ public SwerveModulePosition[] getLatestModulePositions() {
+ return Arrays.stream(moduleSimulations)
+ .map(SelfControlledModuleSimulation::getModulePosition)
+ .toArray(SwerveModulePosition[]::new);
+ }
+
+ /**
+ *
+ *
+ *
Obtains the CACHED module positions measured by the encoders.
+ *
+ * This simulates high-frequency odometry.
+ *
+ *
The module positions, or the value of {@link #getLatestModulePositions()} are cached in every simulation
+ * sub-tick.
+ *
+ *
The array is ordered in a [timeStampIndex][moduleIndex] format.
+ *
+ *
The order for swerve modules is: front-left, front-right, back-left, back-right.
+ *
+ * @return the cached module positions
+ */
+ public SwerveModulePosition[][] getCachedModulePositions() {
+ final SwerveModulePosition[][] cachedModulePositions =
+ new SwerveModulePosition[SimulatedArena.getSimulationSubTicksIn1Period()][moduleSimulations.length];
+
+ for (int moduleIndex = 0; moduleIndex < moduleSimulations.length; moduleIndex++) {
+ final Angle[] wheelPosition = moduleSimulations[moduleIndex].instance.getCachedDriveWheelFinalPositions();
+ final Rotation2d[] swerveModuleFacings =
+ moduleSimulations[moduleIndex].instance.getCachedSteerAbsolutePositions();
+ for (int timeStamp = 0; timeStamp < SimulatedArena.getSimulationSubTicksIn1Period(); timeStamp++)
+ cachedModulePositions[timeStamp][moduleIndex] = new SwerveModulePosition(
+ wheelPosition[timeStamp].in(Radians)
+ * moduleSimulations[0].instance.config.WHEEL_RADIUS.in(Meters),
+ swerveModuleFacings[timeStamp]);
+ }
+
+ return cachedModulePositions;
+ }
+
+ /**
+ *
+ *
+ *
Obtains the raw angle of the gyro.
+ *
+ * Note that the simulated gyro also drifts/skids like real gyros, especially if the robot collides.
+ *
+ *
To obtain the facing of the robot retrieved from the odometry, use {@link #getOdometryEstimatedPose()}; to
+ * obtain the actual facing of the robot, use {@link #getActualPoseInSimulationWorld()}.
+ *
+ * @return the raw (uncalibrated) angle of the simulated gyro.
+ * @deprecated This rotation is NOT the actual facing of the robot; it is uncalibrated and is only
+ * used for features like rotation lock.
+ */
+ @Deprecated
+ public Rotation2d getRawGyroAngle() {
+ return swerveDriveSimulation.gyroSimulation.getGyroReading();
+ }
+
+ /**
+ *
+ *
+ *
Obtains the robot pose measured by the odometry.
+ *
+ * Retrieves the pose estimated by the odometry system (and vision, if applicable).
+ *
+ *
This method wraps around {@link SwerveDrivePoseEstimator#getEstimatedPosition()}.
+ *
+ *
This represents the estimated position of the robot.
+ *
+ *
Note that this estimation includes realistic simulations of measurement errors due to skidding and odometry
+ * drift.
+ *
+ *
To obtain the ACTUAL pose of the robot, with no measurement errors, use
+ * {@link #getActualPoseInSimulationWorld()}.
+ */
+ public Pose2d getOdometryEstimatedPose() {
+ return poseEstimator.getEstimatedPosition();
+ }
+
+ /**
+ *
+ *
+ *
Resets the odometry to a specified position.
+ *
+ * This method wraps around {@link SwerveDrivePoseEstimator#resetPosition(Rotation2d, SwerveModulePosition[],
+ * Pose2d)}.
+ *
+ *
It resets the position of the pose estimator to the given pose.
+ *
+ * @param pose The position on the field where the robot is located.
+ */
+ public void resetOdometry(Pose2d pose) {
+ this.poseEstimator.resetPosition(getRawGyroAngle(), getLatestModulePositions(), pose);
+ }
+
+ /**
+ *
+ *
+ *
Adds a vision estimation to the pose estimator.
+ *
+ * This method wraps around {@link SwerveDrivePoseEstimator#addVisionMeasurement(Pose2d, double)}.
+ *
+ *
Adds a vision measurement to the Kalman Filter, correcting the odometry pose estimate while accounting for
+ * measurement noise.
+ *
+ * @param robotPoseMeters The pose of the robot as measured by the vision camera.
+ * @param timeStampSeconds The timestamp of the vision measurement, in seconds.
+ */
+ public void addVisionEstimation(Pose2d robotPoseMeters, double timeStampSeconds) {
+ this.poseEstimator.addVisionMeasurement(robotPoseMeters, timeStampSeconds);
+ }
+
+ /**
+ *
+ *
+ *
Adds a vision estimation to the pose estimator.
+ *
+ * This method wraps around {@link SwerveDrivePoseEstimator#addVisionMeasurement(Pose2d, double, Matrix)}.
+ *
+ *
Adds a vision measurement to the Kalman Filter, correcting the odometry pose estimate while accounting for
+ * measurement noise.
+ *
+ * @param robotPoseMeters The pose of the robot as measured by the vision camera.
+ * @param timeStampSeconds The timestamp of the vision measurement, in seconds.
+ * @param measurementStdDevs Standard deviations of the vision pose measurement (x position in meters, y position in
+ * meters, and heading in radians). Increase these values to reduce the trust in the vision pose measurement.
+ */
+ public void addVisionEstimation(
+ Pose2d robotPoseMeters, double timeStampSeconds, Matrix measurementStdDevs) {
+ this.poseEstimator.addVisionMeasurement(robotPoseMeters, timeStampSeconds, measurementStdDevs);
+ }
+
+ /**
+ *
+ *
+ * Runs chassis speeds on the simulated swerve drive.
+ *
+ * Runs the specified chassis speeds, either robot-centric or field-centric.
+ *
+ * @param chassisSpeeds The speeds to run, in either robot-centric or field-centric coordinates.
+ * @param centerOfRotationMeters The center of rotation. For example, if you set the center of rotation at one
+ * corner of the robot and provide a chassis speed that has only a dtheta component, the robot will rotate
+ * around that corner.
+ * @param fieldCentricDrive Whether to execute field-centric drive with the provided speed.
+ * @param discretizeSpeeds Whether to apply {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} to the provided
+ * speed.
+ */
+ public void runChassisSpeeds(
+ ChassisSpeeds chassisSpeeds,
+ Translation2d centerOfRotationMeters,
+ boolean fieldCentricDrive,
+ boolean discretizeSpeeds) {
+ if (fieldCentricDrive) {
+ chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(
+ chassisSpeeds, getOdometryEstimatedPose().getRotation());
+ }
+ if (discretizeSpeeds) {
+ chassisSpeeds = ChassisSpeeds.discretize(
+ chassisSpeeds,
+ SimulatedArena.getSimulationDt().in(Seconds) * SimulatedArena.getSimulationSubTicksIn1Period());
+ }
+ final SwerveModuleState[] setPoints = kinematics.toSwerveModuleStates(chassisSpeeds, centerOfRotationMeters);
+ runSwerveStates(setPoints);
+ }
+
+ /**
+ *
+ *
+ *
Runs a raw module states on the chassis.
+ *
+ * Runs the specified module states on the modules.
+ *
+ * @param setPoints an array of {@link SwerveModuleState} yielding the requested states
+ */
+ public void runSwerveStates(SwerveModuleState[] setPoints) {
+ for (int i = 0; i < moduleSimulations.length; i++)
+ setPointsOptimized[i] = moduleSimulations[i].optimizeAndRunModuleState(setPoints[i]);
+ }
+
+ /**
+ *
+ *
+ *
Obtain the MEASURED swerve states.
+ *
+ * The order of the swerve modules is: front-left, front-right, back-left, back-right.
+ *
+ * @return The actual measured swerve states of the simulated swerve.
+ */
+ public SwerveModuleState[] getMeasuredStates() {
+ return Arrays.stream(moduleSimulations)
+ .map(SelfControlledModuleSimulation::getMeasuredState)
+ .toArray(SwerveModuleState[]::new);
+ }
+
+ /**
+ *
+ *
+ *
Obtain the optimized SETPOINTS of the swerve.
+ *
+ * The setpoints are calculated using {@link SwerveDriveKinematics#toSwerveModuleStates(ChassisSpeeds)} in the
+ * most recent call to {@link #runChassisSpeeds(ChassisSpeeds, Translation2d, boolean, boolean)}.
+ *
+ *
The setpoints are optimized using {@link SwerveModuleState#optimize(SwerveModuleState, Rotation2d)}.
+ *
+ *
The order of the swerve modules is: front-left, front-right, back-left, back-right.
+ *
+ * @return The optimized setpoints of the swerve, calculated during the last chassis speed run.
+ */
+ public SwerveModuleState[] getSetPointsOptimized() {
+ return setPointsOptimized;
+ }
+
+ /**
+ *
+ *
+ *
Obtain the field-relative chassis speeds measured from the encoders.
+ *
+ * The speeds are measured from the simulated swerve modules.
+ *
+ * @param useGyroForAngularVelocity Whether to use the gyro for a more accurate angular velocity measurement.
+ * @return The measured chassis speeds, field-relative.
+ */
+ public ChassisSpeeds getMeasuredSpeedsFieldRelative(boolean useGyroForAngularVelocity) {
+ ChassisSpeeds speeds = getMeasuredSpeedsRobotRelative(useGyroForAngularVelocity);
+ speeds = ChassisSpeeds.fromRobotRelativeSpeeds(
+ speeds, getOdometryEstimatedPose().getRotation());
+ return speeds;
+ }
+
+ /**
+ *
+ *
+ *
Obtain the robot-relative chassis speeds measured from the encoders.
+ *
+ * The speeds are measured from the simulated swerve modules.
+ *
+ * @param useGyroForAngularVelocity Whether to use the gyro for a more accurate angular velocity measurement.
+ * @return The measured chassis speeds, robot-relative.
+ */
+ public ChassisSpeeds getMeasuredSpeedsRobotRelative(boolean useGyroForAngularVelocity) {
+ final ChassisSpeeds swerveSpeeds = kinematics.toChassisSpeeds(getMeasuredStates());
+ return new ChassisSpeeds(
+ swerveSpeeds.vxMetersPerSecond,
+ swerveSpeeds.vyMetersPerSecond,
+ useGyroForAngularVelocity
+ ? swerveDriveSimulation
+ .gyroSimulation
+ .getMeasuredAngularVelocity()
+ .in(RadiansPerSecond)
+ : swerveSpeeds.omegaRadiansPerSecond);
+ }
+
+ /**
+ *
+ *
+ *
Obtain the {@link SwerveDriveSimulation} object controlled by this simplified swerve simulation.
+ *
+ * @return The swerve drive simulation.
+ */
+ public SwerveDriveSimulation getDriveTrainSimulation() {
+ return this.swerveDriveSimulation;
+ }
+
+ /**
+ *
+ *
+ * Obtain the ACTUAL robot pose.
+ *
+ * Obtains the ACTUAL robot pose with zero measurement error.
+ *
+ *
To obtain the pose calculated by odometry (where the robot thinks it is), use
+ * {@link #getOdometryEstimatedPose()}.
+ */
+ public Pose2d getActualPoseInSimulationWorld() {
+ return swerveDriveSimulation.getSimulatedDriveTrainPose();
+ }
+
+ /**
+ *
+ *
+ *
Get the ACTUAL field-relative chassis speeds of the robot.
+ *
+ * Wraps around {@link SwerveDriveSimulation#getDriveTrainSimulatedChassisSpeedsFieldRelative()}.
+ *
+ * @return the actual chassis speeds in the simulation world, field-relative
+ */
+ public ChassisSpeeds getActualSpeedsFieldRelative() {
+ return this.swerveDriveSimulation.getDriveTrainSimulatedChassisSpeedsFieldRelative();
+ }
+
+ /**
+ *
+ *
+ *
Get the ACTUAL robot-relative chassis speeds of the robot.
+ *
+ * Wraps around {@link SwerveDriveSimulation#getDriveTrainSimulatedChassisSpeedsRobotRelative()}.
+ *
+ * @return the actual chassis speeds in the simulation world, robot-relative
+ */
+ public ChassisSpeeds getActualSpeedsRobotRelative() {
+ return this.swerveDriveSimulation.getDriveTrainSimulatedChassisSpeedsRobotRelative();
+ }
+
+ /**
+ *
+ *
+ *
Teleport the robot to a specified location on the simulated field.
+ *
+ * This method moves the drivetrain instantly to the specified location on the field, bypassing any obstacles in
+ * its path.
+ *
+ *
Wraps around {@link SwerveDriveSimulation#setSimulationWorldPose(Pose2d)}.
+ *
+ * @param robotPose the pose of the robot to teleport to
+ */
+ public void setSimulationWorldPose(Pose2d robotPose) {
+ this.swerveDriveSimulation.setSimulationWorldPose(robotPose);
+ }
+
+ public SelfControlledSwerveDriveSimulation withSteerPID(PIDController steerController) {
+ for (SelfControlledModuleSimulation moduleSimulation : moduleSimulations)
+ moduleSimulation.withSteerPID(
+ new PIDController(steerController.getP(), steerController.getI(), steerController.getD()));
+ return this;
+ }
+
+ public SelfControlledSwerveDriveSimulation withCurrentLimits(Current driveCurrentLimit, Current steerCurrentLimit) {
+ for (SelfControlledModuleSimulation moduleSimulation : moduleSimulations)
+ moduleSimulation.withCurrentLimits(driveCurrentLimit, steerCurrentLimit);
+
+ return this;
+ }
+
+ public static class SelfControlledModuleSimulation {
+ public final SwerveModuleSimulation instance;
+ private Current driveCurrentLimit;
+
+ private PIDController steerController;
+
+ private final SimulatedMotorController.GenericMotorController driveMotor;
+ private final SimulatedMotorController.GenericMotorController steerMotor;
+
+ public SelfControlledModuleSimulation(SwerveModuleSimulation moduleSimulation) {
+ this.instance = moduleSimulation;
+ steerController = new PIDController(5.0, 0, 0);
+
+ this.driveMotor = this.instance.useGenericMotorControllerForDrive();
+ this.driveMotor.withCurrentLimit(this.driveCurrentLimit = Amps.of(60));
+ this.steerMotor = this.instance.useGenericControllerForSteer();
+ }
+
+ public SelfControlledModuleSimulation withSteerPID(PIDController steerController) {
+ this.steerController = steerController;
+ steerController.enableContinuousInput(-Math.PI, Math.PI);
+ return this;
+ }
+
+ public SelfControlledModuleSimulation withCurrentLimits(Current driveCurrentLimit, Current steerCurrentLimit) {
+ this.driveMotor.withCurrentLimit(this.driveCurrentLimit = driveCurrentLimit);
+ this.steerMotor.withCurrentLimit(steerCurrentLimit);
+ steerController.enableContinuousInput(-Math.PI, Math.PI);
+ return this;
+ }
+
+ /**
+ *
+ *
+ *
Runs the control loops for swerve states on a simulated module.
+ *
+ * Optimizes the set-point using {@link SwerveModuleState#optimize(SwerveModuleState, Rotation2d)}.
+ *
+ *
Executes a closed-loop control on the swerve module.
+ *
+ * @param setPoint the desired state to optimize and apply
+ * @return the optimized swerve module state after control execution
+ */
+ public SwerveModuleState optimizeAndRunModuleState(SwerveModuleState setPoint) {
+ setPoint.optimize(instance.getSteerAbsoluteFacing());
+ runModuleState(setPoint);
+ return setPoint;
+ }
+
+ public void runModuleState(SwerveModuleState setPoint) {
+ final double
+ cosProjectedSpeedMPS = SwerveStateProjection.project(setPoint, instance.getSteerAbsoluteFacing()),
+ driveWheelVelocitySetPointRadPerSec =
+ cosProjectedSpeedMPS / instance.config.WHEEL_RADIUS.in(Meters);
+
+ driveMotor.requestVoltage(instance.config.driveMotorConfigs.calculateVoltage(
+ Amps.of(0), RadiansPerSecond.of(driveWheelVelocitySetPointRadPerSec)));
+
+ steerMotor.requestVoltage(Volts.of(steerController.calculate(
+ instance.getSteerAbsoluteFacing().getRadians(), setPoint.angle.getRadians())));
+ }
+
+ public void runDriveMotorCharacterization(Rotation2d desiredModuleFacing, double volts) {
+ driveMotor.requestVoltage(Volts.of(volts));
+
+ steerMotor.requestVoltage(Volts.of(steerController.calculate(
+ instance.getSteerAbsoluteFacing().getRadians(), desiredModuleFacing.getRadians())));
+ }
+
+ public void runSteerMotorCharacterization(double volts) {
+ driveMotor.requestVoltage(Volts.zero());
+
+ steerMotor.requestVoltage(Volts.of(volts));
+ }
+
+ public SwerveModuleState getMeasuredState() {
+ return instance.getCurrentState();
+ }
+
+ public SwerveModulePosition getModulePosition() {
+ return new SwerveModulePosition(
+ instance.getDriveWheelFinalPosition().in(Radians) * instance.config.WHEEL_RADIUS.in(Meters),
+ instance.getSteerAbsoluteFacing());
+ }
+ }
+
+ /**
+ * @see SwerveDriveSimulation#maxLinearVelocity()
+ */
+ public LinearVelocity maxLinearVelocity() {
+ return swerveDriveSimulation.maxLinearVelocity();
+ }
+
+ /**
+ * @see SwerveDriveSimulation#maxLinearAcceleration(Current)
+ */
+ public LinearAcceleration maxLinearAcceleration() {
+ return swerveDriveSimulation.maxLinearAcceleration(moduleSimulations[0].driveCurrentLimit);
+ }
+
+ /**
+ * @see DriveTrainSimulationConfig#trackWidthY()
+ */
+ public Distance trackWidthY() {
+ return swerveDriveSimulation.config.trackWidthY();
+ }
+
+ /**
+ * @see DriveTrainSimulationConfig#trackLengthX()
+ */
+ public Distance trackLengthX() {
+ return swerveDriveSimulation.config.trackLengthX();
+ }
+
+ /**
+ * @see SwerveDriveSimulation#driveBaseRadius()
+ */
+ public Distance driveBaseRadius() {
+ return swerveDriveSimulation.config.driveBaseRadius();
+ }
+
+ /**
+ * @see SwerveDriveSimulation#maxAngularVelocity()
+ */
+ public AngularVelocity maxAngularVelocity() {
+ return RadiansPerSecond.of(
+ maxLinearVelocity().in(MetersPerSecond) / driveBaseRadius().in(Meters));
+ }
+
+ /**
+ * @see SwerveDriveSimulation#maxAngularAcceleration(Current)
+ */
+ public AngularAcceleration maxAngularAcceleration() {
+ return swerveDriveSimulation.maxAngularAcceleration(moduleSimulations[0].driveCurrentLimit);
+ }
+}
diff --git a/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/SwerveDriveSimulation.java b/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/SwerveDriveSimulation.java
new file mode 100644
index 0000000..2ee420a
--- /dev/null
+++ b/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/SwerveDriveSimulation.java
@@ -0,0 +1,368 @@
+package swervelib.simulation.ironmaple.simulation.drivesims;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.units.measure.*;
+import org.dyn4j.geometry.Vector2;
+import swervelib.simulation.ironmaple.simulation.SimulatedArena;
+import swervelib.simulation.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig;
+import swervelib.simulation.ironmaple.utils.mathutils.GeometryConvertor;
+import swervelib.simulation.ironmaple.utils.mathutils.MapleCommonMath;
+
+import java.util.Arrays;
+import java.util.function.Supplier;
+
+import static edu.wpi.first.units.Units.*;
+
+/**
+ *
+ *
+ *
Simulates a Swerve Drivetrain.
+ *
+ * Check Online
+ * Documentation
+ *
+ *
1. Purpose
+ *
+ * This class simulates a swerve drivetrain composed of more than two {@link SwerveModuleSimulation} modules.
+ *
+ *
It provides a realistic modeling of drivetrain physics, replicating wheel grip and motor propulsion for an actual
+ * swerve drive.
+ *
+ *
2. Simulation Dynamics
+ *
+ *
+ * - 1. Propelling forces generated by the drive motors, computed by
+ * {@link SwerveModuleSimulation#updateSimulationSubTickGetModuleForce(Vector2, Rotation2d, double)}.
+ *
- 2. Friction forces generated by the wheels that "pull" the robot from its current ground velocity to the module
+ * velocities, both translational and rotational.
+ *
- 3. Centripetal forces generated by the steering when the drivetrain makes a turn.
+ *
+ *
+ * 3. Odometry Simulation
+ *
+ * To simulate odometry, follow these steps:
+ *
+ *
+ * - Obtain the {@link SwerveModuleSimulation} instances through {@link #getModules()}.
+ *
- Create an IO
+ * Implementation that wraps around {@link SwerveModuleSimulation} to retrieve encoder readings.
+ *
- Update a {@link edu.wpi.first.math.estimator.SwerveDrivePoseEstimator} using the encoder readings, similar to
+ * how you would on a real robot.
+ *
+ *
+ * Refer to the ModuleIOSim.java
+ * example project for more details.
+ *
+ *
Vision Simulation
+ *
+ * You can obtain the real robot pose from {@link #getSimulatedDriveTrainPose()} and feed it to the PhotonVision
+ * simulation to simulate vision.
+ */
+public class SwerveDriveSimulation extends AbstractDriveTrainSimulation {
+ private final SwerveModuleSimulation[] moduleSimulations;
+ protected final GyroSimulation gyroSimulation;
+ protected final Translation2d[] moduleTranslations;
+ protected final SwerveDriveKinematics kinematics;
+ private final double gravityForceOnEachModule;
+
+ /**
+ *
+ *
+ *
Creates a Swerve Drive Simulation.
+ *
+ * This constructor initializes a swerve drive simulation with the given robot mass, bumper dimensions, module
+ * simulations, module translations, gyro simulation, and initial pose on the field.
+ *
+ * @param config a {@link DriveTrainSimulationConfig} instance containing the configurations of * this drivetrain
+ * @param initialPoseOnField the initial pose of the drivetrain in the simulation world, represented as a
+ * {@link Pose2d}
+ */
+ public SwerveDriveSimulation(DriveTrainSimulationConfig config, Pose2d initialPoseOnField) {
+ super(config, initialPoseOnField);
+ this.moduleTranslations = config.moduleTranslations;
+ this.moduleSimulations = Arrays.stream(config.swerveModuleSimulationFactories)
+ .map(Supplier::get)
+ .toArray(SwerveModuleSimulation[]::new);
+ this.gyroSimulation = config.gyroSimulationFactory.get();
+
+ super.setLinearDamping(1.4);
+ super.setAngularDamping(1.4);
+ this.kinematics = new SwerveDriveKinematics(moduleTranslations);
+
+ this.gravityForceOnEachModule = config.robotMass.in(Kilograms) * 9.8 / moduleSimulations.length;
+ }
+
+ /**
+ *
+ *
+ *
Updates the Swerve Drive Simulation.
+ *
+ * This method performs the following actions during each sub-tick of the simulation:
+ *
+ *
+ * - Applies the translational friction force to the physics engine.
+ *
- Applies the rotational friction torque to the physics engine.
+ *
- Updates the simulation of each swerve module.
+ *
- Applies the propelling forces of the modules to the physics engine.
+ *
- Updates the gyro simulation of the drivetrain.
+ *
+ */
+ @Override
+ public void simulationSubTick() {
+ simulateChassisFrictionForce();
+
+ simulateChassisFrictionTorque();
+
+ simulateModulePropellingForces();
+
+ gyroSimulation.updateSimulationSubTick(super.getAngularVelocity());
+ }
+
+ private Translation2d previousModuleSpeedsFieldRelative = new Translation2d();
+
+ /**
+ *
+ *
+ * Simulates the Translational Friction Force and Applies It to the Physics Engine.
+ *
+ * This method simulates the translational friction forces acting on the robot and applies them to the physics
+ * engine. There are two components of the friction forces:
+ *
+ *
+ * - A portion of the friction force pushes the robot from its current ground speeds
+ * ({@link #getDriveTrainSimulatedChassisSpeedsRobotRelative()}) toward its current module speeds
+ * ({@link #getModuleSpeeds()}).
+ *
- Another portion of the friction force is the centripetal force, which occurs when the chassis changes its
+ * direction of movement.
+ *
+ *
+ * The total friction force should not exceed the tire's grip limit.
+ */
+ private void simulateChassisFrictionForce() {
+ final ChassisSpeeds moduleSpeeds = getModuleSpeeds();
+
+ /* The friction force that tries to bring the chassis from floor speeds to module speeds */
+ final ChassisSpeeds differenceBetweenFloorSpeedAndModuleSpeedsRobotRelative =
+ moduleSpeeds.minus(getDriveTrainSimulatedChassisSpeedsRobotRelative());
+ final Translation2d floorAndModuleSpeedsDiffFieldRelative = new Translation2d(
+ differenceBetweenFloorSpeedAndModuleSpeedsRobotRelative.vxMetersPerSecond,
+ differenceBetweenFloorSpeedAndModuleSpeedsRobotRelative.vyMetersPerSecond)
+ .rotateBy(getSimulatedDriveTrainPose().getRotation());
+ final double FRICTION_FORCE_GAIN = 3.0,
+ totalGrippingForce =
+ moduleSimulations[0].config.getGrippingForceNewtons(gravityForceOnEachModule)
+ * moduleSimulations.length;
+ final Vector2 speedsDifferenceFrictionForce = Vector2.create(
+ Math.min(
+ FRICTION_FORCE_GAIN * totalGrippingForce * floorAndModuleSpeedsDiffFieldRelative.getNorm(),
+ totalGrippingForce),
+ MapleCommonMath.getAngle(floorAndModuleSpeedsDiffFieldRelative).getRadians());
+
+ /* the centripetal friction force during turning */
+ final ChassisSpeeds moduleSpeedsFieldRelative = ChassisSpeeds.fromRobotRelativeSpeeds(
+ moduleSpeeds, getSimulatedDriveTrainPose().getRotation());
+ final Rotation2d dTheta = MapleCommonMath.getAngle(
+ GeometryConvertor.getChassisSpeedsTranslationalComponent(moduleSpeedsFieldRelative))
+ .minus(MapleCommonMath.getAngle(previousModuleSpeedsFieldRelative));
+
+ final double orbitalAngularVelocity =
+ dTheta.getRadians() / SimulatedArena.getSimulationDt().in(Seconds);
+ final Rotation2d centripetalForceDirection =
+ MapleCommonMath.getAngle(previousModuleSpeedsFieldRelative).plus(Rotation2d.fromDegrees(90));
+ final Vector2 centripetalFrictionForce = Vector2.create(
+ previousModuleSpeedsFieldRelative.getNorm() * orbitalAngularVelocity * config.robotMass.in(Kilograms),
+ centripetalForceDirection.getRadians());
+ previousModuleSpeedsFieldRelative =
+ GeometryConvertor.getChassisSpeedsTranslationalComponent(moduleSpeedsFieldRelative);
+
+ /* apply force to physics engine */
+ final Vector2
+ totalFrictionForceUnlimited = centripetalFrictionForce.copy().add(speedsDifferenceFrictionForce),
+ totalFrictionForce =
+ Vector2.create(
+ Math.min(totalGrippingForce, totalFrictionForceUnlimited.getMagnitude()),
+ totalFrictionForceUnlimited.getDirection());
+ super.applyForce(totalFrictionForce);
+ }
+
+ /**
+ *
+ *
+ *
Simulates the Rotational Friction Torque and Applies It to the Physics Engine.
+ *
+ * This method simulates the rotational friction torque acting on the robot and applies them to the physics
+ * engine.
+ *
+ *
The friction torque pushes the robot from its current ground angular velocity
+ * ({@link #getDriveTrainSimulatedChassisSpeedsRobotRelative()}) toward its current modules' angular velocity
+ * ({@link #getModuleSpeeds()}).
+ */
+ private void simulateChassisFrictionTorque() {
+ final double
+ desiredRotationalMotionPercent =
+ Math.abs(getDesiredSpeed().omegaRadiansPerSecond
+ / maxAngularVelocity().in(RadiansPerSecond)),
+ actualRotationalMotionPercent =
+ Math.abs(getAngularVelocity() / maxAngularVelocity().in(RadiansPerSecond)),
+ differenceBetweenFloorSpeedAndModuleSpeed =
+ getModuleSpeeds().omegaRadiansPerSecond - getAngularVelocity(),
+ grippingTorqueMagnitude =
+ moduleSimulations[0].config.getGrippingForceNewtons(gravityForceOnEachModule)
+ * moduleTranslations[0].getNorm()
+ * moduleSimulations.length,
+ FRICTION_TORQUE_GAIN = 1;
+
+ if (actualRotationalMotionPercent < 0.01 && desiredRotationalMotionPercent < 0.02) super.setAngularVelocity(0);
+ else
+ super.applyTorque(Math.copySign(
+ Math.min(
+ FRICTION_TORQUE_GAIN
+ * grippingTorqueMagnitude
+ * Math.abs(differenceBetweenFloorSpeedAndModuleSpeed),
+ grippingTorqueMagnitude),
+ differenceBetweenFloorSpeedAndModuleSpeed));
+ }
+
+ /**
+ *
+ *
+ *
Simulates the Translational Friction Force and Applies It to the Physics Engine.
+ *
+ * This method simulates the translational friction forces acting on the robot and applies them to the physics
+ * engine. There are two components of the friction forces:
+ *
+ *
+ * - A portion of the friction force pushes the robot from its current ground speeds
+ * ({@link #getDriveTrainSimulatedChassisSpeedsRobotRelative()}) toward its current module speeds
+ * ({@link #getModuleSpeeds()}).
+ *
- Another portion of the friction force is the centripetal force, which occurs when the chassis changes its
+ * direction of movement.
+ *
+ *
+ * The total friction force should not exceed the tire's grip limit.
+ */
+ private void simulateModulePropellingForces() {
+ final int arrayLength = moduleSimulations.length; // Call it once
+ final Rotation2d driveRotation = getSimulatedDriveTrainPose().getRotation(); // Call it once per 4 modules.
+ for (int i = 0; i < arrayLength; i++) {
+ final Vector2 moduleWorldPosition = getWorldPoint(GeometryConvertor.toDyn4jVector2(moduleTranslations[i]));
+ final Vector2 moduleForce = moduleSimulations[i].updateSimulationSubTickGetModuleForce(
+ super.getLinearVelocity(moduleWorldPosition), driveRotation, gravityForceOnEachModule);
+ super.applyForce(moduleForce, moduleWorldPosition);
+ }
+ }
+
+ /**
+ *
+ *
+ *
Obtains the Chassis Speeds the Modules Are Attempting to Achieve.
+ *
+ * This method returns the desired chassis speeds that the modules are trying to reach. If the robot maintains
+ * the current driving voltage and steering position for a long enough period, it will achieve these speeds.
+ *
+ * @return the desired chassis speeds, robot-relative
+ */
+ private ChassisSpeeds getDesiredSpeed() {
+ return kinematics.toChassisSpeeds(Arrays.stream(moduleSimulations)
+ .map((SwerveModuleSimulation::getFreeSpinState))
+ .toArray(SwerveModuleState[]::new));
+ }
+
+ /**
+ *
+ *
+ *
Obtains the Current Chassis Speeds of the Modules.
+ *
+ * This method estimates the chassis speeds of the robot based on the swerve states of the modules.
+ *
+ *
Note: These speeds might not represent the actual floor speeds due to potential skidding.
+ *
+ * @return the module speeds, robot-relative
+ */
+ private ChassisSpeeds getModuleSpeeds() {
+ return kinematics.toChassisSpeeds(Arrays.stream(moduleSimulations)
+ .map((SwerveModuleSimulation::getCurrentState))
+ .toArray(SwerveModuleState[]::new));
+ }
+
+ /**
+ *
+ *
+ *
Obtains the maximum achievable linear velocity of the chassis.
+ *
+ * @return the maximum linear velocity
+ * @see swervelib.simulation.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig#maximumGroundSpeed()
+ */
+ public LinearVelocity maxLinearVelocity() {
+ return moduleSimulations[0].config.maximumGroundSpeed();
+ }
+
+ /**
+ *
+ *
+ * Obtains the maximum achievable linear acceleration of the chassis.
+ *
+ * @return the maximum linear acceleration
+ * @see swervelib.simulation.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig#maxAcceleration(Mass, int, Current)
+ */
+ public LinearAcceleration maxLinearAcceleration(Current statorCurrentLimit) {
+ return moduleSimulations[0].config.maxAcceleration(
+ config.robotMass, moduleSimulations.length, statorCurrentLimit);
+ }
+
+ /**
+ *
+ *
+ * Obtains the drive base radius of the swerve drive.
+ *
+ * @return the drive base radius.
+ */
+ public Distance driveBaseRadius() {
+ return config.driveBaseRadius();
+ }
+
+ /**
+ *
+ *
+ * Obtains the maximum achievable angular velocity of the chassis.
+ *
+ * @return the maximum angular velocity
+ */
+ public AngularVelocity maxAngularVelocity() {
+ return RadiansPerSecond.of(maxLinearVelocity().in(MetersPerSecond)
+ / config.driveBaseRadius().in(Meters));
+ }
+
+ /**
+ *
+ *
+ * Obtains the maximum achievable angular acceleration of the chassis.
+ *
+ * @return the maximum angular acceleration
+ */
+ public AngularAcceleration maxAngularAcceleration(Current statorCurrentLimit) {
+ return RadiansPerSecondPerSecond.of(moduleSimulations[0]
+ .config
+ .getTheoreticalPropellingForcePerModule(
+ config.robotMass, moduleSimulations.length, statorCurrentLimit)
+ .in(Newtons)
+ * moduleTranslations[0].getNorm()
+ * moduleSimulations.length
+ / super.getMass().getInertia());
+ }
+
+ public SwerveModuleSimulation[] getModules() {
+ return moduleSimulations;
+ }
+
+ public GyroSimulation getGyroSimulation() {
+ return this.gyroSimulation;
+ }
+}
diff --git a/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/SwerveModuleSimulation.java b/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/SwerveModuleSimulation.java
new file mode 100644
index 0000000..2c8201d
--- /dev/null
+++ b/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/SwerveModuleSimulation.java
@@ -0,0 +1,544 @@
+package swervelib.simulation.ironmaple.simulation.drivesims;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.units.measure.*;
+import org.dyn4j.geometry.Vector2;
+import swervelib.simulation.ironmaple.simulation.SimulatedArena;
+import swervelib.simulation.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig;
+import swervelib.simulation.ironmaple.simulation.motorsims.MapleMotorSim;
+import swervelib.simulation.ironmaple.simulation.motorsims.SimMotorConfigs;
+import swervelib.simulation.ironmaple.simulation.motorsims.SimulatedBattery;
+import swervelib.simulation.ironmaple.simulation.motorsims.SimulatedMotorController;
+
+import java.util.Queue;
+import java.util.concurrent.ConcurrentLinkedQueue;
+
+import static edu.wpi.first.units.Units.*;
+
+/**
+ *
+ *
+ * Simulation for a Single Swerve Module.
+ *
+ * Check Online
+ * Documentation
+ *
+ *
This class provides a simulation for a single swerve module in the {@link SwerveDriveSimulation}.
+ *
+ *
1. Purpose
+ *
+ * This class serves as the bridge between your code and the physics engine.
+ *
+ *
You will apply voltage outputs to the drive/steer motor of the module and obtain their encoder readings in your
+ * code, just as how you deal with your physical motors.
+ *
+ *
2. Perspectives
+ *
+ *
+ * - Simulates the steering mechanism using a custom brushless motor simulator.
+ *
- Simulates the propelling force generated by the driving motor, with a current limit.
+ *
- Simulates encoder readings, which can be used to simulate a {@link SwerveDriveOdometry}.
+ *
+ *
+ * 3. Simulating Odometry
+ *
+ *
+ * - Retrieve the encoder readings from {@link #getDriveEncoderUnGearedPosition()}} and
+ * {@link #getSteerAbsoluteFacing()}.
+ *
- Use {@link SwerveDriveOdometry} to estimate the pose of your robot.
+ *
- 250Hz
+ * Odometry is supported. You can retrive cached encoder readings from every sub-tick through
+ * {@link #getCachedDriveEncoderUnGearedPositions()} and {@link #getCachedSteerAbsolutePositions()}.
+ *
+ *
+ * An example of how to simulate odometry using this class is the ModuleIOSim.java
+ * from the Advanced Swerve Drive with maple-sim example.
+ */
+public class SwerveModuleSimulation {
+ public final SwerveModuleSimulationConfig config;
+
+ private final MapleMotorSim steerMotorSim;
+
+ private Voltage driveMotorAppliedVoltage = Volts.zero();
+ private Current driveMotorStatorCurrent = Amps.zero();
+ private Angle driveWheelFinalPosition = Radians.zero();
+ private AngularVelocity driveWheelFinalSpeed = RadiansPerSecond.zero();
+
+ private SimulatedMotorController driveMotorController;
+
+ private final Angle steerRelativeEncoderOffSet = Radians.of((Math.random() - 0.5) * 30);
+ private final Queue driveWheelFinalPositionCache;
+ private final Queue steerAbsolutePositionCache;
+
+ /**
+ *
+ *
+ * Constructs a Swerve Module Simulation.
+ *
+ * If you are using {@link SimulatedArena#overrideSimulationTimings(Time, int)} to use custom timings, you must
+ * call the method before constructing any swerve module simulations using this constructor.
+ *
+ * @param config the configuration
+ */
+ public SwerveModuleSimulation(SwerveModuleSimulationConfig config) {
+ this.config = config;
+
+ SimulatedBattery.addElectricalAppliances(this::getDriveMotorSupplyCurrent);
+ this.steerMotorSim = new MapleMotorSim(config.steerMotorConfigs);
+
+ this.driveWheelFinalPositionCache = new ConcurrentLinkedQueue<>();
+ for (int i = 0; i < SimulatedArena.getSimulationSubTicksIn1Period(); i++)
+ driveWheelFinalPositionCache.offer(driveWheelFinalPosition);
+ this.steerAbsolutePositionCache = new ConcurrentLinkedQueue<>();
+ for (int i = 0; i < SimulatedArena.getSimulationSubTicksIn1Period(); i++)
+ steerAbsolutePositionCache.offer(getSteerAbsoluteFacing());
+
+ this.driveMotorController = new SimulatedMotorController.GenericMotorController(config.driveMotorConfigs.motor);
+ this.steerMotorSim.useSimpleDCMotorController();
+ }
+
+ public SimMotorConfigs getDriveMotorConfigs() {
+ return config.driveMotorConfigs;
+ }
+
+ public SimMotorConfigs getSteerMotorConfigs() {
+ return steerMotorSim.getConfigs();
+ }
+
+ /**
+ *
+ *
+ *
Sets the motor controller for the drive motor.
+ *
+ * The configured controller runs control loop on the motor.
+ *
+ * @param driveMotorController the motor controller to control the drive motor
+ */
+ public T useDriveMotorController(T driveMotorController) {
+ this.driveMotorController = driveMotorController;
+ return driveMotorController;
+ }
+
+ public SimulatedMotorController.GenericMotorController useGenericMotorControllerForDrive() {
+ return useDriveMotorController(
+ new SimulatedMotorController.GenericMotorController(config.driveMotorConfigs.motor));
+ }
+
+ /**
+ *
+ *
+ * Requests the Steering Motor to Run at a Specified Output.
+ *
+ * Think of it as the requestOutput() of your physical steering motor.
+ *
+ * @param steerMotorController the motor controller to control the steer motor
+ */
+ public T useSteerMotorController(T steerMotorController) {
+ return this.steerMotorSim.useMotorController(steerMotorController);
+ }
+
+ public SimulatedMotorController.GenericMotorController useGenericControllerForSteer() {
+ return this.steerMotorSim.useSimpleDCMotorController();
+ }
+
+ /**
+ *
+ *
+ * Updates the Simulation for This Module.
+ *
+ * Note: Friction forces are not simulated in this method.
+ *
+ * @param moduleCurrentGroundVelocityWorldRelative the current ground velocity of the module, relative to the world
+ * @param robotFacing the absolute facing of the robot, relative to the world
+ * @param gravityForceOnModuleNewtons the gravitational force acting on this module, in newtons
+ * @return the propelling force generated by the module, as a {@link Vector2} object
+ */
+ public Vector2 updateSimulationSubTickGetModuleForce(
+ Vector2 moduleCurrentGroundVelocityWorldRelative,
+ Rotation2d robotFacing,
+ double gravityForceOnModuleNewtons) {
+ /* Step1: Update the steer mechanism simulation */
+ steerMotorSim.update(SimulatedArena.getSimulationDt());
+
+ /* Step2: Simulate the amount of propelling force generated by the module. */
+ final double grippingForceNewtons = config.getGrippingForceNewtons(gravityForceOnModuleNewtons);
+ final Rotation2d moduleWorldFacing = this.getSteerAbsoluteFacing().plus(robotFacing);
+ final Vector2 propellingForce =
+ getPropellingForce(grippingForceNewtons, moduleWorldFacing, moduleCurrentGroundVelocityWorldRelative);
+
+ /* Step3: Updates and caches the encoder readings for odometry simulation. */
+ updateEncoderCaches();
+
+ return propellingForce;
+ }
+
+ /**
+ *
+ *
+ *
Calculates the amount of propelling force that the module generates.
+ *
+ * For most of the time, that propelling force is directly applied to the drivetrain. And the drive wheel runs as
+ * fast as the ground velocity
+ *
+ *
However, if the propelling force exceeds the gripping, only the max gripping force is applied. The rest of the
+ * propelling force will cause the wheel to start skidding and make the odometry inaccurate.
+ *
+ * @param grippingForceNewtons the amount of gripping force that wheel can generate, in newtons
+ * @param moduleWorldFacing the current world facing of the module
+ * @param moduleCurrentGroundVelocity the current ground velocity of the module, world-reference
+ * @return a vector representing the propelling force that the module generates, world-reference
+ */
+ private Vector2 getPropellingForce(
+ double grippingForceNewtons, Rotation2d moduleWorldFacing, Vector2 moduleCurrentGroundVelocity) {
+ final double driveWheelTorque = getDriveWheelTorque();
+ final double wheelRadius = config.WHEEL_RADIUS.in(Meters); // Call it once
+ double propellingForceNewtons = driveWheelTorque / wheelRadius;
+ final boolean skidding = Math.abs(propellingForceNewtons) > grippingForceNewtons;
+ if (skidding) propellingForceNewtons = Math.copySign(grippingForceNewtons, propellingForceNewtons);
+
+ final double moduleAngleRadians = moduleWorldFacing.getRadians(); // Call it once
+ final double floorVelocityProjectionOnWheelDirectionMPS = moduleCurrentGroundVelocity.getMagnitude()
+ * Math.cos(moduleCurrentGroundVelocity.getAngleBetween(moduleAngleRadians));
+
+ // if the chassis is tightly gripped on floor, the floor velocity is projected to the wheel
+ this.driveWheelFinalSpeed = RadiansPerSecond.of(floorVelocityProjectionOnWheelDirectionMPS / wheelRadius);
+
+ // if the module is skidding
+ if (skidding) {
+ final AngularVelocity skiddingEquilibriumWheelSpeed = config.driveMotorConfigs.calculateMechanismVelocity(
+ config.driveMotorConfigs.calculateCurrent(NewtonMeters.of(propellingForceNewtons * wheelRadius)),
+ driveMotorAppliedVoltage);
+ this.driveWheelFinalSpeed = driveWheelFinalSpeed.times(0.5).plus(skiddingEquilibriumWheelSpeed.times(0.5));
+ }
+
+ return Vector2.create(propellingForceNewtons, moduleAngleRadians);
+ }
+
+ /**
+ *
+ *
+ *
Calculates the amount of torque that the drive motor can generate on the wheel.
+ *
+ * @return the amount of torque on the wheel by the drive motor, in Newton * Meters
+ */
+ private double getDriveWheelTorque() {
+ driveMotorAppliedVoltage = driveMotorController.updateControlSignal(
+ driveWheelFinalPosition,
+ driveWheelFinalSpeed,
+ getDriveEncoderUnGearedPosition(),
+ getDriveEncoderUnGearedSpeed());
+
+ driveMotorAppliedVoltage = SimulatedBattery.clamp(driveMotorAppliedVoltage);
+
+ /* calculate the stator current */
+ driveMotorStatorCurrent =
+ config.driveMotorConfigs.calculateCurrent(driveWheelFinalSpeed, driveMotorAppliedVoltage);
+
+ /* calculate the torque generated */
+ Torque driveWheelTorque = config.driveMotorConfigs.calculateTorque(driveMotorStatorCurrent);
+
+ /* calculates the torque if you included losses from friction */
+ Torque driveWheelTorqueWithFriction = NewtonMeters.of(MathUtil.applyDeadband(
+ driveWheelTorque.in(NewtonMeters),
+ config.driveMotorConfigs.friction.in(NewtonMeters),
+ Double.POSITIVE_INFINITY));
+ return driveWheelTorqueWithFriction.in(NewtonMeters);
+ }
+
+ /**
+ * @return the current module state of this simulation module
+ */
+ public SwerveModuleState getCurrentState() {
+ return new SwerveModuleState(
+ MetersPerSecond.of(getDriveWheelFinalSpeed().in(RadiansPerSecond) * config.WHEEL_RADIUS.in(Meters)),
+ getSteerAbsoluteFacing());
+ }
+
+ /**
+ *
+ *
+ * Obtains the "free spin" state of the module
+ *
+ * The "free spin" state of a simulated module refers to its state after spinning freely for a long time under
+ * the current input voltage
+ *
+ * @return the free spinning module state
+ */
+ protected SwerveModuleState getFreeSpinState() {
+ return new SwerveModuleState(
+ config.driveMotorConfigs
+ .calculateMechanismVelocity(
+ config.driveMotorConfigs.calculateCurrent(config.driveMotorConfigs.friction),
+ driveMotorAppliedVoltage)
+ .in(RadiansPerSecond)
+ * config.WHEEL_RADIUS.in(Meters),
+ getSteerAbsoluteFacing());
+ }
+
+ /**
+ *
+ *
+ *
Cache the encoder values for high-frequency odometry.
+ *
+ * An internal method to cache the encoder values to their queues.
+ */
+ private void updateEncoderCaches() {
+ /* Increment of drive wheel position */
+ this.driveWheelFinalPosition =
+ this.driveWheelFinalPosition.plus(this.driveWheelFinalSpeed.times(SimulatedArena.getSimulationDt()));
+
+ /* cache sensor readings to queue for high-frequency odometry */
+ this.steerAbsolutePositionCache.poll();
+ this.steerAbsolutePositionCache.offer(getSteerAbsoluteFacing());
+
+ this.driveWheelFinalPositionCache.poll();
+ this.driveWheelFinalPositionCache.offer(driveWheelFinalPosition);
+ }
+
+ /**
+ *
+ *
+ *
Obtains the Actual Output Voltage of the Drive Motor.
+ *
+ * @return the actual output voltage of the drive motor
+ */
+ public Voltage getDriveMotorAppliedVoltage() {
+ return driveMotorAppliedVoltage;
+ }
+
+ /**
+ *
+ *
+ * Obtains the Actual Output Voltage of the Steering Motor.
+ *
+ * @return the actual output voltage of the steering motor
+ * @see MapleMotorSim#getAppliedVoltage()
+ */
+ public Voltage getSteerMotorAppliedVoltage() {
+ return steerMotorSim.getAppliedVoltage();
+ }
+
+ /**
+ *
+ *
+ * Obtains the Amount of Current Supplied to the Drive Motor.
+ *
+ * @return the current supplied to the drive motor
+ */
+ public Current getDriveMotorSupplyCurrent() {
+ return getDriveMotorStatorCurrent().times(driveMotorAppliedVoltage.div(SimulatedBattery.getBatteryVoltage()));
+ }
+
+ /**
+ *
+ *
+ * Obtains the Stator current the Drive Motor.
+ *
+ * @return the stator current of the drive motor
+ */
+ public Current getDriveMotorStatorCurrent() {
+ return driveMotorStatorCurrent;
+ }
+
+ /**
+ *
+ *
+ * Obtains the Amount of Current Supplied to the Steer Motor.
+ *
+ * @return the current supplied to the steer motor
+ * @see MapleMotorSim#getSupplyCurrent()
+ */
+ public Current getSteerMotorSupplyCurrent() {
+ return steerMotorSim.getSupplyCurrent();
+ }
+
+ /**
+ *
+ *
+ * Obtains the Stator current the Steer Motor.
+ *
+ * @return the stator current of the drive motor
+ * @see MapleMotorSim#getSupplyCurrent()
+ */
+ public Current getSteerMotorStatorCurrent() {
+ return steerMotorSim.getStatorCurrent();
+ }
+
+ /**
+ *
+ *
+ * Obtains the Position of the Drive Encoder.
+ *
+ * This value represents the un-geared position of the encoder, i.e., the amount of radians the drive motor's
+ * encoder has rotated.
+ *
+ * @return the position of the drive motor's encoder (un-geared)
+ */
+ public Angle getDriveEncoderUnGearedPosition() {
+ return getDriveWheelFinalPosition().times(config.DRIVE_GEAR_RATIO);
+ }
+
+ /**
+ *
+ *
+ *
Obtains the Final Position of the Wheel.
+ *
+ * This method provides the final position of the drive encoder in terms of wheel angle.
+ *
+ * @return the final position of the drive encoder (wheel rotations)
+ */
+ public Angle getDriveWheelFinalPosition() {
+ return driveWheelFinalPosition;
+ }
+
+ /**
+ *
+ *
+ *
Obtains the Speed of the Drive Encoder.
+ *
+ * @return the un-geared speed of the drive encoder
+ */
+ public AngularVelocity getDriveEncoderUnGearedSpeed() {
+ return getDriveWheelFinalSpeed().times(config.DRIVE_GEAR_RATIO);
+ }
+
+ /**
+ *
+ *
+ * Obtains the Final Speed of the Wheel.
+ *
+ * @return the final speed of the drive wheel
+ */
+ public AngularVelocity getDriveWheelFinalSpeed() {
+ return driveWheelFinalSpeed;
+ }
+
+ /**
+ *
+ *
+ * Obtains the Relative Position of the Steer Encoder.
+ *
+ * @return the relative encoder position of the steer motor
+ * @see MapleMotorSim#getEncoderPosition()
+ */
+ public Angle getSteerRelativeEncoderPosition() {
+ return getSteerAbsoluteFacing()
+ .getMeasure()
+ .times(config.STEER_GEAR_RATIO)
+ .plus(steerRelativeEncoderOffSet);
+ }
+
+ /**
+ *
+ *
+ * Obtains the Speed of the Steer Relative Encoder (Geared).
+ *
+ * @return the speed of the steer relative encoder
+ * @see MapleMotorSim#getEncoderVelocity()
+ */
+ public AngularVelocity getSteerRelativeEncoderVelocity() {
+ return getSteerAbsoluteEncoderSpeed().times(config.STEER_GEAR_RATIO);
+ }
+
+ /**
+ *
+ *
+ * Obtains the Absolute Facing of the Steer Mechanism.
+ *
+ * @return the absolute facing of the steer mechanism, as a {@link Rotation2d}
+ */
+ public Rotation2d getSteerAbsoluteFacing() {
+ return new Rotation2d(getSteerAbsoluteAngle());
+ }
+
+ /**
+ *
+ *
+ * Obtains the Absolute Angle of the Steer Mechanism.
+ *
+ * @return the (continuous) final angle of the steer mechanism, as a {@link Angle}
+ * @see MapleMotorSim#getAngularPosition()
+ */
+ public Angle getSteerAbsoluteAngle() {
+ return steerMotorSim.getAngularPosition();
+ }
+
+ /**
+ *
+ *
+ * Obtains the Absolute Rotational Velocity of the Steer Mechanism.
+ *
+ * @return the absolute angular velocity of the steer mechanism
+ */
+ public AngularVelocity getSteerAbsoluteEncoderSpeed() {
+ return steerMotorSim.getVelocity();
+ }
+
+ /**
+ *
+ *
+ * Obtains the Cached Readings of the Drive Encoder's Un-Geared Position.
+ *
+ * The values of {@link #getDriveEncoderUnGearedPosition()} are cached at each sub-tick and can be retrieved
+ * using this method.
+ *
+ * @return an array of cached drive encoder un-geared positions
+ */
+ public Angle[] getCachedDriveEncoderUnGearedPositions() {
+ return driveWheelFinalPositionCache.stream()
+ .map(value -> value.times(config.DRIVE_GEAR_RATIO))
+ .toArray(Angle[]::new);
+ }
+
+ /**
+ *
+ *
+ *
Obtains the Cached Readings of the Drive Encoder's Final Position (Wheel Rotations).
+ *
+ * The values of {@link #getDriveWheelFinalPosition()} are cached at each sub-tick and are divided by the gear
+ * ratio to obtain the final wheel rotations.
+ *
+ * @return an array of cached drive encoder final positions (wheel rotations)
+ */
+ public Angle[] getCachedDriveWheelFinalPositions() {
+ return driveWheelFinalPositionCache.toArray(Angle[]::new);
+ }
+
+ /**
+ *
+ *
+ *
Obtains the Cached Readings of the Steer Relative Encoder's Position.
+ *
+ * The values of {@link #getSteerRelativeEncoderPosition()} are cached at each sub-tick and can be retrieved
+ * using this method.
+ *
+ * @return an array of cached steer relative encoder positions
+ */
+ public Angle[] getCachedSteerRelativeEncoderPositions() {
+ return steerAbsolutePositionCache.stream()
+ .map(absoluteFacing -> absoluteFacing
+ .getMeasure()
+ .times(config.STEER_GEAR_RATIO)
+ .plus(steerRelativeEncoderOffSet))
+ .toArray(Angle[]::new);
+ }
+
+ /**
+ *
+ *
+ *
Obtains the Cached Readings of the Steer Absolute Positions.
+ *
+ * The values of {@link #getSteerAbsoluteFacing()} are cached at each sub-tick and can be retrieved using this
+ * method.
+ *
+ * @return an array of cached absolute steer positions, as {@link Rotation2d} objects
+ */
+ public Rotation2d[] getCachedSteerAbsolutePositions() {
+ return steerAbsolutePositionCache.toArray(Rotation2d[]::new);
+ }
+}
diff --git a/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/configs/BoundingCheck.java b/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/configs/BoundingCheck.java
new file mode 100644
index 0000000..fe0b13b
--- /dev/null
+++ b/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/configs/BoundingCheck.java
@@ -0,0 +1,12 @@
+package swervelib.simulation.ironmaple.simulation.drivesims.configs;
+
+import edu.wpi.first.wpilibj.DriverStation;
+
+public class BoundingCheck {
+ public static void check(double value, double lowerBound, double upperBound, String variableName, String unit) {
+ if (lowerBound <= value && value <= upperBound) return;
+ final String errorMessage = "The provided \"" + variableName + "\" is " + value + unit
+ + ", which seems abnormal, please check its correctness";
+ DriverStation.reportError(errorMessage, true);
+ }
+}
diff --git a/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/configs/DriveTrainSimulationConfig.java b/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/configs/DriveTrainSimulationConfig.java
new file mode 100644
index 0000000..0ba1274
--- /dev/null
+++ b/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/configs/DriveTrainSimulationConfig.java
@@ -0,0 +1,324 @@
+package swervelib.simulation.ironmaple.simulation.drivesims.configs;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.Mass;
+import swervelib.simulation.ironmaple.simulation.drivesims.COTS;
+import swervelib.simulation.ironmaple.simulation.drivesims.GyroSimulation;
+import swervelib.simulation.ironmaple.simulation.drivesims.SwerveModuleSimulation;
+
+import java.util.Arrays;
+import java.util.OptionalDouble;
+import java.util.function.Supplier;
+
+import static edu.wpi.first.units.Units.Kilograms;
+import static edu.wpi.first.units.Units.Meters;
+
+/**
+ *
+ *
+ *
Stores the configurations for a swerve drive simulation.
+ *
+ * This class is used to hold all the parameters necessary for simulating a swerve drivetrain, allowing for realistic
+ * performance testing and evaluation.
+ */
+public class DriveTrainSimulationConfig {
+ public Mass robotMass;
+ public Distance bumperLengthX, bumperWidthY;
+ public Supplier[] swerveModuleSimulationFactories;
+ public Supplier gyroSimulationFactory;
+ public Translation2d[] moduleTranslations;
+
+ /**
+ *
+ *
+ * Ordinary Constructor
+ *
+ * Creates an instance of {@link DriveTrainSimulationConfig} with specified parameters.
+ *
+ * @param robotMass the mass of the robot, including bumpers.
+ * @param bumperLengthX the length of the bumper (distance from front to back).
+ * @param bumperWidthY the width of the bumper (distance from left to right).
+ * @param trackLengthX the distance between the front and rear wheels.
+ * @param trackWidthY the distance between the left and right wheels.
+ * @param swerveModuleSimulationFactory the factory that creates appropriate swerve module simulation for the
+ * drivetrain. You can specify one factory to apply the same configuration over all modules or specify four
+ * factories in the order (FL, FR, BL, BR).
+ * @param gyroSimulationFactory the factory that creates appropriate gyro simulation for the drivetrain.
+ */
+ public DriveTrainSimulationConfig(
+ Mass robotMass,
+ Distance bumperLengthX,
+ Distance bumperWidthY,
+ Distance trackLengthX,
+ Distance trackWidthY,
+ Supplier gyroSimulationFactory,
+ Supplier... swerveModuleSimulationFactory) {
+ this.robotMass = robotMass;
+ this.bumperLengthX = bumperLengthX;
+ this.bumperWidthY = bumperWidthY;
+ this.withTrackLengthTrackWidth(trackLengthX, trackWidthY);
+
+ if (swerveModuleSimulationFactory.length == 1) this.withSwerveModule(swerveModuleSimulationFactory[0]);
+ else if (swerveModuleSimulationFactory.length == 4) this.withSwerveModules(swerveModuleSimulationFactory);
+ else
+ throw new IllegalArgumentException("Module simulation factories length must be 1 or 4, provided "
+ + swerveModuleSimulationFactory.length);
+ this.gyroSimulationFactory = gyroSimulationFactory;
+
+ checkRobotMass();
+ checkBumperSize();
+ }
+
+ /**
+ *
+ *
+ * Default Constructor.
+ *
+ * Creates a {@link DriveTrainSimulationConfig} with all the data set to default values.
+ *
+ *
Though the config starts with default values, any configuration can be modified after creation.
+ *
+ *
The default configurations are:
+ *
+ *
+ * - Robot Mass of 45 kilograms.
+ *
- Bumper Length of 0.76 meters.
+ *
- Bumper Width of 0.76 meters.
+ *
- Track Length of 0.52 meters.
+ *
- Track Width of 0.52 meters.
+ *
- Default swerve module simulations based on Falcon 500 motors.
+ *
- Default gyro simulation using the Pigeon2 gyro.
+ *
+ *
+ * @return a new instance of {@link DriveTrainSimulationConfig} with all configs set to default values.
+ */
+ public static DriveTrainSimulationConfig Default() {
+ return new DriveTrainSimulationConfig(
+ Kilograms.of(45),
+ Meters.of(0.76),
+ Meters.of(.76),
+ Meters.of(0.52),
+ Meters.of(0.52),
+ COTS.ofPigeon2(),
+ COTS.ofMark4(DCMotor.getFalcon500(1), DCMotor.getFalcon500(1), COTS.WHEELS.COLSONS.cof, 2));
+ }
+
+ /**
+ *
+ *
+ * Sets the robot mass.
+ *
+ * Updates the mass of the robot in kilograms.
+ *
+ * @param robotMass the new mass of the robot.
+ * @return the current instance of {@link DriveTrainSimulationConfig} for method chaining.
+ */
+ public DriveTrainSimulationConfig withRobotMass(Mass robotMass) {
+ this.robotMass = robotMass;
+ checkRobotMass();
+ return this;
+ }
+
+ /**
+ *
+ *
+ *
Sets the bumper size.
+ *
+ * Updates the dimensions of the bumper.
+ *
+ * @param bumperLengthX the length of the bumper.
+ * @param bumperWidthY the width of the bumper.
+ * @return the current instance of {@link DriveTrainSimulationConfig} for method chaining.
+ */
+ public DriveTrainSimulationConfig withBumperSize(Distance bumperLengthX, Distance bumperWidthY) {
+ this.bumperLengthX = bumperLengthX;
+ this.bumperWidthY = bumperWidthY;
+
+ checkBumperSize();
+ return this;
+ }
+
+ /**
+ *
+ *
+ *
Sets the track length and width.
+ *
+ * Updates the translations for the swerve modules based on the specified track length and track width.
+ *
+ *
For non-rectangular chassis configuration, use {@link #withCustomModuleTranslations(Translation2d[])} instead.
+ *
+ * @param trackLengthX the distance between the front and rear wheels.
+ * @param trackWidthY the distance between the left and right wheels.
+ * @return the current instance of {@link DriveTrainSimulationConfig} for method chaining.
+ */
+ public DriveTrainSimulationConfig withTrackLengthTrackWidth(Distance trackLengthX, Distance trackWidthY) {
+ BoundingCheck.check(trackLengthX.in(Meters), 0.5, 1.5, "track length", "meters");
+ BoundingCheck.check(trackWidthY.in(Meters), 0.5, 1.5, "track width", "meters");
+
+ this.moduleTranslations = new Translation2d[]{
+ new Translation2d(trackLengthX.in(Meters) / 2, trackWidthY.in(Meters) / 2),
+ new Translation2d(trackLengthX.in(Meters) / 2, -trackWidthY.in(Meters) / 2),
+ new Translation2d(-trackLengthX.in(Meters) / 2, trackWidthY.in(Meters) / 2),
+ new Translation2d(-trackLengthX.in(Meters) / 2, -trackWidthY.in(Meters) / 2)
+ };
+ return this;
+ }
+
+ /**
+ *
+ *
+ *
Sets custom module translations.
+ *
+ * Updates the translations of the swerve modules with user-defined values.
+ *
+ *
For ordinary rectangular modules configuration, use {@link #withTrackLengthTrackWidth(Distance, Distance)}
+ * instead.
+ *
+ * @param moduleTranslations the custom translations for the swerve modules.
+ * @return the current instance of {@link DriveTrainSimulationConfig} for method chaining.
+ */
+ public DriveTrainSimulationConfig withCustomModuleTranslations(Translation2d[] moduleTranslations) {
+ checkModuleTranslations();
+ this.moduleTranslations = moduleTranslations;
+ return this;
+ }
+
+ /**
+ *
+ *
+ *
Sets the swerve module simulation factory.
+ *
+ * Updates the factory used to create swerve module simulations.
+ *
+ * @param swerveModuleSimulationFactory the new factory (or factories) for swerve module simulations. You can
+ * specify one factory to apply the same configuration over all modules, or specify four factories in the order
+ * (FL, FR, BL, BR)
+ * @return the current instance of {@link DriveTrainSimulationConfig} for method chaining.
+ */
+ public DriveTrainSimulationConfig withSwerveModules(
+ Supplier... swerveModuleSimulationFactory) {
+ if (swerveModuleSimulationFactory.length == 1) return withSwerveModule(swerveModuleSimulationFactory[0]);
+
+ if (swerveModuleSimulationFactory.length != moduleTranslations.length)
+ throw new IllegalArgumentException("Module simulation factories length must be 1 or 4, provided "
+ + swerveModuleSimulationFactory.length);
+
+ this.swerveModuleSimulationFactories = swerveModuleSimulationFactory;
+ return this;
+ }
+
+ /**
+ *
+ *
+ * Sets the swerve module simulation factory.
+ *
+ * Updates the factory used to create swerve module simulations.
+ *
+ *
Uses the same configuration over all the modules
+ *
+ * @param swerveModuleSimulationFactory the new factory for swerve module simulations.
+ * @return the current instance of {@link DriveTrainSimulationConfig} for method chaining.
+ */
+ public DriveTrainSimulationConfig withSwerveModule(Supplier swerveModuleSimulationFactory) {
+ this.swerveModuleSimulationFactories = new Supplier[moduleTranslations.length];
+ Arrays.fill(this.swerveModuleSimulationFactories, swerveModuleSimulationFactory);
+ return this;
+ }
+
+ /**
+ *
+ *
+ * Sets the gyro simulation factory.
+ *
+ * Updates the factory used to create gyro simulations.
+ *
+ * @param gyroSimulationFactory the new factory for gyro simulations.
+ * @return the current instance of {@link DriveTrainSimulationConfig} for method chaining.
+ */
+ public DriveTrainSimulationConfig withGyro(Supplier gyroSimulationFactory) {
+ this.gyroSimulationFactory = gyroSimulationFactory;
+ return this;
+ }
+
+ /**
+ *
+ *
+ * Calculates the density of the robot.
+ *
+ * Returns the density of the robot based on its mass and bumper dimensions.
+ *
+ * @return the density in kilograms per square meter.
+ */
+ public double getDensityKgPerSquaredMeters() {
+ return robotMass.in(Kilograms) / (bumperLengthX.in(Meters) * bumperWidthY.in(Meters));
+ }
+
+ /**
+ *
+ *
+ *
Calculates the track length in the X direction.
+ *
+ * Returns the total distance between the frontmost and rearmost module translations in the X direction.
+ *
+ * @return the track length.
+ * @throws IllegalStateException if the module translations are empty.
+ */
+ public Distance trackLengthX() {
+ final OptionalDouble maxModuleX = Arrays.stream(moduleTranslations)
+ .mapToDouble(Translation2d::getX)
+ .max();
+ final OptionalDouble minModuleX = Arrays.stream(moduleTranslations)
+ .mapToDouble(Translation2d::getX)
+ .min();
+ if (maxModuleX.isEmpty() || minModuleX.isEmpty())
+ throw new IllegalStateException("Modules translations are empty");
+ return Meters.of(maxModuleX.getAsDouble() - minModuleX.getAsDouble());
+ }
+
+ /**
+ *
+ *
+ *
Calculates the track width in the Y direction.
+ *
+ * Returns the total distance between the leftmost and rightmost module translations in the Y direction.
+ *
+ * @return the track width.
+ * @throws IllegalStateException if the module translations are empty.
+ */
+ public Distance trackWidthY() {
+ final OptionalDouble maxModuleY = Arrays.stream(moduleTranslations)
+ .mapToDouble(Translation2d::getY)
+ .max();
+ final OptionalDouble minModuleY = Arrays.stream(moduleTranslations)
+ .mapToDouble(Translation2d::getY)
+ .min();
+ if (maxModuleY.isEmpty() || minModuleY.isEmpty())
+ throw new IllegalStateException("Modules translations are empty");
+ return Meters.of(maxModuleY.getAsDouble() - minModuleY.getAsDouble());
+ }
+
+ public Distance driveBaseRadius() {
+ return Meters.of(Math.hypot(trackLengthX().in(Meters), trackWidthY().in(Meters)));
+ }
+
+ private void checkRobotMass() {
+ BoundingCheck.check(robotMass.in(Kilograms), 10, 80, "robot mass", "kg");
+ }
+
+ private void checkBumperSize() {
+ BoundingCheck.check(bumperLengthX.in(Meters), 0.5, 1.5, "bumper length", "meters");
+ BoundingCheck.check(bumperWidthY.in(Meters), 0.5, 1.5, "bumper width", "meters");
+ }
+
+ private void checkModuleTranslations() {
+ for (int i = 0; i < moduleTranslations.length; i++)
+ BoundingCheck.check(
+ moduleTranslations[i].getNorm(),
+ 0.2,
+ 1.2,
+ "module number " + i + " translation magnitude",
+ "meters");
+ }
+}
diff --git a/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/configs/SwerveModuleSimulationConfig.java b/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/configs/SwerveModuleSimulationConfig.java
new file mode 100644
index 0000000..4fb958c
--- /dev/null
+++ b/yagsl/java/swervelib/simulation/ironmaple/simulation/drivesims/configs/SwerveModuleSimulationConfig.java
@@ -0,0 +1,129 @@
+package swervelib.simulation.ironmaple.simulation.drivesims.configs;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.*;
+import swervelib.simulation.ironmaple.simulation.SimulatedArena;
+import swervelib.simulation.ironmaple.simulation.drivesims.SwerveModuleSimulation;
+import swervelib.simulation.ironmaple.simulation.motorsims.SimMotorConfigs;
+
+import java.util.function.Supplier;
+
+import static edu.wpi.first.units.Units.*;
+
+public class SwerveModuleSimulationConfig implements Supplier {
+ public final SimMotorConfigs driveMotorConfigs, steerMotorConfigs;
+ public final double DRIVE_GEAR_RATIO, STEER_GEAR_RATIO, WHEELS_COEFFICIENT_OF_FRICTION;
+ public final Voltage DRIVE_FRICTION_VOLTAGE;
+ public final Distance WHEEL_RADIUS;
+
+ /**
+ *
+ *
+ * Constructs a Configuration for Swerve Module Simulation.
+ *
+ * If you are using {@link SimulatedArena#overrideSimulationTimings(Time, int)} to use custom timings, you must
+ * call the method before constructing any swerve module simulations using this constructor.
+ *
+ * @param driveMotorModel the model of the driving motor
+ * @param steerMotorModel; the model of the steering motor
+ * @param driveGearRatio the gear ratio for the driving motor, >1 is reduction
+ * @param steerGearRatio the gear ratio for the steering motor, >1 is reduction
+ * @param driveFrictionVoltage the measured minimum amount of voltage that can turn the driving rotter
+ * @param steerFrictionVoltage the measured minimum amount of voltage that can turn the steering rotter
+ * @param wheelRadius the radius of the wheels.
+ * @param steerRotationalInertia the rotational inertia of the entire steering mechanism
+ * @param wheelsCoefficientOfFriction the coefficient
+ * of friction of the tires, normally around 1.2 {@link Units#inchesToMeters(double)}.
+ */
+ public SwerveModuleSimulationConfig(
+ DCMotor driveMotorModel,
+ DCMotor steerMotorModel,
+ double driveGearRatio,
+ double steerGearRatio,
+ Voltage driveFrictionVoltage,
+ Voltage steerFrictionVoltage,
+ Distance wheelRadius,
+ MomentOfInertia steerRotationalInertia,
+ double wheelsCoefficientOfFriction) {
+ BoundingCheck.check(driveGearRatio, 4, 24, "drive gear ratio", "times reduction");
+ BoundingCheck.check(steerGearRatio, 6, 50, "steer gear ratio", "times reduction");
+ BoundingCheck.check(driveFrictionVoltage.in(Volts), 0.01, 0.35, "drive friction voltage", "volts");
+ BoundingCheck.check(steerFrictionVoltage.in(Volts), 0.01, 0.6, "steer friction voltage", "volts");
+ BoundingCheck.check(wheelRadius.in(Inches), 1, 3.2, "drive wheel radius", "inches");
+ BoundingCheck.check(
+ steerRotationalInertia.in(KilogramSquareMeters), 0.005, 0.06, "steer rotation inertia", "kg * m^2");
+ BoundingCheck.check(wheelsCoefficientOfFriction, 0.6, 1.9, "tire coefficient of friction", "");
+
+ this.driveMotorConfigs =
+ new SimMotorConfigs(driveMotorModel, driveGearRatio, KilogramSquareMeters.zero(), driveFrictionVoltage);
+ this.steerMotorConfigs =
+ new SimMotorConfigs(steerMotorModel, steerGearRatio, steerRotationalInertia, steerFrictionVoltage);
+ DRIVE_GEAR_RATIO = driveGearRatio;
+ STEER_GEAR_RATIO = steerGearRatio;
+ WHEELS_COEFFICIENT_OF_FRICTION = wheelsCoefficientOfFriction;
+ DRIVE_FRICTION_VOLTAGE = driveFrictionVoltage;
+ WHEEL_RADIUS = wheelRadius;
+ }
+
+ @Override
+ public SwerveModuleSimulation get() {
+ return new SwerveModuleSimulation(this);
+ }
+
+ public double getGrippingForceNewtons(double gravityForceOnModuleNewtons) {
+ return gravityForceOnModuleNewtons * WHEELS_COEFFICIENT_OF_FRICTION;
+ }
+
+ /**
+ *
+ *
+ *
Obtains the theoretical speed that the module can achieve.
+ *
+ * @return the theoretical maximum ground speed that the module can achieve, in m/s
+ */
+ public LinearVelocity maximumGroundSpeed() {
+ return MetersPerSecond.of(
+ driveMotorConfigs.freeSpinMechanismVelocity().in(RadiansPerSecond) * WHEEL_RADIUS.in(Meters));
+ }
+
+ /**
+ *
+ *
+ * Obtains the theoretical maximum propelling force of ONE module.
+ *
+ * Calculates the maximum propelling force with respect to the gripping force and the drive motor's torque under
+ * its current limit.
+ *
+ * @param robotMass the mass of the robot
+ * @param modulesCount the amount of modules on the robot, assumed to be sharing the gravity force equally
+ * @return the maximum propelling force of EACH module
+ */
+ public Force getTheoreticalPropellingForcePerModule(Mass robotMass, int modulesCount, Current statorCurrentLimit) {
+ final double
+ maxThrustNewtons =
+ driveMotorConfigs.calculateTorque(statorCurrentLimit).in(NewtonMeters)
+ / WHEEL_RADIUS.in(Meters),
+ maxGrippingNewtons = 9.8 * robotMass.in(Kilograms) / modulesCount * WHEELS_COEFFICIENT_OF_FRICTION;
+
+ return Newtons.of(Math.min(maxThrustNewtons, maxGrippingNewtons));
+ }
+
+ /**
+ *
+ *
+ *
Obtains the theatrical linear acceleration that the robot can achieve.
+ *
+ * Calculates the maximum linear acceleration of a robot, with respect to its mass and
+ * {@link #getTheoreticalPropellingForcePerModule(Mass, int, Current)}.
+ *
+ * @param robotMass the mass of the robot
+ * @param modulesCount the amount of modules on the robot, assumed to be sharing the gravity force equally
+ */
+ public LinearAcceleration maxAcceleration(Mass robotMass, int modulesCount, Current statorCurrentLimit) {
+ return getTheoreticalPropellingForcePerModule(robotMass, modulesCount, statorCurrentLimit)
+ .times(modulesCount)
+ .div(robotMass);
+ }
+}
diff --git a/yagsl/java/swervelib/simulation/ironmaple/simulation/gamepieces/GamePiece.java b/yagsl/java/swervelib/simulation/ironmaple/simulation/gamepieces/GamePiece.java
new file mode 100644
index 0000000..d445b10
--- /dev/null
+++ b/yagsl/java/swervelib/simulation/ironmaple/simulation/gamepieces/GamePiece.java
@@ -0,0 +1,58 @@
+package swervelib.simulation.ironmaple.simulation.gamepieces;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Translation3d;
+
+/**
+ *
+ *
+ *
Interface for all Game pieces.
+ *
+ * This class contains basic functions all game pieces will have so that game pieces of different types can be used
+ * interchangeably in collision detection.
+ */
+public interface GamePiece {
+
+ /**
+ *
+ *
+ *
Gives the pose3d of a game piece.
+ *
+ * @return The pose of this piece as a Pose3d.
+ */
+ Pose3d getPose3d();
+
+ /**
+ *
+ *
+ * Gives the string type of the current game piece.
+ *
+ * @return The game piece string type (ex "Coral", "Algae", "Note").
+ */
+ String getType();
+
+ /**
+ *
+ *
+ * Gives the velocity of the game piece.
+ *
+ * For grounded game pieces the z access velocity does not exist and so will be set to 0 automatically.
+ *
+ * @return The velocity of the game piece as a Translation3d.
+ */
+ Translation3d getVelocity3dMPS();
+
+
+
+ /**
+ *
+ *
+ *
Gives wether or not the piece is "grounded".
+ *
+ * A grounded piece is likely a child of {@link GamePieceOnFieldSimulation} while a non grounded piece is likely a
+ * child of{@link GamePieceProjectile}.
+ *
+ * @return wether or not the piece is grounded.
+ */
+ boolean isGrounded();
+}
diff --git a/yagsl/java/swervelib/simulation/ironmaple/simulation/gamepieces/GamePieceOnFieldSimulation.java b/yagsl/java/swervelib/simulation/ironmaple/simulation/gamepieces/GamePieceOnFieldSimulation.java
new file mode 100644
index 0000000..55ea347
--- /dev/null
+++ b/yagsl/java/swervelib/simulation/ironmaple/simulation/gamepieces/GamePieceOnFieldSimulation.java
@@ -0,0 +1,183 @@
+package swervelib.simulation.ironmaple.simulation.gamepieces;
+
+import edu.wpi.first.math.geometry.*;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.Mass;
+import org.dyn4j.dynamics.Body;
+import org.dyn4j.dynamics.BodyFixture;
+import org.dyn4j.geometry.Convex;
+import org.dyn4j.geometry.MassType;
+import swervelib.simulation.ironmaple.simulation.IntakeSimulation;
+import swervelib.simulation.ironmaple.simulation.SimulatedArena;
+import swervelib.simulation.ironmaple.utils.mathutils.GeometryConvertor;
+
+import java.util.function.DoubleSupplier;
+
+import static edu.wpi.first.units.Units.Kilogram;
+import static edu.wpi.first.units.Units.Meters;
+
+/**
+ *
+ *
+ *
Simulates a Game Piece on the Field.
+ *
+ * This class simulates a game piece on the field, which has a collision space and interacts with other objects.
+ *
+ *
Game pieces can be "grabbed" by an {@link IntakeSimulation}.
+ *
+ *
For the simulation to actually run, every instance must be added to a
+ * {@link swervelib.simulation.ironmaple.simulation.SimulatedArena} through
+ * {@link SimulatedArena#addGamePiece(GamePieceOnFieldSimulation)}.
+ */
+public class GamePieceOnFieldSimulation extends Body implements GamePiece {
+ public static final double COEFFICIENT_OF_FRICTION = 0.8, MINIMUM_BOUNCING_VELOCITY = 0.2;
+
+ /**
+ *
+ *
+ *
Supplier of the Current Z-Pose (Height) of the Game Piece.
+ *
+ * Normally, the height is fixed at half the thickness of the game piece to simulate it being "on the ground."
+ *
+ *
If the game piece is flying at a low height, the height is calculated using the law of free-fall.
+ */
+ private final DoubleSupplier zPositionSupplier;
+ /**
+ *
+ *
+ *
The Type of the Game Piece.
+ *
+ * Affects the result of {@link SimulatedArena#getGamePiecesPosesByType(String)}.
+ */
+ public final String type;
+
+ /**
+ *
+ *
+ *
Creates a Game Piece on the Field with Fixed Height.
+ *
+ * @param info info about the game piece type
+ * @param initialPose the initial position of the game piece on the field
+ */
+ public GamePieceOnFieldSimulation(GamePieceInfo info, Pose2d initialPose) {
+ this(info, () -> info.gamePieceHeight.in(Meters) / 2, initialPose, new Translation2d());
+ }
+
+ /**
+ *
+ *
+ * Creates a Game Piece on the Field with Custom Height Supplier and Initial Velocity.
+ *
+ * @param info info about the game piece type
+ * @param zPositionSupplier a supplier that provides the current Z-height of the game piece
+ * @param initialPose the initial position of the game piece on the field
+ * @param initialVelocityMPS the initial velocity of the game piece, in meters per second
+ */
+ public GamePieceOnFieldSimulation(
+ GamePieceInfo info,
+ DoubleSupplier zPositionSupplier,
+ Pose2d initialPose,
+ Translation2d initialVelocityMPS) {
+ super();
+ this.type = info.type;
+ this.zPositionSupplier = zPositionSupplier;
+
+ BodyFixture bodyFixture = super.addFixture(info.shape);
+
+ bodyFixture.setFriction(COEFFICIENT_OF_FRICTION);
+ bodyFixture.setRestitution(info.coefficientOfRestitution);
+ bodyFixture.setRestitutionVelocity(MINIMUM_BOUNCING_VELOCITY);
+
+ bodyFixture.setDensity(info.gamePieceMass.in(Kilogram) / info.shape.getArea());
+ super.setMass(MassType.NORMAL);
+
+ super.setLinearDamping(info.linearDamping);
+ super.setAngularDamping(info.angularDamping);
+ super.setBullet(true);
+
+ super.setTransform(GeometryConvertor.toDyn4jTransform(initialPose));
+ super.setLinearVelocity(GeometryConvertor.toDyn4jVector2(initialVelocityMPS));
+ }
+
+ /**
+ *
+ *
+ * Sets the world velocity of this game piece.
+ *
+ * @param chassisSpeedsWorldFrame the speeds of the game piece
+ */
+ public void setVelocity(ChassisSpeeds chassisSpeedsWorldFrame) {
+ super.setLinearVelocity(GeometryConvertor.toDyn4jLinearVelocity(chassisSpeedsWorldFrame));
+ super.setAngularVelocity(chassisSpeedsWorldFrame.omegaRadiansPerSecond);
+ }
+
+ /**
+ *
+ *
+ * Obtains the 2d position of the game piece
+ *
+ * @return the 2d position of the game piece
+ */
+ public Pose2d getPoseOnField() {
+ return GeometryConvertor.toWpilibPose2d(super.getTransform());
+ }
+
+ /**
+ *
+ *
+ * Obtains a 3d pose of the game piece.
+ *
+ * The 3d position is calculated from both the {@link #getPoseOnField()} and {@link #zPositionSupplier}
+ *
+ * @return the 3d position of the game piece
+ */
+ @Override
+ public Pose3d getPose3d() {
+ final Pose2d pose2d = getPoseOnField();
+ return new Pose3d(
+ pose2d.getX(),
+ pose2d.getY(),
+ zPositionSupplier.getAsDouble(),
+ new Rotation3d(0, 0, pose2d.getRotation().getRadians()));
+ }
+
+ /**
+ *
+ *
+ *
Stores the info of a type of game piece
+ *
+ * @param type the type of the game piece, affecting categorization within the arena
+ * @param shape the shape of the collision space for the game piece
+ * @param gamePieceHeight the height (thickness) of the game piece, in meters
+ * @param gamePieceMass the mass of the game piece, in kilograms
+ */
+ public record GamePieceInfo(
+ String type,
+ Convex shape,
+ Distance gamePieceHeight,
+ Mass gamePieceMass,
+ double linearDamping,
+ double angularDamping,
+ double coefficientOfRestitution) {
+ }
+
+ public void onIntake(String intakeTargetGamePieceType) {
+ }
+
+ @Override
+ public String getType() {
+ return this.type;
+ }
+
+ @Override
+ public Translation3d getVelocity3dMPS() {
+ return new Translation3d(GeometryConvertor.toWpilibTranslation2d(this.getLinearVelocity()));
+ }
+
+ @Override
+ public boolean isGrounded() {
+ return true;
+ }
+
+}
diff --git a/yagsl/java/swervelib/simulation/ironmaple/simulation/gamepieces/GamePieceProjectile.java b/yagsl/java/swervelib/simulation/ironmaple/simulation/gamepieces/GamePieceProjectile.java
new file mode 100644
index 0000000..118f8f9
--- /dev/null
+++ b/yagsl/java/swervelib/simulation/ironmaple/simulation/gamepieces/GamePieceProjectile.java
@@ -0,0 +1,658 @@
+package swervelib.simulation.ironmaple.simulation.gamepieces;
+
+import edu.wpi.first.math.geometry.*;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.LinearVelocity;
+import edu.wpi.first.wpilibj.Timer;
+import swervelib.simulation.ironmaple.simulation.SimulatedArena;
+import swervelib.simulation.ironmaple.utils.LegacyFieldMirroringUtils2024;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Queue;
+import java.util.Set;
+import java.util.concurrent.ArrayBlockingQueue;
+import java.util.function.Consumer;
+import java.util.function.Supplier;
+
+import static edu.wpi.first.units.Units.*;
+
+/**
+ *
+ *
+ * Simulates a Game Piece Launched into the Air
+ *
+ * CheckOnline
+ * Documentation
+ *
+ *
The movement is modeled by simple projectile motion.
+ *
+ *
If the projectile flies off the field, touches the ground, or hits its target, it will be automatically removed.
+ *
+ *
Additional Features:
+ *
+ *
+ * - Optionally, it can be configured to become a {@link GamePieceOnFieldSimulation} upon touching the ground.
+ *
- Optionally, it can be configured to have a "desired target." Upon hitting the target, it can be configured to
+ * run a callback.
+ *
+ *
+ * Limitations:
+ *
+ *
+ * - Air drag is ignored.
+ *
- DOES NOT have collision space when flying.
+ *
+ */
+public class GamePieceProjectile implements GamePiece {
+ /**
+ * This value may seem unusual compared to the standard 9.8 m/s² for gravity. However, through experimentation, it
+ * appears more realistic in our simulation, possibly due to the ignoring of air drag.
+ */
+ public static final double GRAVITY = 11;
+
+ // Properties of the game piece projectile:
+ protected final GamePieceOnFieldSimulation.GamePieceInfo info;
+ public final String gamePieceType;
+ protected Translation2d initialPosition;
+ protected final Translation2d initialLaunchingVelocityMPS;
+ protected final double initialHeight, initialVerticalSpeedMPS;
+ protected final Rotation3d gamePieceRotation;
+ protected final Timer launchedTimer;
+
+ /**
+ *
+ *
+ * Visualizes the Projectile Flight Trajectory.
+ *
+ * Optionally, this callback will be used to visualize the projectile flight trajectory in a telemetry system,
+ * such as Advantage Scope.
+ */
+ private Consumer> projectileTrajectoryDisplayCallBackHitTarget = projectileTrajectory -> {
+ };
+
+ private Consumer> projectileTrajectoryDisplayCallBackMiss = projectileTrajectory -> {
+ };
+
+ // Optional properties of the game piece, used if we want it to become a
+ // GamePieceOnFieldSimulation upon touching ground:
+ protected boolean becomesGamePieceOnGroundAfterTouchGround = false;
+
+ // Optional properties of the game piece, used if we want it to have a target:
+ private Translation3d tolerance = new Translation3d(0.2, 0.2, 0.2);
+ private Supplier targetPositionSupplier = () -> new Translation3d(0, 0, -100);
+ private Runnable hitTargetCallBack = () -> {
+ };
+ private double heightAsTouchGround = 0.5;
+
+ /**
+ *
+ *
+ * Time to Hit the Desired Target.
+ *
+ * This value represents the amount of time it takes for the projectile to hit the desired target, calculated
+ * when the {@link #launch()} method is called.
+ *
+ *
Determines the results of {@link #hasHitTarget()} and {@link #willHitTarget()}
+ *
+ *
If the projectile never hits the target, or if there is no target, this value remains
+ * -1.
+ */
+ private double calculatedHitTargetTime = -1;
+
+ private boolean hitTargetCallBackCalled = false;
+
+ /**
+ *
+ *
+ *
Creates a Game Piece Projectile Ejected from a Shooter.
+ *
+ * @param info the info of the game piece
+ * @param robotPosition the position of the robot (not the shooter) at the time of launching the game piece
+ * @param shooterPositionOnRobot the translation from the shooter's position to the robot's center, in the robot's
+ * frame of reference
+ * @param chassisSpeedsFieldRelative the field-relative velocity of the robot chassis when launching the game piece,
+ * influencing the initial velocity of the game piece
+ * @param shooterFacing the direction in which the shooter is facing at launch
+ * @param initialHeight the initial height of the game piece when launched, i.e., the height of the shooter from the
+ * ground
+ * @param launchingSpeed the speed at which the game piece is launch
+ * @param shooterAngle the pitch angle of the shooter when launching
+ */
+ public GamePieceProjectile(
+ GamePieceOnFieldSimulation.GamePieceInfo info,
+ Translation2d robotPosition,
+ Translation2d shooterPositionOnRobot,
+ ChassisSpeeds chassisSpeedsFieldRelative,
+ Rotation2d shooterFacing,
+ Distance initialHeight,
+ LinearVelocity launchingSpeed,
+ Angle shooterAngle) {
+ this(
+ info,
+ robotPosition.plus(shooterPositionOnRobot.rotateBy(shooterFacing)),
+ calculateInitialProjectileVelocityMPS(
+ shooterPositionOnRobot,
+ chassisSpeedsFieldRelative,
+ shooterFacing,
+ launchingSpeed.in(MetersPerSecond) * Math.cos(shooterAngle.in(Radians))),
+ initialHeight.in(Meters),
+ launchingSpeed.in(MetersPerSecond) * Math.sin(shooterAngle.in(Radians)),
+ new Rotation3d(0, -shooterAngle.in(Radians), shooterFacing.getRadians()));
+ }
+
+ /**
+ *
+ *
+ * Calculates the Initial Velocity of the Game Piece Projectile in the X-Y Plane.
+ *
+ * This method calculates the initial velocity of the game piece projectile, accounting for the chassis's
+ * translational and rotational motion as well as the shooter's ground speed.
+ *
+ * @param shooterPositionOnRobot the translation of the shooter on the robot, in the robot's frame of reference
+ * @param chassisSpeeds the speeds of the chassis when the game piece is launched, including translational and
+ * rotational velocities
+ * @param chassisFacing the direction the chassis is facing at the time of the launch
+ * @param groundSpeedMPS the ground component of the projectile's initial velocity, provided as a scalar in meters
+ * per second (m/s)
+ * @return the calculated initial velocity of the projectile as a {@link Translation2d} in meters per second
+ */
+ private static Translation2d calculateInitialProjectileVelocityMPS(
+ Translation2d shooterPositionOnRobot,
+ ChassisSpeeds chassisSpeeds,
+ Rotation2d chassisFacing,
+ double groundSpeedMPS) {
+ final Translation2d
+ chassisTranslationalVelocity =
+ new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond),
+ shooterGroundVelocityDueToChassisRotation =
+ shooterPositionOnRobot
+ .rotateBy(chassisFacing)
+ .rotateBy(Rotation2d.fromDegrees(90))
+ .times(chassisSpeeds.omegaRadiansPerSecond),
+ shooterGroundVelocity = chassisTranslationalVelocity.plus(shooterGroundVelocityDueToChassisRotation);
+
+ return shooterGroundVelocity.plus(new Translation2d(groundSpeedMPS, chassisFacing));
+ }
+
+ /**
+ *
+ *
+ *
Creates a Game Piece Projectile Ejected from a Shooter.
+ *
+ * @param info the info of the game piece
+ * @param initialPosition the position of the game piece at the moment it is launched into the air
+ * @param initialLaunchingVelocityMPS the horizontal component of the initial velocity in the X-Y plane, in meters
+ * per second (m/s)
+ * @param initialHeight the initial height of the game piece when launched (the height of the shooter from the
+ * ground)
+ * @param initialVerticalSpeedMPS the vertical component of the initial velocity, in meters per second (m/s)
+ * @param gamePieceRotation the 3D rotation of the game piece during flight (only affects visualization of the game
+ * piece)
+ */
+ public GamePieceProjectile(
+ GamePieceOnFieldSimulation.GamePieceInfo info,
+ Translation2d initialPosition,
+ Translation2d initialLaunchingVelocityMPS,
+ double initialHeight,
+ double initialVerticalSpeedMPS,
+ Rotation3d gamePieceRotation) {
+ this.info = info;
+ this.gamePieceType = info.type();
+ this.initialPosition = initialPosition;
+ this.initialLaunchingVelocityMPS = initialLaunchingVelocityMPS;
+ this.initialHeight = initialHeight;
+ this.initialVerticalSpeedMPS = initialVerticalSpeedMPS;
+ this.gamePieceRotation = gamePieceRotation;
+ this.launchedTimer = new Timer();
+ }
+
+ /**
+ *
+ *
+ * Starts the Game Piece Projectile Simulation.
+ *
+ * This method initiates the projectile motion of the game piece with the following actions:
+ *
+ *
+ * - Initiates the projectile motion of the game piece. The current pose can be obtained with
+ * {@link #getPose3d()}.
+ *
- Calculates whether the projectile will hit the target during its flight. The result can be obtained using
+ * {@link #willHitTarget()}.
+ *
- Calculates a preview trajectory by simulating the projectile's motion for up to 100 steps, with each step
+ * lasting 0.02 seconds.
+ *
- If specified, displays the trajectory using
+ * {@link GamePieceProjectile#projectileTrajectoryDisplayCallBackHitTarget}, which can be set via
+ * {@link GamePieceProjectile#withProjectileTrajectoryDisplayCallBack(Consumer)}.
+ *
- Starts the {@link #launchedTimer}, which stores the amount of time elapsed after the game piece is launched
+ *
+ */
+ public void launch() {
+ final int maxIterations = 100;
+ final double stepSeconds = 0.02;
+ List trajectoryPoints = new ArrayList<>();
+
+ for (int i = 0; i < maxIterations; i++) {
+ final double t = i * stepSeconds;
+ final Translation3d currentPosition = getPositionAtTime(t);
+ trajectoryPoints.add(new Pose3d(currentPosition, gamePieceRotation));
+
+ if (currentPosition.getZ() < heightAsTouchGround && t * GRAVITY > initialVerticalSpeedMPS) break;
+ if (isOutOfField(t)) break;
+ final Translation3d displacementToTarget =
+ targetPositionSupplier.get().minus(currentPosition);
+ if (Math.abs(displacementToTarget.getX()) < tolerance.getX()
+ && Math.abs(displacementToTarget.getY()) < tolerance.getY()
+ && Math.abs(displacementToTarget.getZ()) < tolerance.getZ()) {
+ this.calculatedHitTargetTime = t;
+ break;
+ }
+ }
+ if (willHitTarget()) projectileTrajectoryDisplayCallBackHitTarget.accept(trajectoryPoints);
+ else projectileTrajectoryDisplayCallBackMiss.accept(trajectoryPoints);
+ this.hitTargetCallBackCalled = false;
+
+ launchedTimer.start();
+ }
+
+ /**
+ *
+ *
+ * Checks if the Game Piece Has Touched the Ground.
+ *
+ * This method determines whether the game piece has touched the ground at the current time.
+ *
+ *
+ * - The result is calculated during the {@link #launch()} method.
+ *
- Before calling {@link #launch()}, this method will always return
false.
+ *
+ *
+ * @return true if the game piece has touched the ground, otherwise false
+ *
+ */
+ public boolean hasHitGround() {
+ return getPositionAtTime(launchedTimer.get()).getZ() <= heightAsTouchGround
+ && launchedTimer.get() * GRAVITY > initialVerticalSpeedMPS;
+ }
+
+ /**
+ *
+ *
+ * Checks if the Game Piece Has Flown Out of the Field's Boundaries.
+ *
+ * This method determines whether the game piece has flown out of the field's boundaries (outside the fence).
+ *
+ *
+ * - The result is calculated during the {@link #launch()} method.
+ *
- Before calling {@link #launch()}, this method will always return
false.
+ *
+ *
+ * @return true if the game piece has flown out of the field's boundaries, otherwise false
+ */
+ public boolean hasGoneOutOfField() {
+ return isOutOfField(launchedTimer.get());
+ }
+
+ private boolean isOutOfField(double time) {
+ final Translation3d position = getPositionAtTime(time);
+ final double EDGE_TOLERANCE = 2;
+ return position.getX() < -EDGE_TOLERANCE
+ || position.getX() > LegacyFieldMirroringUtils2024.FIELD_WIDTH + EDGE_TOLERANCE
+ || position.getY() < -EDGE_TOLERANCE
+ || position.getY() > LegacyFieldMirroringUtils2024.FIELD_HEIGHT + EDGE_TOLERANCE;
+ }
+
+ /**
+ *
+ *
+ * Checks if the projectile will hit the target AT SOME MOMENT during its flight
+ *
+ *
+ * - The result is calculated during the {@link #launch()} method.
+ *
- Before calling {@link #launch()}, this method will always return
false.
+ *
+ *
+ * This is different from {@link #hasHitTarget()}
+ */
+ public boolean willHitTarget() {
+ return calculatedHitTargetTime != -1;
+ }
+
+ /**
+ *
+ *
+ *
Checks if the Projectile Has Already Hit the Target At the Moment.
+ *
+ * This method checks whether the projectile has hit the target at the current time.
+ *
+ *
+ * - Before calling {@link #launch()}, this method will always return
false.
+ * - This is different from {@link #willHitTarget()}, which predicts whether the projectile will eventually hit
+ * the target.
+ *
+ *
+ * @return true if the projectile has hit the target at the current time, otherwise false
+ */
+ public boolean hasHitTarget() {
+ return willHitTarget() && launchedTimer.get() >= calculatedHitTargetTime;
+ }
+
+ /**
+ *
+ *
+ * Clean up the trajectory through {@link #projectileTrajectoryDisplayCallBackHitTarget}
+ *
+ * @return this instance
+ */
+ public GamePieceProjectile cleanUp() {
+ this.projectileTrajectoryDisplayCallBackHitTarget.accept(new ArrayList<>());
+ this.projectileTrajectoryDisplayCallBackMiss.accept(new ArrayList<>());
+ return this;
+ }
+
+ /**
+ *
+ *
+ * Calculates the Projectile's Position at a Given Time.
+ *
+ * This method calculates the position of the projectile using the physics formula for projectile motion.
+ *
+ * @param t the time elapsed after the launch of the projectile, in seconds
+ * @return the calculated position of the projectile at time t as a {@link Translation3d} object
+ */
+ protected Translation3d getPositionAtTime(double t) {
+ final double height = initialHeight + initialVerticalSpeedMPS * t - 1.0 / 2.0 * GRAVITY * t * t;
+
+ final Translation2d current2dPosition = initialPosition.plus(initialLaunchingVelocityMPS.times(t));
+ return new Translation3d(current2dPosition.getX(), current2dPosition.getY(), height);
+ }
+
+ /**
+ *
+ *
+ *
Calculates the Projectile's Velocity at a Given Time.
+ *
+ * This method calculates the 3d velocity of the projectile using the physics formula for projectile motion.
+ *
+ * @param t the time elapsed after the launch of the projectile, in seconds
+ * @return a {@link Translation3d} object representing the calculated 3d velocity of the projectile at time t
+ * , in meters per second
+ */
+ private Translation3d getVelocityMPSAtTime(double t) {
+ final double verticalVelocityMPS = initialVerticalSpeedMPS - GRAVITY * t;
+
+ return new Translation3d(
+ initialLaunchingVelocityMPS.getX(), initialLaunchingVelocityMPS.getY(), verticalVelocityMPS);
+ }
+
+ /**
+ *
+ *
+ *
Calculates the Projectile's Current Position.
+ *
+ * The position is calculated using {@link #getPositionAtTime(double)} while the rotation is pre-stored.
+ *
+ * @return a {@link Pose3d} object representing the current pose of the game piece
+ */
+ @Override
+ public Pose3d getPose3d() {
+ return new Pose3d(getPositionAtTime(launchedTimer.get()), gamePieceRotation);
+ }
+
+ /**
+ *
+ *
+ *
Calculates the Projectile's Velocity at a Given Time.
+ *
+ * @return a {@link Translation3d} object representing the calculated 3d velocity of the projectile at time t
+ * , in meters per second
+ * @see #getVelocityMPSAtTime(double)
+ */
+ @Override
+ public Translation3d getVelocity3dMPS() {
+ return getVelocityMPSAtTime(launchedTimer.get());
+ }
+
+ /**
+ *
+ *
+ * Adds a {@link GamePieceOnFieldSimulation} to a {@link SimulatedArena} to Simulate the Game Piece After
+ * Touch-Ground.
+ *
+ * The added {@link GamePieceOnFieldSimulation} will have the initial velocity of the game piece projectile.
+ *
+ *
The game piece will start falling from mid-air until it touches the ground.
+ *
+ *
The added {@link GamePieceOnFieldSimulation} will always have collision space on the field, even before
+ * touching the ground.
+ *
+ * @param simulatedArena the arena simulation to which the game piece will be added, usually obtained from
+ * {@link SimulatedArena#getInstance()}
+ */
+ public void addGamePieceAfterTouchGround(SimulatedArena simulatedArena) {
+ if (!becomesGamePieceOnGroundAfterTouchGround) return;
+ simulatedArena.addGamePiece(new GamePieceOnFieldSimulation(
+ info,
+ () -> Math.max(
+ info.gamePieceHeight().in(Meters) / 2,
+ getPositionAtTime(launchedTimer.get()).getZ()),
+ new Pose2d(getPositionAtTime(launchedTimer.get()).toTranslation2d(), new Rotation2d()),
+ initialLaunchingVelocityMPS));
+ }
+
+ /**
+ *
+ *
+ *
Check every {@link GamePieceProjectile} instance for available actions.
+ *
+ * 1. If a game piece {@link #hasHitTarget()}, remove it and run {@link #hitTargetCallBack} specified by
+ * {@link #withHitTargetCallBack(Runnable)}
+ *
+ *
2. If a game piece {@link #hasHitGround()}, remove it and create a corresponding
+ * {@link GamePieceOnFieldSimulation} using {@link #addGamePieceAfterTouchGround(SimulatedArena)}
+ *
+ *
3. If a game piece {@link #hasGoneOutOfField()}, remove it.
+ */
+ public static void updateGamePieceProjectiles(
+ SimulatedArena simulatedArena, Set gamePieceProjectiles) {
+ final Queue toRemoves = new ArrayBlockingQueue<>(5);
+ for (GamePieceProjectile gamePieceProjectile : gamePieceProjectiles) {
+ if (gamePieceProjectile.hasHitTarget()
+ || gamePieceProjectile.hasHitGround()
+ || gamePieceProjectile.hasGoneOutOfField()) toRemoves.offer(gamePieceProjectile);
+ if (gamePieceProjectile.hasHitTarget() && !gamePieceProjectile.hitTargetCallBackCalled) {
+ gamePieceProjectile.hitTargetCallBack.run();
+ gamePieceProjectile.hitTargetCallBackCalled = true;
+ }
+ if (gamePieceProjectile.hasHitGround()) gamePieceProjectile.addGamePieceAfterTouchGround(simulatedArena);
+ }
+
+ while (!toRemoves.isEmpty()) simulatedArena.removePiece(toRemoves.poll().cleanUp());
+ }
+
+ // The rest are methods to configure a game piece projectile simulation
+
+ /**
+ *
+ *
+ * Configures the Game Piece Projectile to Automatically Become a {@link GamePieceOnFieldSimulation} Upon
+ * Touching Ground.
+ *
+ * This method configures the game piece projectile to transform into a {@link GamePieceOnFieldSimulation} when
+ * it touches the ground.
+ *
+ * @return the current instance of {@link GamePieceProjectile} to allow method chaining
+ */
+ public GamePieceProjectile enableBecomesGamePieceOnFieldAfterTouchGround() {
+ this.becomesGamePieceOnGroundAfterTouchGround = true;
+ return this;
+ }
+
+ /**
+ *
+ *
+ *
Configures the Game Piece Projectile to Disappear Upon Touching Ground.
+ *
+ * Reverts the effect of {@link #enableBecomesGamePieceOnFieldAfterTouchGround()}.
+ */
+ public GamePieceProjectile disableBecomesGamePieceOnFieldAfterTouchGround() {
+ this.becomesGamePieceOnGroundAfterTouchGround = false;
+ return this;
+ }
+
+ /**
+ *
+ *
+ *
Sets a Target for the Game Projectile.
+ *
+ * Configures the {@link #targetPositionSupplier} of this game piece projectile.
+ *
+ *
The method {@link #launch()} will estimate whether or not the game piece will hit the target.
+ *
+ *
After calling {@link #launch()}, {@link #hasHitTarget()} will indicate whether the game piece has already hit
+ * the target.
+ *
+ *
Before calling this method, the target position is 0, 0, -100 (x,y,z), which the projectile will
+ * never hit.
+ *
+ * @param targetPositionSupplier the position of the target, represented as a {@link Translation3d}
+ * @return the current instance of {@link GamePieceProjectile} to allow method chaining
+ */
+ public GamePieceProjectile withTargetPosition(Supplier targetPositionSupplier) {
+ this.targetPositionSupplier = targetPositionSupplier;
+ return this;
+ }
+
+ /**
+ *
+ *
+ * Sets the Target Tolerance for the Game Projectile.
+ *
+ * Configures the {@link #tolerance} for determining whether the game piece has hit the target. The tolerance
+ * defines how close the projectile needs to be to the target for it to be considered a hit.
+ *
+ *
If this method is not called, the default tolerance is 0.2, 0.2, 0.2 (x,y,z)
+ *
+ * @param tolerance the tolerance for the target, represented as a {@link Translation3d}
+ * @return the current instance of {@link GamePieceProjectile} to allow method chaining
+ */
+ public GamePieceProjectile withTargetTolerance(Translation3d tolerance) {
+ this.tolerance = tolerance;
+ return this;
+ }
+
+ /**
+ *
+ *
+ *
Configures a callback to be executed when the game piece hits the target.
+ *
+ * Sets the {@link #hitTargetCallBack} to Execute When the Game Piece Hits the Target.
+ *
+ *
The callback will be triggered when {@link #hasHitTarget()} becomes true.
+ *
+ * @param hitTargetCallBack the callback to run when the game piece hits the target
+ * @return the current instance of {@link GamePieceProjectile} to allow method chaining
+ */
+ public GamePieceProjectile withHitTargetCallBack(Runnable hitTargetCallBack) {
+ this.hitTargetCallBack = hitTargetCallBack;
+ return this;
+ }
+
+ /**
+ *
+ *
+ *
Configures a Callback to Display the Trajectory of the Projectile When Launched.
+ *
+ * Sets the {@link #projectileTrajectoryDisplayCallBackHitTarget} to be fed with data during the
+ * {@link #launch()} method.
+ *
+ *
A {@link List} containing up to 50 {@link Pose3d} objects will be passed to the callback, representing the
+ * future trajectory of the projectile.
+ *
+ *
This is usually for visualizing the trajectory of the projectile on a telemetry, like Advantage Scope
+ *
+ * @param projectileTrajectoryDisplayCallBack the callback that will receive the list of {@link Pose3d} objects
+ * representing the projectile's trajectory
+ * @return the current instance of {@link GamePieceProjectile} to allow method chaining
+ */
+ public GamePieceProjectile withProjectileTrajectoryDisplayCallBack(
+ Consumer> projectileTrajectoryDisplayCallBack) {
+ this.projectileTrajectoryDisplayCallBackMiss =
+ this.projectileTrajectoryDisplayCallBackHitTarget = projectileTrajectoryDisplayCallBack;
+ return this;
+ }
+
+ /**
+ *
+ *
+ * Configures a Callback to Display the Trajectory of the Projectile When Launched.
+ *
+ * Sets the {@link #projectileTrajectoryDisplayCallBackHitTarget} to be fed with data during the
+ * {@link #launch()} method.
+ *
+ *
A {@link List} containing up to 50 {@link Pose3d} objects will be passed to the callback, representing the
+ * future trajectory of the projectile.
+ *
+ *
This is usually for visualizing the trajectory of the projectile on a telemetry, like Advantage Scope
+ *
+ * @param projectileTrajectoryDisplayCallBackHitTarget the callback that will receive the list of {@link Pose3d}
+ * objects representing the projectile's trajectory, called if the projectile will hit the target on its path
+ * @param projectileTrajectoryDisplayCallBackHitTargetMiss the callback that will receive the list of {@link Pose3d}
+ * objects representing the projectile's trajectory, called if the projectile will be off the target
+ * @return the current instance of {@link GamePieceProjectile} to allow method chaining
+ */
+ public GamePieceProjectile withProjectileTrajectoryDisplayCallBack(
+ Consumer> projectileTrajectoryDisplayCallBackHitTarget,
+ Consumer> projectileTrajectoryDisplayCallBackHitTargetMiss) {
+ this.projectileTrajectoryDisplayCallBackHitTarget = projectileTrajectoryDisplayCallBackHitTarget;
+ this.projectileTrajectoryDisplayCallBackMiss = projectileTrajectoryDisplayCallBackHitTargetMiss;
+ return this;
+ }
+
+ /**
+ *
+ *
+ * Configures the Height at Which the Projectile Is Considered to Be Touching Ground.
+ *
+ * Sets the {@link #heightAsTouchGround}, defining the height at which the projectile is considered to have
+ * landed.
+ *
+ *
When the game piece is below this height, it will either be deleted or, if configured, transformed into a
+ * {@link GamePieceOnFieldSimulation} using {@link #enableBecomesGamePieceOnFieldAfterTouchGround()}.
+ *
+ * @param heightAsTouchGround the height (in meters) at which the projectile is considered to touch the ground
+ * @return the current instance of {@link GamePieceProjectile} to allow method chaining
+ */
+ public GamePieceProjectile withTouchGroundHeight(double heightAsTouchGround) {
+ this.heightAsTouchGround = heightAsTouchGround;
+ return this;
+ }
+
+ /**
+ *
+ *
+ *
Configures the position of the robot (not the shooter) at the time of launching the game piece.
+ *
+ * @param initialPosition the position of the robot (not the shooter) at the time of launching the game piece
+ * @return the current instance of {@link GamePieceProjectile} to allow method chaining
+ */
+ public GamePieceProjectile replaceRobotPosition(Translation2d initialPosition) {
+ this.initialPosition = initialPosition;
+ return this;
+ }
+
+ @Override
+ public String getType() {
+ return this.gamePieceType;
+ }
+
+ @Override
+ public boolean isGrounded() {
+ return false;
+ }
+
+}
diff --git a/yagsl/java/swervelib/simulation/ironmaple/simulation/motorsims/MapleMotorSim.java b/yagsl/java/swervelib/simulation/ironmaple/simulation/motorsims/MapleMotorSim.java
new file mode 100644
index 0000000..7aa263b
--- /dev/null
+++ b/yagsl/java/swervelib/simulation/ironmaple/simulation/motorsims/MapleMotorSim.java
@@ -0,0 +1,194 @@
+package swervelib.simulation.ironmaple.simulation.motorsims;
+
+import edu.wpi.first.units.measure.*;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+
+import static edu.wpi.first.units.Units.*;
+
+/**
+ *
+ *
+ * {@link DCMotorSim} with a bit of extra spice.
+ *
+ * This class extends the functionality of the original {@link DCMotorSim} and
+ * models the following aspects in addition:
+ *
+ *
+ * - Motor Controller Closed Loops.
+ *
- Smart current limiting.
+ *
- Friction force on the rotor.
+ *
+ */
+public class MapleMotorSim {
+ private final SimMotorConfigs configs;
+
+ private SimMotorState state;
+ private SimulatedMotorController controller;
+ private Voltage appliedVoltage;
+ private Current statorCurrent;
+
+ /**
+ *
+ *
+ * Constructs a Brushless Motor Simulation Instance.
+ *
+ * @param configs the configuration for this motor
+ */
+ public MapleMotorSim(SimMotorConfigs configs) {
+ this.configs = configs;
+ this.state = new SimMotorState(Radians.zero(), RadiansPerSecond.zero());
+ this.controller = (mechanismAngle, mechanismVelocity, encoderAngle, encoderVelocity) -> Volts.of(0);
+ this.appliedVoltage = Volts.zero();
+ this.statorCurrent = Amps.zero();
+
+ SimulatedBattery.addMotor(this);
+ }
+
+ /**
+ *
+ *
+ * Updates the simulation.
+ *
+ * This is equivalent to{@link DCMotorSim#update(double)}.
+ */
+ public void update(Time dt) {
+ this.appliedVoltage = controller.updateControlSignal(
+ state.mechanismAngularPosition,
+ state.mechanismAngularVelocity,
+ state.mechanismAngularPosition.times(configs.gearing),
+ state.mechanismAngularVelocity.times(configs.gearing));
+ this.appliedVoltage = SimulatedBattery.clamp(appliedVoltage);
+ this.statorCurrent = configs.calculateCurrent(state.mechanismAngularVelocity, appliedVoltage);
+ this.state.step(configs.calculateTorque(statorCurrent), configs.friction, configs.loadMOI, dt);
+
+ if (state.mechanismAngularPosition.lte(configs.reverseHardwareLimit))
+ state = new SimMotorState(configs.reverseHardwareLimit, RadiansPerSecond.zero());
+ else if (state.mechanismAngularPosition.gte(configs.forwardHardwareLimit))
+ state = new SimMotorState(configs.forwardHardwareLimit, RadiansPerSecond.zero());
+ }
+
+ public T useMotorController(T motorController) {
+ this.controller = motorController;
+ return motorController;
+ }
+
+ public SimulatedMotorController.GenericMotorController useSimpleDCMotorController() {
+ return useMotorController(new SimulatedMotorController.GenericMotorController(configs.motor));
+ }
+
+ /**
+ *
+ *
+ * Obtains the final position of the mechanism.
+ *
+ * This is equivalent to {@link DCMotorSim#getAngularPosition()}.
+ *
+ * @return the angular position of the mechanism, continuous
+ */
+ public Angle getAngularPosition() {
+ return state.mechanismAngularPosition;
+ }
+
+ /**
+ *
+ *
+ *
Obtains the angular position measured by the relative encoder of the motor.
+ *
+ * @return the angular position measured by the encoder, continuous
+ */
+ public Angle getEncoderPosition() {
+ return getAngularPosition().times(configs.gearing);
+ }
+
+ /**
+ *
+ *
+ * Obtains the final velocity of the mechanism.
+ *
+ * This is equivalent to {@link DCMotorSim#getAngularVelocity()}.
+ *
+ * @return the final angular velocity of the mechanism
+ */
+ public AngularVelocity getVelocity() {
+ return state.mechanismAngularVelocity;
+ }
+
+ /**
+ *
+ *
+ *
Obtains the angular velocity measured by the relative encoder of the motor.
+ *
+ * @return the angular velocity measured by the encoder
+ */
+ public AngularVelocity getEncoderVelocity() {
+ return getVelocity().times(configs.gearing);
+ }
+
+ /**
+ *
+ *
+ * Obtains the applied voltage by the motor controller.
+ *
+ * The applied voltage is calculated by the motor controller in the previous call to {@link #update(Time)}
+ *
+ *
The motor controller specified by {@link #useMotorController(SimulatedMotorController)} is used to calculate
+ * the applied voltage.
+ *
+ *
The applied voltage is also restricted for current limit and battery voltage.
+ *
+ * @return the applied voltage
+ */
+ public Voltage getAppliedVoltage() {
+ return appliedVoltage;
+ }
+
+ /**
+ *
+ *
+ *
Obtains the stator current.
+ *
+ * This is equivalent to {@link DCMotorSim#getCurrentDrawAmps()}
+ *
+ * @return the stator current of the motor
+ */
+ public Current getStatorCurrent() {
+ return statorCurrent;
+ }
+
+ /**
+ *
+ *
+ *
Obtains the supply current.
+ *
+ * The supply current is different from the stator current, as described here.
+ *
+ * @return the supply current of the motor
+ */
+ public Current getSupplyCurrent() {
+ // Supply Power = Stator Power (Conservation of Energy)
+ // Hence,
+ // Battery Voltage x Supply Current = Applied Voltage x Stator Current
+ // Supply Current = Stator Current * Applied Voltage / Battery Voltage
+ return getStatorCurrent().times(appliedVoltage.div(SimulatedBattery.getBatteryVoltage()));
+ }
+
+ /**
+ *
+ *
+ *
Obtains the configuration of the motor.
+ *
+ * You can modify the configuration of this motor by:
+ *
+ *
+ * mapleMotorSim.getConfigs()
+ * .with...(...)
+ * .with...(...);
+ *
+ *
+ * @return the configuration of the motor
+ */
+ public SimMotorConfigs getConfigs() {
+ return this.configs;
+ }
+}
diff --git a/yagsl/java/swervelib/simulation/ironmaple/simulation/motorsims/SimMotorConfigs.java b/yagsl/java/swervelib/simulation/ironmaple/simulation/motorsims/SimMotorConfigs.java
new file mode 100644
index 0000000..5084e59
--- /dev/null
+++ b/yagsl/java/swervelib/simulation/ironmaple/simulation/motorsims/SimMotorConfigs.java
@@ -0,0 +1,202 @@
+package swervelib.simulation.ironmaple.simulation.motorsims;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.units.measure.*;
+
+import static edu.wpi.first.units.Units.*;
+
+/**
+ *
+ *
+ * Stores the configurations of the motor.
+ *
+ * This class encapsulates the various configuration parameters required to simulate and control a motor in a system.
+ * The configurations include:
+ *
+ *
+ * - motor: The motor model used in the simulation (e.g., Falcon 500, NEO).
+ *
- gearing: The gear ratio between the motor and the load, affecting the output torque and speed.
+ *
- loadMOI: The moment of inertia (MOI) of the load connected to the motor, which determines the
+ * resistance to changes in rotational speed.
+ *
- friction: The torque friction characteristics applied to the motor's simulation, representing
+ * real-world losses.
+ *
- positionVoltageController: PID controller for controlling the motor's position via voltage.
+ *
- velocityVoltageController: PID controller for controlling the motor's velocity via voltage.
+ *
- positionCurrentController: PID controller for controlling the motor's position via current.
+ *
- velocityCurrentController: PID controller for controlling the motor's velocity via current.
+ *
- feedforward: A feedforward controller used to compensate for the desired motor behavior based
+ * on input speeds.
+ *
- forwardHardwareLimit: The forward limit for motor rotation, specified in angle units.
+ *
- reverseHardwareLimit: The reverse limit for motor rotation, specified in angle units.
+ *
- currentLimit: The current limit applied to the motor to protect it from overcurrent
+ * conditions.
+ *
+ */
+public final class SimMotorConfigs {
+ public final DCMotor motor;
+ public final double gearing;
+ public final MomentOfInertia loadMOI;
+ public final Torque friction;
+
+ Angle forwardHardwareLimit, reverseHardwareLimit;
+
+ /**
+ *
+ *
+ * Constructs a simulated motor configuration.
+ *
+ * This constructor initializes a {@link SimMotorConfigs} object with the necessary parameters for motor
+ * simulation, including the motor model, gearing ratio, load moment of inertia, and friction characteristics.
+ *
+ * @param motor the motor model to be used in the simulation (e.g., Falcon 500, NEO).
+ * @param gearing the gear ratio between the motor and the load, affecting torque and speed output.
+ * @param loadMOI the moment of inertia of the load connected to the motor, representing rotational resistance.
+ * @param frictionVoltage the voltage applied to simulate frictional torque losses in the motor.
+ */
+ public SimMotorConfigs(DCMotor motor, double gearing, MomentOfInertia loadMOI, Voltage frictionVoltage) {
+ this.motor = motor;
+ this.gearing = gearing;
+ this.loadMOI = loadMOI;
+ this.friction = NewtonMeters.of(motor.getTorque(motor.getCurrent(0, frictionVoltage.in(Volts))));
+
+ forwardHardwareLimit = Radians.of(Double.POSITIVE_INFINITY);
+ reverseHardwareLimit = Radians.of(-Double.POSITIVE_INFINITY);
+ }
+
+ /**
+ *
+ *
+ *
Calculates the voltage of the motor.
+ *
+ * This method uses the {@link DCMotor} model to find the voltage for a given current and angular velocity.
+ *
+ * @param current the current flowing through the motor
+ * @param mechanismVelocity the final angular velocity of the mechanism
+ * @return the voltage required for the motor to achieve the specified current and angular velocity
+ * @see DCMotor#getVoltage(double, double) for the underlying implementation.
+ */
+ public Voltage calculateVoltage(Current current, AngularVelocity mechanismVelocity) {
+ return Volts.of(motor.getVoltage(current.in(Amps), mechanismVelocity.in(RadiansPerSecond) * gearing));
+ }
+
+ /**
+ *
+ *
+ *
Calculates the velocity of the motor.
+ *
+ * This method uses the {@link DCMotor} model to find the angular velocity for a given current and voltage.
+ *
+ * @param current the current flowing through the motor.
+ * @param voltage the voltage applied to the motor.
+ * @return the final angular velocity of the mechanism.
+ * @see DCMotor#getSpeed(double, double) for the underlying implementation.
+ */
+ public AngularVelocity calculateMechanismVelocity(Current current, Voltage voltage) {
+ return RadiansPerSecond.of(motor.getSpeed(motor.getTorque(current.in(Amps)), voltage.in(Volts)))
+ .div(gearing);
+ }
+
+ /**
+ *
+ *
+ *
Calculates the current of the motor.
+ *
+ * This method uses the {@link DCMotor} model to find the current for a given angular velocity and voltage.
+ *
+ * @param mechanismVelocity the final angular velocity of the mechanism.
+ * @param voltage the voltage applied to the moto.
+ * @return the current drawn by the motor.
+ * @see DCMotor#getCurrent(double, double) for the underlying implementation.
+ */
+ public Current calculateCurrent(AngularVelocity mechanismVelocity, Voltage voltage) {
+ return Amps.of(motor.getCurrent(mechanismVelocity.in(RadiansPerSecond) * gearing, voltage.in(Volts)));
+ }
+
+ /**
+ *
+ *
+ *
Calculates the current based on the motor's torque.
+ *
+ * This method uses the {@link DCMotor} model to find the current required for a given torque.
+ *
+ * @param torque the final torque generated by the motor on the mechanism.
+ * @return the current required to produce the specified torque.
+ * @see DCMotor#getCurrent(double) for the underlying implementation.
+ */
+ public Current calculateCurrent(Torque torque) {
+ return Amps.of(motor.getCurrent(torque.in(NewtonMeters) / gearing));
+ }
+
+ /**
+ *
+ *
+ *
Calculates the torque based on the motor's current.
+ *
+ * This method uses the {@link DCMotor} model to find the torque generated by a given current.
+ *
+ * @param current the current flowing through the motor.
+ * @return the torque generated by the motor.
+ * @see DCMotor#getTorque(double) for the underlying implementation.
+ */
+ public Torque calculateTorque(Current current) {
+ return NewtonMeters.of(motor.getTorque(current.in(Amps)) * gearing);
+ }
+
+ /**
+ *
+ *
+ *
Configures the hard limits for the motor.
+ *
+ * This method sets the hardware limits for the motor's movement. When either the forward or reverse limit is
+ * reached, the motor will be physically restricted from moving beyond that point, based on the motor's hardware
+ * constraints.
+ *
+ * @param forwardLimit the forward hardware limit angle, beyond which the motor cannot move
+ * @param reverseLimit the reverse hardware limit angle, beyond which the motor cannot move
+ * @return this instance for method chaining
+ */
+ public SimMotorConfigs withHardLimits(Angle forwardLimit, Angle reverseLimit) {
+ this.forwardHardwareLimit = forwardLimit;
+ this.reverseHardwareLimit = reverseLimit;
+ return this;
+ }
+
+ public AngularVelocity freeSpinMechanismVelocity() {
+ return RadiansPerSecond.of(motor.freeSpeedRadPerSec / gearing);
+ }
+
+ public Current freeSpinCurrent() {
+ return Amps.of(motor.freeCurrentAmps);
+ }
+
+ public Current stallCurrent() {
+ return Amps.of(motor.stallCurrentAmps);
+ }
+
+ public Torque stallTorque() {
+ return NewtonMeters.of(motor.stallTorqueNewtonMeters);
+ }
+
+ public Voltage nominalVoltage() {
+ return Volts.of(motor.nominalVoltageVolts);
+ }
+
+ @Override
+ protected SimMotorConfigs clone() {
+ SimMotorConfigs cfg = new SimMotorConfigs(
+ motor, gearing, loadMOI, Volts.of(motor.getVoltage(friction.in(NewtonMeter), 0.0)))
+ .withHardLimits(forwardHardwareLimit, reverseHardwareLimit);
+
+ return cfg;
+ }
+
+ @Override
+ public String toString() {
+ return "SimMotorConfigs {"
+ + "\n motor = " + motor // Relies on DCMotor.toString() for details
+ + "\n gearing = " + gearing
+ + "\n loadMOI (kg·m^2) = " + loadMOI.in(KilogramSquareMeters)
+ + "\n friction (N·m) = " + friction.in(NewtonMeters)
+ + "\n}";
+ }
+}
diff --git a/yagsl/java/swervelib/simulation/ironmaple/simulation/motorsims/SimMotorState.java b/yagsl/java/swervelib/simulation/ironmaple/simulation/motorsims/SimMotorState.java
new file mode 100644
index 0000000..f912cb3
--- /dev/null
+++ b/yagsl/java/swervelib/simulation/ironmaple/simulation/motorsims/SimMotorState.java
@@ -0,0 +1,95 @@
+package swervelib.simulation.ironmaple.simulation.motorsims;
+
+import edu.wpi.first.units.measure.*;
+
+import static edu.wpi.first.units.Units.*;
+
+/**
+ *
+ *
+ *
Represents the state of a simulated motor at a given point in time.
+ *
+ * This record holds the final angular position and velocity of the motor. It is used to track the motor's state
+ * during each simulation step.
+ */
+public class SimMotorState {
+ public Angle mechanismAngularPosition;
+ public AngularVelocity mechanismAngularVelocity;
+
+ /**
+ *
+ *
+ *
Constructs a new sim motor state with initial angle and velocity
+ *
+ * @param mechanismAngularPosition the final angular position of the motor, in radians
+ * @param mechanismAngularVelocity the final angular velocity of the motor, in radians per second
+ */
+ public SimMotorState(Angle mechanismAngularPosition, AngularVelocity mechanismAngularVelocity) {
+ this.mechanismAngularPosition = mechanismAngularPosition;
+ this.mechanismAngularVelocity = mechanismAngularVelocity;
+ }
+
+ /**
+ *
+ *
+ * Simulates a step in the motor's motion based on the applied forces.
+ *
+ * This method calculates the new angular position and velocity of the motor after applying electric and
+ * frictional torques over a time step.
+ *
+ *
The method follows these steps:
+ *
+ *
+ * - Convert all units to SI units for calculation.
+ *
- Apply the electric torque to the current angular velocity.
+ *
- Compute the change in angular velocity due to friction.
+ *
- If friction reverses the direction of angular velocity, the velocity is set to zero.
+ *
- Integrate the angular velocity to find the new position.
+ *
+ *
+ * @param finalElectricTorque the final applied electric torque, in Newton-meters
+ * @param finalFrictionTorque the final frictional torque, in Newton-meters
+ * @param loadMOI the moment of inertia of the load, in kilogram square meters
+ * @param dt the time step for the simulation, in seconds
+ */
+ public void step(Torque finalElectricTorque, Torque finalFrictionTorque, MomentOfInertia loadMOI, Time dt) {
+ // Step 0: Convert all units to SI units (radians, radians per second, Newton-meters, seconds, kg*m²)
+ double currentAngularPositionRadians = mechanismAngularPosition.in(Radians);
+ double currentAngularVelocityRadiansPerSecond = mechanismAngularVelocity.in(RadiansPerSecond);
+ final double electricTorqueNewtonsMeters = finalElectricTorque.in(NewtonMeters);
+ final double frictionTorqueNewtonsMeters = finalFrictionTorque.in(NewtonMeters);
+ final double loadMOIKgMetersSquared = loadMOI.in(KilogramSquareMeters);
+ final double dtSeconds = dt.in(Seconds);
+
+ // Step 1: Apply electric torque to the angular velocity.
+ // The torque causes a change in the angular velocity, according to the moment of inertia.
+ currentAngularVelocityRadiansPerSecond += electricTorqueNewtonsMeters / loadMOIKgMetersSquared * dtSeconds;
+
+ // Step 2: Calculate the change in angular velocity due to friction.
+ // Friction opposes the motion and reduces the angular velocity over time.
+ final double deltaAngularVelocityDueToFrictionRadPerSec =
+ Math.copySign(frictionTorqueNewtonsMeters, -currentAngularVelocityRadiansPerSecond)
+ / loadMOIKgMetersSquared
+ * dtSeconds;
+
+ // Step 3: Check if the angular velocity changes direction due to friction, or if it reaches zero.
+ // If friction causes the motor to reverse direction, or if the velocity reaches zero, set the angular velocity
+ // to zero.
+ if ((currentAngularVelocityRadiansPerSecond + deltaAngularVelocityDueToFrictionRadPerSec)
+ * currentAngularVelocityRadiansPerSecond
+ <= 0)
+ // The velocity has reversed direction or reached zero, so stop the motor
+ currentAngularVelocityRadiansPerSecond = 0;
+ else
+ // Otherwise, apply the change due to friction
+ currentAngularVelocityRadiansPerSecond += deltaAngularVelocityDueToFrictionRadPerSec;
+
+ // Step 4: Integrate angular velocity to find the new position.
+ // The new angular position is the current position plus the change in position over the time step.
+ currentAngularPositionRadians += currentAngularVelocityRadiansPerSecond * dtSeconds;
+
+ // Return a new instance with the updated angular position and velocity
+ this.mechanismAngularPosition = Radians.of(currentAngularPositionRadians);
+ this.mechanismAngularVelocity = RadiansPerSecond.of(currentAngularVelocityRadiansPerSecond);
+ }
+}
diff --git a/yagsl/java/swervelib/simulation/ironmaple/simulation/motorsims/SimulatedBattery.java b/yagsl/java/swervelib/simulation/ironmaple/simulation/motorsims/SimulatedBattery.java
new file mode 100644
index 0000000..0c4ed27
--- /dev/null
+++ b/yagsl/java/swervelib/simulation/ironmaple/simulation/motorsims/SimulatedBattery.java
@@ -0,0 +1,153 @@
+package swervelib.simulation.ironmaple.simulation.motorsims;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.filter.LinearFilter;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.simulation.BatterySim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+import java.util.ArrayList;
+import java.util.List;
+import java.util.function.Supplier;
+
+import static edu.wpi.first.units.Units.Amps;
+import static edu.wpi.first.units.Units.Volts;
+
+/**
+ *
+ *
+ * Simulates the main battery of the robot.
+ *
+ * This class simulates the behavior of a robot's battery. Electrical appliances can be added to the battery to draw
+ * current. The battery voltage is affected by the current drawn from various appliances.
+ */
+public class SimulatedBattery {
+ // Nominal voltage for a fully charged battery (13.5 volts).
+ private static final double BATTERY_NOMINAL_VOLTAGE = 13.5;
+
+ // Filter to smooth the current readings.
+ private static final LinearFilter currentFilter = LinearFilter.movingAverage(50);
+
+ private static final List> electricalAppliances = new ArrayList<>();
+
+ // The current battery voltage in volts.
+ private static double batteryVoltageVolts = BATTERY_NOMINAL_VOLTAGE;
+
+ private static boolean disableBatterySim = false;
+
+ /**
+ * Disables the battery simulation. This is a lazy quick fix to help the opponent simulation.
+ */
+ public static void disableBatterySim() {
+ disableBatterySim = true;
+ electricalAppliances.clear();
+ batteryVoltageVolts = BATTERY_NOMINAL_VOLTAGE;
+ }
+
+ /**
+ *
+ *
+ * Adds a custom electrical appliance.
+ *
+ * Connects the electrical appliance to the battery, allowing it to draw current from the battery.
+ *
+ * @param customElectricalAppliances The supplier for the current drawn by the appliance.
+ */
+ public static void addElectricalAppliances(Supplier customElectricalAppliances) {
+ electricalAppliances.add(customElectricalAppliances);
+ }
+
+ /**
+ *
+ *
+ * Adds a motor to the list of electrical appliances.
+ *
+ * The motor will draw current from the battery.
+ *
+ * @param mapleMotorSim The motor simulation object.
+ */
+ public static void addMotor(MapleMotorSim mapleMotorSim) {
+ electricalAppliances.add(mapleMotorSim::getSupplyCurrent);
+ }
+
+ /**
+ *
+ *
+ *
Updates the battery simulation.
+ *
+ * Calculates the battery voltage based on the current drawn by all appliances.
+ *
+ *
The battery voltage is clamped to avoid going below the brownout voltage.
+ */
+ public static void simulationSubTick() {
+ double totalCurrentAmps = getTotalCurrentDrawn().in(Amps);
+ totalCurrentAmps = currentFilter.calculate(totalCurrentAmps);
+
+ if (Double.isNaN(batteryVoltageVolts)) {
+ batteryVoltageVolts = 12.0;
+ DriverStation.reportError(
+ "[MapleSim] Internal Library Error: Calculated battery voltage is invalid"
+ + ", reverting to normal operation voltage...",
+ false);
+ }
+ if (batteryVoltageVolts < RoboRioSim.getBrownoutVoltage()) {
+ batteryVoltageVolts = RoboRioSim.getBrownoutVoltage();
+ DriverStation.reportError("[MapleSim] BrownOut Detected, protecting battery voltage...", false);
+ }
+
+ /// Quick fix to lock battery simulation to nominal voltage.
+ if (!disableBatterySim) {
+ batteryVoltageVolts =
+ BatterySim.calculateLoadedBatteryVoltage(BATTERY_NOMINAL_VOLTAGE, 0.02, totalCurrentAmps);
+ }
+
+ RoboRioSim.setVInVoltage(batteryVoltageVolts);
+ SmartDashboard.putNumber("BatterySim/TotalCurrent (Amps)", totalCurrentAmps);
+ SmartDashboard.putNumber("BatterySim/BatteryVoltage (Volts)", batteryVoltageVolts);
+ }
+
+ /**
+ *
+ *
+ *
Obtains the voltage of the battery.
+ *
+ * @return The battery voltage as a {@link Voltage} object.
+ */
+ public static Voltage getBatteryVoltage() {
+ return Volts.of(batteryVoltageVolts);
+ }
+
+ /**
+ *
+ *
+ * Obtains the total current drawn from the battery.
+ *
+ * Iterates through all the appliances to obtain the total current used.
+ *
+ * @return The total current as a {@link Current} object.
+ */
+ public static Current getTotalCurrentDrawn() {
+ double totalCurrentAmps = electricalAppliances.stream()
+ .mapToDouble(currentSupplier -> currentSupplier.get().in(Amps))
+ .sum();
+ return Amps.of(totalCurrentAmps);
+ }
+
+ /**
+ *
+ *
+ *
Clamps the voltage according to the supplied voltage and the battery's capabilities.
+ *
+ * If the supplied voltage exceeds the battery's maximum voltage, it will be reduced to match the battery's
+ * voltage.
+ *
+ * @param voltage The voltage to be clamped.
+ * @return The clamped voltage as a {@link Voltage} object.
+ */
+ public static Voltage clamp(Voltage voltage) {
+ return Volts.of(MathUtil.clamp(voltage.in(Volts), -batteryVoltageVolts, batteryVoltageVolts));
+ }
+}
diff --git a/yagsl/java/swervelib/simulation/ironmaple/simulation/motorsims/SimulatedMotorController.java b/yagsl/java/swervelib/simulation/ironmaple/simulation/motorsims/SimulatedMotorController.java
new file mode 100644
index 0000000..381209b
--- /dev/null
+++ b/yagsl/java/swervelib/simulation/ironmaple/simulation/motorsims/SimulatedMotorController.java
@@ -0,0 +1,108 @@
+package swervelib.simulation.ironmaple.simulation.motorsims;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Voltage;
+
+import static edu.wpi.first.units.Units.*;
+
+public interface SimulatedMotorController {
+ Voltage updateControlSignal(
+ Angle mechanismAngle,
+ AngularVelocity mechanismVelocity,
+ Angle encoderAngle,
+ AngularVelocity encoderVelocity);
+
+ final class GenericMotorController implements SimulatedMotorController {
+ private final DCMotor model;
+ private Current currentLimit = Amps.of(150);
+ private Angle forwardSoftwareLimit = Radians.of(Double.POSITIVE_INFINITY),
+ reverseSoftwareLimit = Radians.of(-Double.POSITIVE_INFINITY);
+
+ private Voltage requestedVoltage = Volts.zero();
+ private Voltage appliedVoltage = Volts.zero();
+
+ public GenericMotorController(DCMotor model) {
+ this.model = model;
+ }
+
+ public GenericMotorController withCurrentLimit(Current currentLimit) {
+ this.currentLimit = currentLimit;
+ return this;
+ }
+
+ public GenericMotorController withSoftwareLimits(Angle forwardSoftwareLimit, Angle reverseSoftwareLimit) {
+ this.forwardSoftwareLimit = forwardSoftwareLimit;
+ this.reverseSoftwareLimit = reverseSoftwareLimit;
+ return this;
+ }
+
+ public void requestVoltage(Voltage voltage) {
+ this.requestedVoltage = voltage;
+ }
+
+ /**
+ *
+ *
+ *
(Utility Function) Constrains the Output Voltage of a Motor.
+ *
+ * Constrains the output voltage of a motor such that the stator current does not exceed the
+ * current limit
+ *
+ *
Prevents motor from exceeding software limits
+ *
+ * @param encoderAngle the angle of the encoder
+ * @param encoderVelocity the velocity of the encoder
+ * @param requestedVoltage the requested voltage
+ * @return the constrained voltage that satisfied the limits
+ */
+ public Voltage constrainOutputVoltage(
+ Angle encoderAngle, AngularVelocity encoderVelocity, Voltage requestedVoltage) {
+ // Don't use WPILib Units
+ final double motorCurrentVelocityRadPerSec = encoderVelocity.in(RadiansPerSecond);
+ final double requestedOutputVoltageVolts = requestedVoltage.in(Volts);
+ final double currentLimitAmps = currentLimit.in(Amps);
+ final double kCurrentThreshold = 1.2;
+ final double thresholdedCurrentLimitAmps = kCurrentThreshold * currentLimitAmps;
+
+ final double currentAtRequestedVoltageAmps =
+ model.getCurrent(motorCurrentVelocityRadPerSec, requestedOutputVoltageVolts);
+
+ double limitedVoltage = requestedOutputVoltageVolts;
+
+ // Resource for current limiting:
+ // https://file.tavsys.net/control/controls-engineering-in-frc.pdf (sec 12.1.3)
+ if (Math.abs(currentAtRequestedVoltageAmps) > thresholdedCurrentLimitAmps) {
+ final double limitedCurrent = Math.copySign(currentLimitAmps, currentAtRequestedVoltageAmps);
+ limitedVoltage = model.getVoltage(model.getTorque(limitedCurrent), motorCurrentVelocityRadPerSec);
+
+ // Ensure the current limit doesn't cause an increase to output voltage
+ if (Math.abs(limitedVoltage) > Math.abs(requestedOutputVoltageVolts))
+ limitedVoltage = requestedOutputVoltageVolts;
+ }
+
+ // Apply software limits
+ if (limitedVoltage > 0 && encoderAngle.gte(forwardSoftwareLimit)) limitedVoltage = 0;
+ if (limitedVoltage < 0 && encoderAngle.lte(reverseSoftwareLimit)) limitedVoltage = 0;
+
+ // Constrain the output voltage to the battery voltage
+ return Volts.of(limitedVoltage);
+ }
+
+ @Override
+ public Voltage updateControlSignal(
+ Angle mechanismAngle,
+ AngularVelocity mechanismVelocity,
+ Angle encoderAngle,
+ AngularVelocity encoderVelocity) {
+ appliedVoltage = constrainOutputVoltage(encoderAngle, encoderVelocity, requestedVoltage);
+ return appliedVoltage;
+ }
+
+ public Voltage getAppliedVoltage() {
+ return appliedVoltage;
+ }
+ }
+}
diff --git a/yagsl/java/swervelib/simulation/ironmaple/simulation/opponents/EmptyOpponent.java b/yagsl/java/swervelib/simulation/ironmaple/simulation/opponents/EmptyOpponent.java
new file mode 100644
index 0000000..ff7a32b
--- /dev/null
+++ b/yagsl/java/swervelib/simulation/ironmaple/simulation/opponents/EmptyOpponent.java
@@ -0,0 +1,131 @@
+package swervelib.simulation.ironmaple.simulation.opponents;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import swervelib.simulation.ironmaple.simulation.opponentsim.OpponentManager;
+import swervelib.simulation.ironmaple.simulation.opponentsim.SmartOpponent;
+import swervelib.simulation.ironmaple.simulation.opponentsim.SmartOpponentConfig;
+
+import java.util.Optional;
+import java.util.Set;
+import java.util.function.Supplier;
+
+import static edu.wpi.first.units.Units.*;
+
+public class EmptyOpponent extends SmartOpponent {
+ private Optional> defenseTarget = Optional.empty();
+ /**
+ * A Dumb SmartOpponent.
+ * Can only do default SmartOpponent things. Non Season Specific
+ * Loads with default initial starting and queening poses. Set manually for more than one bot.
+ * Check {@link OpponentManager} for some poses.
+ *
+ * @param name the opponent name. Typically just "Defense Bot 1".
+ * Names should not be the same.
+ * @param alliance the opponents {@link DriverStation.Alliance}.
+ */
+ public EmptyOpponent(String name, DriverStation.Alliance alliance) {
+ /// All Options should be set in the constructor.
+ super(new SmartOpponentConfig()
+ .withName(name)
+ .withAlliance(alliance)
+ .withQueeningPose(new Pose2d(-6, 0, new Rotation2d()))
+ .withStartingPose(new Pose2d(15, 6, Rotation2d.fromDegrees(180)))
+ .withChassisConfig(SmartOpponentConfig.ChassisConfig.Presets.SimpleSquareChassis.getConfig()
+ .withMaxLinearVelocity(MetersPerSecond.of(6))
+ .withMaxAngularVelocity(DegreesPerSecond.of(360)))
+ .withAutoEnable());
+ }
+
+ /**
+ * The collect state to run.
+ * Does nothing
+ *
+ * @return a runnable that runs the state.
+ */
+ @Override
+ protected Command collectState() {
+ return Commands.none();
+ }
+
+ /**
+ * The score state to run.
+ * Does nothing
+ *
+ * @return a runnable that runs the state.
+ */
+ @Override
+ protected Command scoreState() {
+ return Commands.none();
+ }
+
+ // TODO
+ public EmptyOpponent withXboxController(CommandXboxController xboxController) {
+ config.withJoystick(xboxController);
+ config.withState("Joystick", this::joystickState);
+ config.withBehavior(
+ "Player",
+ startingState("Joystick").andThen(startingState("Joystick").ignoringDisable(false)));
+ config.updateBehaviorChooser();
+ /// Enable Manipulator control
+ xboxController.leftBumper().and(config.isStateTrigger("Joystick")).whileTrue(manipulatorSim.intake("Intake"));
+ xboxController.rightBumper().and(config.isStateTrigger("Joystick")).whileTrue(manipulatorSim.score("Coral"));
+ return this;
+ }
+
+ /**
+ * Defense State to simply follow the defense target if present.
+ * Pretty demanding since it just refreshes a new pathfind constantly.
+ *
+ * @return a {@link Command} to run defense.
+ */
+ private Command defenseState() {
+ return Commands.defer(() ->
+ pathfind(Pair.of("Defense Target", defenseTarget.orElse(this::getOpponentPose).get()), config.chassis.maxLinearVelocity),
+ Set.of(this)).withTimeout(.5).repeatedly();
+ }
+
+ /**
+ * Enables a dumb defense bot to be an annoyance.
+ *
+ * @param defenseTarget the pose to attack.
+ * @return this, for chaining or something.
+ */
+ public EmptyOpponent withDefense(Supplier defenseTarget) {
+ this.defenseTarget = Optional.of(defenseTarget);
+ config.withState("Defense", this::defenseState);
+ config.withBehavior(
+ "Defense",
+ startingState("Defense").ignoringDisable(false));
+ config.updateBehaviorChooser();
+ return this;
+ }
+
+ /**
+ * The joystick state to run. Should be inaccessible when not set.
+ *
+ * @return the joystick state to run.
+ */
+ private Command joystickState() {
+ final CommandXboxController xbox = ((CommandXboxController) config.getJoystick());
+ return drive(
+ () -> new ChassisSpeeds(
+ MathUtil.applyDeadband(
+ xbox.getLeftY() * -config.chassis.maxLinearVelocity.in(MetersPerSecond),
+ config.joystickDeadband),
+ MathUtil.applyDeadband(
+ xbox.getLeftX() * -config.chassis.maxLinearVelocity.in(MetersPerSecond),
+ config.joystickDeadband),
+ MathUtil.applyDeadband(
+ xbox.getRightX() * -config.chassis.maxAngularVelocity.in(RadiansPerSecond),
+ config.joystickDeadband)),
+ false);
+ }
+}
diff --git a/yagsl/java/swervelib/simulation/ironmaple/simulation/opponentsim/ManipulatorSim.java b/yagsl/java/swervelib/simulation/ironmaple/simulation/opponentsim/ManipulatorSim.java
new file mode 100644
index 0000000..5ecd0ea
--- /dev/null
+++ b/yagsl/java/swervelib/simulation/ironmaple/simulation/opponentsim/ManipulatorSim.java
@@ -0,0 +1,135 @@
+package swervelib.simulation.ironmaple.simulation.opponentsim;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import swervelib.simulation.ironmaple.simulation.IntakeSimulation;
+import swervelib.simulation.ironmaple.simulation.SimulatedArena;
+import swervelib.simulation.ironmaple.simulation.gamepieces.GamePieceProjectile;
+
+import java.util.HashMap;
+import java.util.Map;
+import java.util.function.Supplier;
+
+public class ManipulatorSim extends SubsystemBase {
+ /// Simulation Maps of saved manipulators
+ private final Map intakeSimulations;
+ private final Map> projectileSimulations;
+
+ /**
+ * Creates a new manipulator simulation.
+ */
+ public ManipulatorSim() {
+ this.intakeSimulations = new HashMap<>();
+ this.projectileSimulations = new HashMap<>();
+ }
+
+ /**
+ * The Map of {@link IntakeSimulation} types.
+ *
+ * @return a Map of {@link IntakeSimulation} types.
+ */
+ public Map getIntakeSimulations() {
+ return intakeSimulations;
+ }
+
+ /**
+ * The Map of the {@link GamePieceProjectile} types.
+ *
+ * @return a Map of the {@link GamePieceProjectile} types.
+ */
+ public Map> getProjectileSimulations() {
+ return projectileSimulations;
+ }
+
+ /**
+ * Adds an intake simulation to the manipulator simulation.
+ *
+ * @param intakeName The name of the intake simulation.
+ * @param intakeSimulation The simulation to add.
+ * @return this, for chaining.
+ */
+ public ManipulatorSim addIntakeSimulation(String intakeName, IntakeSimulation intakeSimulation) {
+ this.intakeSimulations.put(intakeName, intakeSimulation);
+ return this;
+ }
+
+ /**
+ * Adds a projectile simulation to the manipulator simulation.
+ *
+ * @param projectileName The name of the projectile simulation.
+ * @param projectileSimulation The simulation to add.
+ * @return this, for chaining.
+ */
+ public ManipulatorSim addProjectileSimulation(
+ String projectileName, Supplier projectileSimulation) {
+ this.projectileSimulations.put(projectileName, projectileSimulation);
+ return this;
+ }
+
+ /**
+ * Gets an intake simulation from the manipulator simulation.
+ *
+ * @param intakeName The name of the intake simulation.
+ * @return The simulation.
+ */
+ public IntakeSimulation getIntakeSimulation(String intakeName) {
+ return this.intakeSimulations.get(intakeName);
+ }
+
+ /**
+ * Gets a projectile simulation from the manipulator simulation.
+ *
+ * @param projectileName The name of the simulation.
+ * @return The simulation.
+ */
+ public Supplier getProjectileSimulation(String projectileName) {
+ return this.projectileSimulations.get(projectileName);
+ }
+
+ /**
+ * Runs the intake, stops running the intake when the command ends.
+ *
+ * @param intakeName
+ * @return
+ */
+ public Command intake(String intakeName) {
+ return runEnd(() -> getIntakeSimulation(intakeName).startIntake(),
+ () -> getIntakeSimulation(intakeName).stopIntake());
+ }
+
+ /**
+ * Runs intake(String) until the game piece count in the intake goes up.
+ *
+ * @param intakeName
+ * @return
+ */
+ public Command intakeUntilCollected(String intakeName) {
+ final int count = getIntakeSimulation(intakeName).getGamePiecesAmount();
+ return intake(intakeName)
+ .until(() -> count > getIntakeSimulation(intakeName).getGamePiecesAmount());
+ }
+
+ /**
+ * Adds a projectile to the simulation.
+ *
+ * @param projectileName The name of the projectile simulation.
+ * @return a command to add the game piece projectile to the simulation.
+ */
+ public Command score(String projectileName) {
+ return runOnce(() -> SimulatedArena.getInstance()
+ .addGamePieceProjectile(getProjectileSimulation(projectileName).get()));
+ }
+
+ /**
+ * A command that only calls score(String) when there is greater than zero(>0)
+ * {@link swervelib.simulation.ironmaple.simulation.gamepieces.GamePiece} in the intake.
+ *
+ * @param intakeName the intake to check.
+ * @param projectileName the piece to score.
+ * @return score(String) when valid.
+ */
+ public Command scoreWithIntake(String intakeName, String projectileName) {
+ return score(projectileName)
+ .onlyIf(() -> getIntakeSimulation(intakeName).getGamePiecesAmount() > 0);
+ }
+}
diff --git a/yagsl/java/swervelib/simulation/ironmaple/simulation/opponentsim/OpponentManager.java b/yagsl/java/swervelib/simulation/ironmaple/simulation/opponentsim/OpponentManager.java
new file mode 100644
index 0000000..e7c36c0
--- /dev/null
+++ b/yagsl/java/swervelib/simulation/ironmaple/simulation/opponentsim/OpponentManager.java
@@ -0,0 +1,482 @@
+package swervelib.simulation.ironmaple.simulation.opponentsim;
+
+import com.pathplanner.lib.commands.PathfindingCommand;
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.Time;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import swervelib.simulation.ironmaple.simulation.opponentsim.pathfinding.MapleADStar;
+
+import java.util.*;
+
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.Milliseconds;
+
+public class OpponentManager {
+ // Map of possible scoring poses and types. For example, Map<"Hoops", Map<"CourtLeft", Pose2d>>
+ protected static final Map> scoringMap = new HashMap<>();
+ // Map of possible collecting poses and types. For example, Map<"CollectStation", Map<"StationCenter", Pose2d>>
+ protected static final Map> collectingMap = new HashMap<>();
+ /// Cached opponent data for dynamic polling.
+ // Cached list of all opponent obstacles.
+ protected static final List> opponentObstacles = new ArrayList<>();
+ protected double lastObstaclePoll = 0;
+ protected static final List> blueOpponentObstacles = new ArrayList<>();
+ protected double lastBlueObstaclePoll = 0;
+ protected static final List> redOpponentObstacles = new ArrayList<>();
+ protected double lastRedObstaclePoll = 0;
+ // Cached list of all opponent targets.
+ protected static final List> opponentTargets = new ArrayList<>();
+ protected double lastTargetPoll = 0;
+ protected static final List> blueOpponentTargets = new ArrayList<>();
+ protected double lastBlueTargetPoll = 0;
+ protected static final List> redOpponentTargets = new ArrayList<>();
+ protected double lastRedTargetPoll = 0;
+ // Cached list of all opponent poses.
+ protected static final List opponentPoses = new ArrayList<>();
+ protected double lastPosePoll = 0;
+ protected static final List blueOpponentPoses = new ArrayList<>();
+ protected double lastBluePosePoll = 0;
+ protected static final List redOpponentPoses = new ArrayList<>();
+ protected double lastRedPosePoll = 0;
+ // List of possible starting poses
+ protected static final List initialBluePoses = new ArrayList<>();
+ protected static final List initialRedPoses = new ArrayList<>();
+ // Map of queening poses
+ protected static final List queeningPoses = new ArrayList<>();
+ // Bounding box buffer, used to determine how big to make the obstacles.
+ protected static Distance boundingBoxBuffer;
+ // Bounding box offset Translation2d
+ protected final Translation2d boundingBoxTranslation;
+
+ /// List of all opponent robots.
+ protected static final List opponents = new ArrayList<>();
+
+ /**
+ * MapleSim Opponent currently relies on Pathplanner with a modified pathfinder. This is to be changed soon. ^TM
+ */
+ public OpponentManager() {
+ boundingBoxBuffer = Meters.of(0.6);
+ this.boundingBoxTranslation = new Translation2d(boundingBoxBuffer, boundingBoxBuffer);
+ }
+
+ /**
+ * MapleSim Opponent currently relies on Pathplanner with a modified pathfinder. This is to be changed soon. ^TM
+ *
+ * @param withDefaults whether to call withDefaults or not.
+ */
+ public OpponentManager(boolean withDefaults) {
+ this();
+ if (withDefaults) {
+ withDefaults();
+ }
+ }
+
+ /**
+ * Calls withDefaultQueeningPoses() and withDefaultInitialPoses(). Also warms up pathplanner.
+ *
+ * @return this, for chaining.
+ */
+ public OpponentManager withDefaults() {
+ CommandScheduler.getInstance().schedule(PathfindingCommand.warmupCommand());
+ this
+ .withDefaultInitialPoses()
+ .withDefaultQueeningPoses();
+ return this;
+ }
+
+ /**
+ * Adds 6 default queening poses to the list.
+ *
+ * @return this, for chaining.
+ */
+ public OpponentManager withDefaultQueeningPoses() {
+ /// Add 6 queening poses for 3v3 without any additional
+ queeningPoses.add(new Pose2d(-6, 0, new Rotation2d()));
+ queeningPoses.add(new Pose2d(-5, 0, new Rotation2d()));
+ queeningPoses.add(new Pose2d(-4, 0, new Rotation2d()));
+ queeningPoses.add(new Pose2d(-3, 0, new Rotation2d()));
+ queeningPoses.add(new Pose2d(-2, 0, new Rotation2d()));
+ queeningPoses.add(new Pose2d(-1, 0, new Rotation2d()));
+ return this;
+ }
+
+ /**
+ * Add 6 basic starting positions, 3 on each side.
+ *
+ * @return this, for chaining.
+ */
+ public OpponentManager withDefaultInitialPoses() {
+ /// Add 6 basic starting positions, 3 on each side.
+ initialRedPoses.add(new Pose2d(15, 6, Rotation2d.fromDegrees(180)));
+ initialRedPoses.add(new Pose2d(15, 4, Rotation2d.fromDegrees(180)));
+ initialRedPoses.add(new Pose2d(15, 2, Rotation2d.fromDegrees(180)));
+ initialBluePoses.add(new Pose2d(1.6, 6, Rotation2d.kZero));
+ initialBluePoses.add(new Pose2d(1.6, 4, Rotation2d.kZero));
+ initialBluePoses.add(new Pose2d(1.6, 2, Rotation2d.kZero));
+ return this;
+ }
+
+ /**
+ * Adds an opponent robot to the list of robots to grab things from.
+ *
+ * @param opponent the {@link SmartOpponent} to manage.
+ * @return this, for chaining.
+ */
+ public OpponentManager registerOpponent(SmartOpponent opponent) {
+ opponents.add(opponent);
+ return this;
+ }
+
+ /**
+ * Makes a list of opponents on the given alliance.
+ *
+ * @param alliance the {@link DriverStation.Alliance} to collect opponents from.
+ * @return a {@link List} of the given alliance.
+ */
+ public List getOpponents(DriverStation.Alliance alliance) {
+ List allianceOpponents = new ArrayList<>();
+ for (SmartOpponent opponent : opponents) {
+ if (opponent.config.alliance == alliance) {
+ allianceOpponents.add(opponent);
+ }
+ }
+ return allianceOpponents;
+ }
+
+ /**
+ * Gets the list of all registered SmartOpponents.
+ *
+ * @return the {@link List} of registered opponents.
+ */
+ public List getOpponents() {
+ return opponents;
+ }
+
+ /**
+ * Returns only the opponent targets for the given alliance.
+ *
+ * @param alliance which {@link DriverStation.Alliance} targets to grab.
+ * @return a list of poses targeted by opponents on the given alliance.
+ */
+ public List> getOpponentTargetsDynamic(DriverStation.Alliance alliance, Time pollRate) {
+ // If elapsed time is less than pollRate return our cached list.
+ final var isBlue = alliance == DriverStation.Alliance.Blue;
+ final var targetList = isBlue ? blueOpponentTargets : redOpponentTargets;
+ if (System.currentTimeMillis() - (isBlue ? lastBlueTargetPoll : lastRedTargetPoll) < pollRate.in(Milliseconds)) {
+ return targetList;
+ }
+ // New list to filter.
+ final var filteredOpponents = new ArrayList<>(opponents);
+ // If elapsed time exceeds our pollRate, refresh the list.
+ targetList.clear();
+ filteredOpponents.stream()
+ // Remove all opponents not on given alliance.
+ .filter(opponent -> opponent.config.alliance != alliance)
+ // Collect all their poses.
+ .forEach(opponent -> targetList.add(opponent.getTarget()));
+ // Update our refresh timestamp.
+ if (isBlue) {
+ lastBlueTargetPoll = System.currentTimeMillis();
+ } else {
+ lastRedTargetPoll = System.currentTimeMillis();
+ }
+ return targetList;
+ }
+
+ /**
+ * Dynamically caches and refreshed a list of opponent targets.
+ *
+ * @param pollRate how long to wait before refreshing
+ * @return the list of registered opponent target poses.
+ */
+ public List> getOpponentTargetsDynamic(Time pollRate) {
+ // If elapsed time is less than pollRate return our cached list.
+ if (System.currentTimeMillis() - lastTargetPoll < pollRate.in(Milliseconds)) {
+ return opponentTargets;
+ }
+ // If elapsed time exceeds our pollRate, refresh the list.
+ opponentTargets.clear();
+ // Call the other dynamic methods so we don't update if not needed.
+ opponentTargets.addAll(getOpponentTargetsDynamic(DriverStation.Alliance.Blue, pollRate));
+ opponentTargets.addAll(getOpponentTargetsDynamic(DriverStation.Alliance.Red, pollRate));
+ // Update our refresh timestamp.
+ lastTargetPoll = System.currentTimeMillis();
+ return opponentTargets;
+ }
+
+ /**
+ * Checks if the given pose is near any opponent target pose.
+ *
+ * @param pose the pose to check against.
+ * @param pollRate how long to wait before refreshing
+ * @param tolerance the translation tolerance in {@link Distance}.
+ * @return
+ */
+ public boolean isNearTarget(Pose2d pose, Time pollRate, Distance tolerance) {
+ for (Pair existingTarget : getOpponentTargetsDynamic(pollRate)) {
+ // Check if the new target is within the tolerance distance of any existing target
+ if (Objects.nonNull(existingTarget)) {
+ if (existingTarget.getSecond().getTranslation().getDistance(pose.getTranslation()) < tolerance.in(Meters)) {
+ return true;
+ }
+ }
+ }
+ return false;
+ }
+
+ /**
+ * Returns only the opponent poses for the given alliance.
+ *
+ * @param alliance which {@link DriverStation.Alliance} poses to grab.
+ * @return a list of opponent poses on the given alliance.
+ */
+ public List getOpponentPosesDynamic(DriverStation.Alliance alliance, Time pollRate) {
+ // If elapsed time is less than pollRate return our cached list.
+ final var isBlue = alliance == DriverStation.Alliance.Blue;
+ final var poseList = isBlue ? blueOpponentPoses : redOpponentPoses;
+ if (System.currentTimeMillis() - (isBlue ? lastBluePosePoll : lastRedPosePoll) < pollRate.in(Milliseconds)) {
+ return poseList;
+ }
+ // New list to filter.
+ final var filteredOpponents = new ArrayList<>(opponents);
+ // If elapsed time exceeds our pollRate, refresh the list.
+ poseList.clear();
+ filteredOpponents.stream()
+ // Remove all opponents not on a given alliance.
+ .filter(opponent -> opponent.config.alliance != alliance)
+ // Collect all their poses.
+ .forEach(opponent -> poseList.add(opponent.getOpponentPose()));
+ // Update our refresh timestamp.
+ if (isBlue) {
+ lastBluePosePoll = System.currentTimeMillis();
+ } else {
+ lastRedPosePoll = System.currentTimeMillis();
+ }
+ return poseList;
+ }
+
+ /**
+ * Returns all registered opponent poses.
+ *
+ * @return a list of all opponents poses on the given alliance.
+ */
+ public List getOpponentPosesDynamic(Time pollRate) {
+ // If elapsed time is less than pollRate return our cached list.
+ if (System.currentTimeMillis() - lastPosePoll < pollRate.in(Milliseconds)) {
+ return opponentPoses;
+ }
+ // If elapsed time exceeds our pollRate, refresh the list.
+ opponentPoses.clear();
+ // Call the other dynamic methods so we don't update if not needed.
+ opponentPoses.addAll(getOpponentPosesDynamic(DriverStation.Alliance.Blue, pollRate));
+ opponentPoses.addAll(getOpponentPosesDynamic(DriverStation.Alliance.Red, pollRate));
+ // Update our refresh timestamp.
+ lastPosePoll = System.currentTimeMillis();
+ return opponentPoses;
+ }
+
+ /**
+ * Saves a list globally in {@link OpponentManager} and returns that list only updating it if the last update
+ * exceeds the given time.
+ *
+ * @param pollRate how long to wait before updating.
+ * @return a list of obstacles usable by {@link MapleADStar}.
+ */
+ protected List> getObstaclesDynamic(DriverStation.Alliance alliance, Time pollRate) {
+ // If elapsed time is less than pollRate return our cached list.
+ final var isBlue = alliance == DriverStation.Alliance.Blue;
+ final var obstacleList = isBlue ? blueOpponentObstacles : redOpponentObstacles;
+ if (System.currentTimeMillis() - (isBlue ? lastBlueObstaclePoll : lastRedObstaclePoll) < pollRate.in(Milliseconds)) {
+ return obstacleList;
+ }
+ // New list to filter.
+ final var targets = isBlue
+ ? getOpponentTargetsDynamic(DriverStation.Alliance.Blue, pollRate)
+ : getOpponentTargetsDynamic(DriverStation.Alliance.Red, pollRate);
+ final var poses = isBlue
+ ? getOpponentPosesDynamic(DriverStation.Alliance.Blue, pollRate)
+ : getOpponentPosesDynamic(DriverStation.Alliance.Red, pollRate);
+ obstacleList.clear();
+ // Format poses into obstacles and add them.
+ targets.forEach(target -> {
+ obstacleList.add(poseToObstacle(Objects.requireNonNullElse(target.getSecond(), Pose2d.kZero)));
+ });
+ poses.forEach(pose -> {
+ obstacleList.add(poseToObstacle(pose));
+ });
+ // Update our refresh timestamp.
+ if (isBlue) {
+ lastBlueObstaclePoll = System.currentTimeMillis();
+ } else {
+ lastRedObstaclePoll = System.currentTimeMillis();
+ }
+ return obstacleList;
+ }
+
+ /**
+ * Saves a list globally in {@link OpponentManager} and returns that list only updating it if the last update
+ * exceeds the given time.
+ *
+ * @param pollRate how long to wait before updating.
+ * @return a list of obstacles usable by {@link MapleADStar}.
+ */
+ protected List> getObstaclesDynamic(Time pollRate) {
+ // If elapsed time is less than pollRate return our cached list.
+ if (System.currentTimeMillis() - lastObstaclePoll < pollRate.in(Milliseconds)) {
+ return opponentObstacles;
+ }
+ // If elapsed time exceeds our pollRate, refresh the list.
+ opponentObstacles.clear();
+ // Call the other dynamic methods so we don't update if not needed.
+ opponentObstacles.addAll(getObstaclesDynamic(DriverStation.Alliance.Blue, pollRate));
+ opponentObstacles.addAll(getObstaclesDynamic(DriverStation.Alliance.Red, pollRate));
+ // Update our refresh timestamp.
+ lastObstaclePoll = System.currentTimeMillis();
+ return opponentObstacles;
+ }
+
+ /**
+ * Formats a pose2d as a bounding box for obstacles using the boundingBoxBuffer.
+ *
+ * @param pose the pose to format.
+ * @return an obstacle usable by pathplanner.
+ */
+ protected Pair poseToObstacle(Pose2d pose) {
+ return Pair.of(
+ pose.getTranslation().plus(boundingBoxTranslation),
+ pose.getTranslation().minus(boundingBoxTranslation)
+ );
+ }
+
+ /**
+ * Gets a queening pose, removing it from the list.
+ *
+ * @return a queening pose.
+ */
+ public Pose2d getQueeningPose() {
+ var pose = queeningPoses.get(0);
+ queeningPoses.remove(0);
+ return pose;
+ }
+
+ /**
+ * Gets an initial pose, removing it from the list.
+ *
+ * @return a initial pose.
+ */
+ public Pose2d getInitialPose(DriverStation.Alliance alliance) {
+ Pose2d pose;
+ if (alliance == DriverStation.Alliance.Blue) {
+ pose = initialBluePoses.get(0);
+ initialBluePoses.remove(0);
+ } else {
+ pose = initialRedPoses.get(0);
+ initialRedPoses.remove(0);
+ }
+ return pose;
+ }
+
+ /**
+ * Adds a scoring pose.
+ *
+ * @param poseType The type of pose to add. For example, "Hoops".
+ * @param poseName The name of the pose to add. For example, "CourtLeft".
+ * @param pose The pose to add.
+ * @return this, for chaining.
+ */
+ public OpponentManager addScoringPose(String poseType, String poseName, Pose2d pose) {
+ scoringMap.putIfAbsent(poseType, new HashMap<>());
+ scoringMap.get(poseType).putIfAbsent(poseName, pose);
+ if (scoringMap.get(poseType).get(poseName) != pose) {
+ throw new IllegalArgumentException("Failed to add score pose: " + poseName + "/n");
+ }
+ return this;
+ }
+
+ /**
+ * Gets the raw scoring map. Setup as Map>
+ *
+ * @return the raw scoring map.
+ */
+ public Map> getRawScoringMap() {
+ return scoringMap;
+ }
+
+ /**
+ * Compiles a compacted scoring map. Setup as Map
+ *
+ * @return a compacted scoring map.
+ */
+ public Map getScoringMap() {
+ Map scoringPoses = new HashMap<>();
+ scoringMap.forEach((poseType, poseMap) -> poseMap.forEach((name, pose) -> {
+ scoringPoses.put(poseType + name, pose);
+ }));
+ return scoringPoses;
+ }
+
+ /**
+ * Compiles a list of all scoring poses.
+ *
+ * @return a list of all scoring poses.
+ */
+ public List getScoringPoses() {
+ List scoringPoses = new ArrayList<>();
+ scoringMap.forEach((poseType, poseMap) -> poseMap.forEach((poseName, pose) -> scoringPoses.add(pose)));
+ return scoringPoses;
+ }
+
+ /**
+ * Adds a collecting pose to the config.
+ *
+ * @param poseType The type of pose to add. For example, "CollectStation".
+ * @param poseName The name of the pose to add. For example, "StationCenter".
+ * @param pose The pose to add.
+ * @return this, for chaining.
+ */
+ public OpponentManager addCollectingPose(String poseType, String poseName, Pose2d pose) {
+ collectingMap.putIfAbsent(poseType, new HashMap<>());
+ collectingMap.get(poseType).putIfAbsent(poseName, pose);
+ if (collectingMap.get(poseType).get(poseName) != pose) {
+ throw new IllegalArgumentException("Failed to add collect pose: " + poseName + "/n");
+ }
+ return this;
+ }
+
+ /**
+ * Gets the raw collecting map. Setup as Map>
+ *
+ * @return the raw collecting map.
+ */
+ public Map> getRawCollectingMap() {
+ return collectingMap;
+ }
+
+ /**
+ * Compiles a compacted collecting map. Setup as Map
+ *
+ * @return a compacted collecting map.
+ */
+ public Map getCollectingMap() {
+ Map collectingPoses = new HashMap<>();
+ collectingMap.forEach(
+ (poseType, poseMap) -> poseMap.forEach((name, pose2d) -> collectingPoses.put(poseType + name, pose2d)));
+ return collectingPoses;
+ }
+
+ /**
+ * Compiles a list of all collecting poses.
+ *
+ * @return a list of all collecting poses.
+ */
+ public List getCollectingPoses() {
+ List collectingPoses = new ArrayList<>();
+ collectingMap.forEach((poseType, poseMap) -> poseMap.forEach((poseName, pose) -> collectingPoses.add(pose)));
+ return collectingPoses;
+ }
+}
diff --git a/yagsl/java/swervelib/simulation/ironmaple/simulation/opponentsim/SmartOpponent.java b/yagsl/java/swervelib/simulation/ironmaple/simulation/opponentsim/SmartOpponent.java
new file mode 100644
index 0000000..79f17da
--- /dev/null
+++ b/yagsl/java/swervelib/simulation/ironmaple/simulation/opponentsim/SmartOpponent.java
@@ -0,0 +1,587 @@
+package swervelib.simulation.ironmaple.simulation.opponentsim;
+
+import com.pathplanner.lib.commands.FollowPathCommand;
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+import com.pathplanner.lib.path.*;
+import com.pathplanner.lib.trajectory.PathPlannerTrajectoryState;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.math.filter.LinearFilter;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.StringPublisher;
+import edu.wpi.first.networktables.StructPublisher;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.LinearVelocity;
+import edu.wpi.first.units.measure.Time;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
+import swervelib.simulation.ironmaple.simulation.SimulatedArena;
+import swervelib.simulation.ironmaple.simulation.drivesims.SelfControlledSwerveDriveSimulation;
+import swervelib.simulation.ironmaple.simulation.opponentsim.pathfinding.MapleADStar;
+import swervelib.simulation.ironmaple.utils.FieldMirroringUtils;
+
+import java.util.*;
+import java.util.function.Supplier;
+
+import static edu.wpi.first.units.Units.*;
+
+public abstract class SmartOpponent extends SubsystemBase {
+ /// Publishers
+ protected StringPublisher statePublisher;
+ protected StructPublisher posePublisher;
+ // String used in telemetry for alliance.
+ protected String allianceString;
+ /// The SmartOpponentConfig to use.
+ protected SmartOpponentConfig config;
+ // The opponent manager if set
+ protected OpponentManager manager;
+ /// The drivetrain simulation.
+ protected SelfControlledSwerveDriveSimulation drivetrainSim;
+ /// The Manipulator Sim
+ protected ManipulatorSim manipulatorSim;
+ /// The pathplanner config
+ protected RobotConfig pathplannerConfig;
+ /// Pathplanner HolonomicDriveController
+ protected PPHolonomicDriveController driveController;
+ // Pathfinding class cloned for modification.
+ protected final MapleADStar mapleADStar;
+ // Behavior Chooser Publisher
+ protected StringPublisher selectedBehaviorPublisher;
+ // Target Pose
+ protected Pair target;
+ // Whether the state was interrupted and should restart.
+ protected boolean restartInterrupt;
+ /// Collision Detection
+ protected final Debouncer collisionDebouncer;
+ /// Not Moving Detection
+ protected final Timer notMovingTimer;
+ protected final LinearVelocity notMovingThreshold;
+
+ /**
+ * The SmartOpponent base abstracted class.
+ *
+ * @param config the {@link SmartOpponentConfig} to base the opponent off of.
+ */
+ public SmartOpponent(SmartOpponentConfig config) {
+ /// Create and verify config.
+ this.config = config;
+ config.validateConfigs(); // Throw an error if the config is invalid.
+ if (config.manager != null) {
+ this.manager = config.manager;
+ }
+ this.driveController = new PPHolonomicDriveController(
+ new PIDConstants(5),
+ new PIDConstants(5));
+ // Cloned Pathfinder for use here.
+ this.mapleADStar = new MapleADStar();
+ // Preset an empty manipulator
+ this.manipulatorSim = new ManipulatorSim();
+ /// Initialize simulations
+ this.drivetrainSim = config.chassis.createDriveTrainSim(config.queeningPose);
+ this.pathplannerConfig = config.chassis.updatePathplannerConfig();
+ // Alliance string for telemetry.
+ this.allianceString = DriverStation.Alliance.Blue.equals(config.alliance) ? "Blue Alliance/" : "Red Alliance/";
+ // NetworkTable setup.
+ this.statePublisher = NetworkTableInstance.getDefault()
+ .getStringTopic(config.telemetryPath + "SimulatedOpponents/States/"
+ + allianceString + config.name + "'s Current State").publish();
+ this.posePublisher = NetworkTableInstance.getDefault()
+ .getStructTopic(config.telemetryPath + "SimulatedOpponents/Poses/"
+ + allianceString + config.name + "'s Pose2d", Pose2d.struct).publish();
+ this.selectedBehaviorPublisher = NetworkTableInstance.getDefault()
+ .getTable(config.telemetryPath + "SimulatedOpponents/Behaviors/"
+ + allianceString + config.name + "'s Behaviors")
+ .getStringTopic("selected").publish();
+ /// Adds the required states to run the {@link swervelib.simulation.ironmaple.simulation.opponentsim.SmartOpponent}.
+ config.withState("Standby", this::standbyState);
+ config.withState("Starting", () -> startingState("Collect"));
+ config.withState("Collect", this::collectState);
+ config.withState("Score", this::scoreState);
+ setState("Standby");
+ /// Adds options to the behavior sendable chooser.
+ config.withBehavior("Disabled", runState("Standby", true), true);
+ config.withBehavior("Enabled", runState("Starting", true));
+ // Update the chooser and then publish it.
+ SmartDashboard.putData(
+ config.smartDashboardPath
+ + "SimulatedOpponents/Behaviors/"
+ + allianceString + config.name
+ + "'s Behaviors",
+ config.updateBehaviorChooser());
+ /// Run behavior command when changed.
+ config.getBehaviorChooser().onChange(a -> CommandScheduler.getInstance().schedule(a));
+ /// Finally, add our simulation
+ SimulatedArena.getInstance().addDriveTrainSimulation(drivetrainSim.getDriveTrainSimulation());
+ if (config.isAutoEnable) {
+ RobotModeTriggers.teleop()
+ .onTrue(Commands.runOnce(
+ () -> CommandScheduler.getInstance().schedule(config.getBehaviorChooser().getSelected())));
+ RobotModeTriggers.disabled().onTrue(standbyState());
+ }
+ /// If a manager is set register with it
+ if (config.manager != null) {
+ config.manager.registerOpponent(this);
+ }
+ // Initialize an empty target.
+ this.target = Pair.of("None", Pose2d.kZero);
+ /// Collision Detection
+ this.collisionDebouncer = new Debouncer(1, Debouncer.DebounceType.kRising);
+ /// Not Moving Detection
+ this.notMovingThreshold = FeetPerSecond.of(1);
+ this.notMovingTimer = new Timer();
+ // Caches weighted poses for a faster search later.
+ config.loadWeightedPoses();
+ }
+
+ /**
+ * Returns whether the opponent is on the given alliance.
+ *
+ * @param alliance the alliance to check.
+ * @return whether the opponent is on the given alliance.
+ */
+ public boolean isAlliance(DriverStation.Alliance alliance) {
+ return this.config.alliance == alliance;
+ }
+
+ /**
+ * Updates the selected behavior {@link edu.wpi.first.wpilibj.smartdashboard.SendableChooser}, as if it was
+ * changed from the dashboard. This is done with a {@link StringPublisher} pointing to the
+ * {@link edu.wpi.first.wpilibj.smartdashboard.SendableChooser} selected string.
+ *
+ * @param behavior which option to select.
+ * @return a command that updated the selected behavior.
+ */
+ protected Command setSelectedBehavior(String behavior) {
+ return runOnce(() -> selectedBehaviorPublisher.set(behavior));
+ }
+
+ /**
+ * The standby state to run.
+ *
+ * @return a Command that runs the state.
+ */
+ protected Command standbyState() {
+ return run(() -> {
+ drivetrainSim.setSimulationWorldPose(config.queeningPose);
+ drivetrainSim.runChassisSpeeds(new ChassisSpeeds(), new Translation2d(), false, false);
+ restartInterrupt = false;
+ }).ignoringDisable(true);
+ }
+
+ /**
+ * The starting state to run.
+ *
+ * @return a Command that runs the state.
+ */
+ protected Command startingState(String nextState) {
+ return runOnce(() -> drivetrainSim.runChassisSpeeds(new ChassisSpeeds(), new Translation2d(), false, false))
+ .andThen(runOnce(() -> drivetrainSim.setSimulationWorldPose(config.initialPose)))
+ .andThen(Commands.waitSeconds(0.25))
+ .finallyDo(() -> setState(nextState))
+ .ignoringDisable(config.isAutoEnable); // Only move to the field while disabled if autoEnable is off.
+ }
+
+ /**
+ * The collect state to run.
+ *
+ * @return a runnable that runs the state.
+ */
+ protected abstract Command collectState();
+
+ /**
+ * The score state to run.
+ *
+ * @return a runnable that runs the state.
+ */
+ protected abstract Command scoreState();
+
+ @Override
+ public void simulationPeriodic() {
+ // If command not in progress and standby isn't the desired state. Or if a restartInterrupt occurred, restarting the current state.
+ if (!config.commandInProgress && !Objects.equals("Standby", config.desiredState)) {
+ CommandScheduler.getInstance().schedule(runState(config.desiredState, false));
+ }
+ // If restart requested, do that now.
+ if (restartInterrupt) {
+ CommandScheduler.getInstance().schedule(runState(config.desiredState, true));
+ notMovingTimer.restart();
+ restartInterrupt = false;
+ }
+ // If we are very stuck reload from start.
+ if (notMovingTimer.hasElapsed(5)) {
+ drivetrainSim.setSimulationWorldPose(config.initialPose);
+ CommandScheduler.getInstance().schedule(runState(config.desiredState, true));
+ }
+ // If the timer is not running and a command is running and the opponent is not moving, start the timer.
+ if (!notMovingTimer.isRunning() && config.commandInProgress && !isMoving(notMovingThreshold)) {
+ notMovingTimer.restart();
+ } else {
+ notMovingTimer.stop();
+ }
+ statePublisher.set(config.currentState);
+ posePublisher.set(drivetrainSim.getActualPoseInSimulationWorld());
+ }
+
+ /**
+ * Sets the current state of the robot. This waits its turn patiently for the command to finish.
+ *
+ * @param state The state to set.
+ * @return this, for chaining.
+ */
+ protected SmartOpponent setState(String state) {
+ config.desiredState = state;
+ return this;
+ }
+
+ /**
+ * Runs a state as a command.
+ *
+ * @param state The state to run.
+ * @param forceState Whether to force the state to run even if it is already running.
+ * @return The command to run the state.
+ */
+ protected Command runState(String state, boolean forceState) {
+ // If forceState, cancel any commands.
+ if (forceState) {
+ Command currentCommand = getCurrentCommand();
+ if (currentCommand != null) {
+ currentCommand.cancel();
+ }
+ return config.getStates().get(state).get();
+ }
+ // Don't force the state. If there's a command running or already in state, wait.
+ if (config.currentState.equals(state)
+ || getCurrentCommand() != null
+ || (getCurrentCommand() != null && !getCurrentCommand().isFinished())
+ && (!RobotModeTriggers.disabled().getAsBoolean() && config.isAutoEnable)) {
+ setState(state); // Make state wait for command to finish.
+ return Commands.none();
+ }
+ // Nothing in the way, get our state.
+ return config.getStates().get(state).get();
+ }
+
+ /**
+ * Gets the current actual opponent pose.
+ *
+ * @return the opponent {@link Pose2d}.
+ */
+ public Pose2d getOpponentPose() {
+ return drivetrainSim.getActualPoseInSimulationWorld();
+ }
+
+ /**
+ * Gets the opponent's current active target pose.
+ *
+ * @return either the opponent target pose or null if there is no active target.
+ */
+ public Pair getTarget() {
+ return target;
+ }
+
+ protected boolean isColliding() {
+ final var collisionFactor =
+ LinearFilter.movingAverage(50).calculate(Math.abs(
+ (Arrays.stream(drivetrainSim.getDriveTrainSimulation().getModules()).findFirst().get().getDriveMotorSupplyCurrent()).in(Amps)
+ - Arrays.stream(drivetrainSim.getDriveTrainSimulation().getModules()).findAny().get().getDriveMotorAppliedVoltage().in(Volts)));
+ return collisionDebouncer.calculate(MathUtil.isNear(0.6, collisionFactor, 0.3));
+ }
+
+ /**
+ * Pathfinds to a target pose.
+ *
+ * @param target The target.
+ * @return A command to pathfind to the target pose.
+ */
+ public Command pathfind(Pair target, LinearVelocity desiredVelocity) {
+ if (Objects.isNull(target)) {
+ System.out.println(config.name + " Opponent's target is null, skipping this cycle.");
+ return Commands.none();
+ }
+ this.target = Pair.of(target.getFirst(), ifShouldFlip(target.getSecond()));
+ // Add offset after setting flipped generic target.
+ final Pose2d finalPose = this.target.getSecond().plus(config.pathfindOffset);
+ /// Set up the pathfinder
+ mapleADStar.setStartPosition(drivetrainSim.getActualPoseInSimulationWorld().getTranslation());
+ mapleADStar.setGoalPosition(finalPose.getTranslation());
+ mapleADStar.runThread();
+ return Commands.run(() -> {
+ // Initialize our poses and targets
+ final var currentPose = drivetrainSim.getActualPoseInSimulationWorld();
+ final var waypoints = mapleADStar.currentWaypoints;
+ final Translation2d targetTranslation;
+ final Rotation2d targetRotation;
+ // If waypoints exist, load our next target.
+ if (!waypoints.isEmpty()) {
+ // Set anchor as a target.
+ targetTranslation = waypoints.get(0).anchor();
+ // Incorrectly interpolate rotation goal.
+ targetRotation = currentPose.getRotation().interpolate(finalPose.getRotation(), waypoints.size());
+ // If close to current target transfer to the next.
+ if (currentPose.getTranslation().getDistance(targetTranslation) < config.chassis.driveToPoseTolerance.times(3).in(Meters)) {
+ waypoints.remove(0);
+ }
+ // If no waypoints, set the final pose as the target.
+ } else {
+ targetTranslation = finalPose.getTranslation();
+ targetRotation = finalPose.getRotation();
+ }
+ final var targetPose = new Pose2d(targetTranslation, targetRotation);
+ final PathPlannerTrajectoryState state = new PathPlannerTrajectoryState();
+ state.pose = targetPose;
+ // Run out calculated speeds.
+ drivetrainSim.runChassisSpeeds(
+ driveController.calculateRobotRelativeSpeeds(
+ currentPose,
+ state), new Translation2d(), false, false);
+ }, this)
+ .until(() -> { // Run until no waypoints and within tolerances.
+ List waypoints = mapleADStar.currentWaypoints;
+ return waypoints.isEmpty() && nearPose(finalPose, config.chassis.driveToPoseTolerance, config.chassis.driveToPoseAngleTolerance);
+ }) // If an opponent seems stuck, restart.
+ .until(() -> {
+ final boolean bool = notMovingFor(Seconds.of(0.5)) || isColliding();
+ if (bool) {
+ notMovingTimer.restart();
+ restartInterrupt = true;
+ }
+ return bool;
+ })
+ // When finished, stop moving.
+ .finallyDo(() -> drivetrainSim.runChassisSpeeds(new ChassisSpeeds(), new Translation2d(), false, false));
+ }
+
+ /**
+ * Has the opponent run a new FollowPathCommand.
+ *
+ * @param path the path to follow.
+ * @return a follow path command.
+ */
+ protected Command followPath(PathPlannerPath path) {
+ return new FollowPathCommand(
+ path,
+ () -> drivetrainSim.getActualPoseInSimulationWorld(),
+ () -> drivetrainSim.getActualSpeedsRobotRelative(),
+ (speeds, ffNotUsed) -> drivetrainSim.runChassisSpeeds(speeds, new Translation2d(), false, false),
+ driveController,
+ pathplannerConfig,
+ () -> config.alliance == DriverStation.Alliance.Blue,
+ this);
+ }
+
+ /**
+ * A {@link Command} to drive the {@link SmartOpponent}. Meant to be used for joystick drives.
+ *
+ * @param chassisSpeeds speeds supplier to run the robot.
+ * @return {@link Command} to drive the {@link SmartOpponent}.
+ */
+ protected Command drive(Supplier chassisSpeeds, boolean fieldCentric) {
+ if (fieldCentric) {
+ return run(() -> {
+ // Blue alliance faces 180°, Red faces 0°
+ Rotation2d driverFacing =
+ config.alliance.equals(DriverStation.Alliance.Blue) ? Rotation2d.k180deg : Rotation2d.kZero;
+
+ // Convert to field-centric and run
+ ChassisSpeeds fieldSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds(chassisSpeeds.get(), driverFacing);
+ drivetrainSim.runChassisSpeeds(fieldSpeeds, new Translation2d(), true, false);
+ });
+ }
+
+ // Robot-relative
+ return run(() -> drivetrainSim.runChassisSpeeds(chassisSpeeds.get(), new Translation2d(), false, false));
+ }
+
+ /**
+ * Gets a random pose from a map. If the map is empty reports an error and returns null.
+ *
+ * @param poseMap The map to get a pose from.
+ * @return A random pose from the map.
+ */
+ protected Pair getRandomFromMap(Map poseMap) {
+ Map.Entry randomEntry = poseMap.entrySet().stream()
+ .skip(new Random().nextInt(poseMap.size()))
+ .findFirst()
+ .orElse(null);
+
+ return randomEntry != null ? Pair.of(randomEntry.getKey(), randomEntry.getValue()) : null;
+ }
+
+ /**
+ * Gets a random pose from a map.
+ * If the map is empty returns null.
+ *
+ * @param poseMap The map to get a pose from.
+ * @return A random pose from the map, unless empty.
+ */
+ protected Pair getTargetFromMap(Map poseMap) {
+ final Map newMap = new HashMap<>(poseMap);
+ // Remove all already targeted poses.
+ newMap.values().removeAll(manager.getOpponentTargetsDynamic(config.pollRate)); // ignore warning, if it doesn't exist or match, nothing will be removed.
+ // Return null if empty, otherwise continue.
+ return newMap.isEmpty() ? null : getRandomFromMap(newMap);
+ }
+
+ /**
+ * Gets a random scoring target based on pose weights.
+ *
+ * @return a random scoring target based on pose weights.
+ */
+ protected Pair getScoringTarget() {
+ // If no entries exist, fail.
+ if (config.cachedScoringEntries.isEmpty()) {
+ return null;
+ }
+
+ // Cache opponent targets and weight map to avoid repeated calls.
+ final var opponentTargets = manager.getOpponentTargetsDynamic(config.alliance, config.pollRate);
+ final var scoringWeights = config.getScoringWeights();
+ double random = Math.random();
+ double cumulativeWeight = 0;
+ double totalWeight = 0;
+
+ // First pass: calculate total weight of available entries.
+ for (var entry : config.cachedScoringEntries) {
+ // Check both that the target is not already taken and not near any existing targets
+ if (!opponentTargets.contains(entry.getValue()) &&
+ !manager.isNearTarget(entry.getValue(), config.pollRate, Meters.of(1.0))) {
+ totalWeight += scoringWeights.getOrDefault(entry.getKey(), 1.0);
+ }
+ }
+
+ // If no available targets, fail.
+ if (totalWeight == 0) {
+ return null;
+ }
+
+ // Scale random value by total weight for weighted selection.
+ random *= totalWeight;
+
+ // Second pass: find weighted target using cumulative weights.
+ for (var entry : config.cachedScoringEntries) {
+ if (!opponentTargets.contains(entry.getValue())) {
+ cumulativeWeight += scoringWeights.getOrDefault(entry.getKey(), 1.0);
+ if (random <= cumulativeWeight) {
+ return Pair.of(entry.getKey(), entry.getValue());
+ }
+ }
+ }
+
+ // No target found (shouldn't happen if totalWeight > 0).
+ return null;
+ }
+
+ /**
+ * Gets a random collect target based on pose weights.
+ *
+ * @return a random collect target based on pose weights.
+ */
+ protected Pair getCollectTarget() {
+ // If no entries exist, fail.
+ if (config.cachedCollectingEntries.isEmpty()) {
+ return null;
+ }
+
+ final var opponentTargets = manager.getOpponentTargetsDynamic(config.alliance, config.pollRate);
+ final var collectWeights = config.getCollectWeights();
+ double random = Math.random();
+ double cumulativeWeight = 0;
+ double totalWeight = 0;
+
+ // First pass: calculate total weight of available entries.
+ for (var entry : config.cachedCollectingEntries) {
+ // Check both that the target is not already taken and not near any existing targets
+ if (!opponentTargets.contains(entry.getValue()) &&
+ !manager.isNearTarget(entry.getValue(), config.pollRate, Meters.of(1.0))) {
+ totalWeight += collectWeights.getOrDefault(entry.getKey(), 1.0);
+ }
+ }
+
+ // If no available targets, fail.
+ if (totalWeight == 0) {
+ return null;
+ }
+
+ // Scale random value by total weight for weighted selection.
+ random *= totalWeight;
+
+ // Second pass: find weighted target using cumulative weights.
+ for (var entry : config.cachedCollectingEntries) {
+ if (!opponentTargets.contains(entry.getValue())) {
+ cumulativeWeight += collectWeights.getOrDefault(entry.getKey(), 1.0);
+ if (random <= cumulativeWeight) {
+ return Pair.of(entry.getKey(), entry.getValue());
+ }
+ }
+ }
+
+ // No target found (shouldn't happen if totalWeight > 0).
+ return null;
+ }
+
+ /**
+ * Checks if the {@link SmartOpponent} is near the given pose within a given tolerance.
+ *
+ * @param pose the pose to check against.
+ * @param tolerance the translation tolerance in {@link Distance}.
+ * @param angleTolerance the rotation tolerance in {@link Angle}.
+ * @return
+ */
+ public boolean nearPose(Pose2d pose, Distance tolerance, Angle angleTolerance) {
+ Pose2d robotPose = drivetrainSim.getActualPoseInSimulationWorld();
+ return robotPose.getTranslation().getDistance(pose.getTranslation()) < tolerance.in(Meters)
+ && Math.abs(robotPose.getRotation().minus(pose.getRotation()).getDegrees())
+ < angleTolerance.in(Degrees);
+ }
+
+ /**
+ * Checks if the {@link SmartOpponent} is moving within the given tolerance. This checks only the linear velocity.
+ *
+ * @param tolerance the movement speed tolerance in {@link LinearVelocity}.
+ * @return true if the robot is moving faster than the given tolerance.
+ */
+ public boolean isMoving(LinearVelocity tolerance) {
+ final var speeds = drivetrainSim.getActualSpeedsRobotRelative();
+ return speeds.vxMetersPerSecond > tolerance.in(MetersPerSecond)
+ || speeds.vyMetersPerSecond > tolerance.in(MetersPerSecond);
+ }
+
+ /**
+ * Checks if the {@link SmartOpponent} has not been moving for given time.
+ *
+ * @param elapsedTime how long the robot should be stuck for.
+ * @return true if the robot has been stopped for the elaspedTime.
+ */
+ public boolean notMovingFor(Time elapsedTime) {
+ return notMovingTimer.hasElapsed(elapsedTime.in(Seconds)) && !isMoving(notMovingThreshold);
+ }
+
+ /**
+ * If robot is red, returns pose as is. If the robot is blue, it returns the flipped pose.
+ *
+ * @param pose The pose to flip.
+ * @return The flipped pose.
+ */
+ protected Pose2d ifShouldFlip(Pose2d pose) {
+ if (config.alliance == DriverStation.Alliance.Blue) {
+ return pose;
+ } else {
+ return new Pose2d(
+ FieldMirroringUtils.flip(pose.getTranslation()), FieldMirroringUtils.flip(pose.getRotation()));
+ }
+ }
+}
diff --git a/yagsl/java/swervelib/simulation/ironmaple/simulation/opponentsim/SmartOpponentConfig.java b/yagsl/java/swervelib/simulation/ironmaple/simulation/opponentsim/SmartOpponentConfig.java
new file mode 100644
index 0000000..7df2a54
--- /dev/null
+++ b/yagsl/java/swervelib/simulation/ironmaple/simulation/opponentsim/SmartOpponentConfig.java
@@ -0,0 +1,1639 @@
+package swervelib.simulation.ironmaple.simulation.opponentsim;
+
+import com.pathplanner.lib.config.RobotConfig;
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.units.measure.*;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import swervelib.simulation.ironmaple.simulation.SimulatedArena;
+import swervelib.simulation.ironmaple.simulation.drivesims.*;
+import swervelib.simulation.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig;
+import swervelib.simulation.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig;
+
+import java.util.*;
+import java.util.function.Supplier;
+
+import static edu.wpi.first.units.Units.*;
+
+/**
+ * The config is required to make a {@link SmartOpponent}.
+ * It should throw if configured wrong.
+ */
+public class SmartOpponentConfig {
+ /// Basic Required Options
+ public String name;
+ public DriverStation.Alliance alliance;
+ public Pose2d queeningPose;
+ public Pose2d initialPose;
+ /// Basic Optional Options
+ // Telemetry Options
+ public String telemetryPath;
+ public String smartDashboardPath;
+ protected final SendableChooser behaviorChooser;
+ // Joystick Options
+ protected Optional
