Skip to content

Commit fc86404

Browse files
Ishan1522Ishan1522
committed
wat da heck
1 parent d2fc185 commit fc86404

File tree

9 files changed

+883
-453
lines changed

9 files changed

+883
-453
lines changed

‎build.gradle‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
plugins {
22
id "java"
3-
id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-2"
3+
id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-3"
44
id "com.peterabeles.gversion" version "1.10"
55
id "com.diffplug.spotless" version "6.25.0"
66
id "pmd"

‎src/main/java/frc/robot/extras/simulation/field/SimArena.java‎

Lines changed: 243 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,243 @@
1+
package frc.robot.extras.simulation.field;
2+
3+
import edu.wpi.first.math.geometry.Pose2d;
4+
import edu.wpi.first.math.geometry.Pose3d;
5+
import edu.wpi.first.math.geometry.Translation2d;
6+
import edu.wpi.first.math.geometry.Translation3d;
7+
import edu.wpi.first.units.measure.Time;
8+
import edu.wpi.first.wpilibj.IterativeRobotBase;
9+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
10+
import frc.robot.extras.simulation.gamePiece.GamePieceSimulation;
11+
12+
import java.util.List;
13+
import java.util.Set;
14+
import java.util.concurrent.ConcurrentHashMap;
15+
import java.util.concurrent.locks.ReentrantLock;
16+
17+
import static edu.wpi.first.units.Units.Seconds;
18+
19+
import java.util.ArrayList;
20+
import java.util.function.Consumer;
21+
import java.util.function.Supplier;
22+
import java.util.stream.Stream;
23+
24+
import org.dyn4j.dynamics.Body;
25+
import org.dyn4j.dynamics.BodyFixture;
26+
import org.dyn4j.geometry.Convex;
27+
import org.dyn4j.geometry.Geometry;
28+
import org.dyn4j.geometry.MassType;
29+
import org.dyn4j.world.PhysicsWorld;
30+
import org.dyn4j.world.World;
31+
32+
public abstract class SimArena {
33+
public static final class SimEnvTiming {
34+
public final Time period;
35+
public final int ticksPerPeriod;
36+
public final Time dt;
37+
38+
private SimEnvTiming(double robotPeriodSeconds, int simulationSubTicksPerPeriod) {
39+
this.period = Seconds.of(robotPeriodSeconds);
40+
this.ticksPerPeriod = simulationSubTicksPerPeriod;
41+
this.dt = Seconds.of(robotPeriodSeconds / ((double) ticksPerPeriod));
42+
}
43+
}
44+
45+
protected final ReentrantLock worldLock = new ReentrantLock();
46+
protected final World<Body> physicsWorld = new World<>();
47+
protected final Set<GamePieceSimulation> gamePieces = ConcurrentHashMap.newKeySet();
48+
protected final Set<ShamRobot<?>> robots = ConcurrentHashMap.newKeySet();
49+
public final SimEnvTiming timing;
50+
51+
/**
52+
*
53+
*
54+
* <h2>Constructs a new simulation arena with the specified field map of obstacles.</h2>
55+
*
56+
* <p>This constructor initializes a physics world with zero gravity and adds the provided
57+
* obstacles to the world.
58+
*
59+
* <p>It also sets up the collections for drivetrain simulations, game pieces, projectiles, and
60+
* intake simulations.
61+
*
62+
* @param obstaclesMap the season-specific field map containing the layout of obstacles for the
63+
* simulation
64+
* @param period the duration of each simulation period in seconds
65+
* @param ticksPerPeriod the number of sub-ticks to execute in each simulation period
66+
*/
67+
protected SimArena(FieldMap obstaclesMap, double period, int ticksPerPeriod) {
68+
this.timing = new SimEnvTiming(period, ticksPerPeriod);
69+
this.physicsWorld.setGravity(PhysicsWorld.ZERO_GRAVITY);
70+
for (Body obstacle : obstaclesMap.obstacles) {
71+
this.physicsWorld.addBody(obstacle);
72+
}
73+
}
74+
75+
void withWorld(Consumer<World<Body>> worldModifier) {
76+
try {
77+
worldLock.lock();
78+
worldModifier.accept(physicsWorld);
79+
} finally {
80+
worldLock.unlock();
81+
}
82+
}
83+
84+
/**
85+
*
86+
*
87+
* <h2>Registers a {@link ShamGamePiece} to the Simulation.</h2>
88+
*
89+
* <p>The user will have to call one the following methods to add the game piece to the field:
90+
* <ul>
91+
* <li>{@link ShamGamePiece#place(Translation2d)}
92+
* <li>{@link ShamGamePiece#slide(Translation2d, Translation2d)}
93+
* <li>{@link ShamGamePiece#launch(Pose3d, Translation3d, ProjectileUtil.ProjectileDynamics)}
94+
* <li>{@link ShamGamePiece#intake(Supplier, Supplier)}
95+
* </ul>
96+
*
97+
* @param variant the variant of game piece to be registered
98+
* @return the game piece that was created and registered
99+
*/
100+
public GamePieceSimulation createGamePiece(GamePieceVariant variant) {
101+
GamePieceSimulation gamePiece = new GamePieceSimulation(variant, this);
102+
this.gamePieces.add(gamePiece);
103+
return gamePiece.userControlled();
104+
}
105+
106+
/**
107+
*
108+
*
109+
* <h2>Update the simulation world.</h2>
110+
*
111+
* <p>This method should be called ONCE in {@link IterativeRobotBase#simulationPeriodic()}
112+
*/
113+
public void simulationPeriodic() {
114+
final long t0 = System.nanoTime();
115+
competitionPeriodic();
116+
// move through a few sub-periods in each update
117+
for (int i = 0; i < timing.ticksPerPeriod; i++) {
118+
simulationSubTick();
119+
}
120+
121+
SmartDashboard.putNumber(
122+
"MapleArenaSimulation/Dyn4jEngineCPUTimeMS", (System.nanoTime() - t0) / 1000000.0);
123+
}
124+
125+
private void simulationSubTick() {
126+
for (final ShamRobot<?> otherRobot : robots)
127+
otherRobot.simTick();
128+
129+
for (final ShamGamePiece gamePiece : gamePieces)
130+
gamePiece.simulationSubTick();
131+
132+
withWorld(world -> world.update(timing.dt.in(Seconds)));
133+
}
134+
135+
/**
136+
* Returns a stream of all game pieces tracked by the arena.
137+
*
138+
* There are some helpful methods to better utilize the stream.
139+
*
140+
* <pre><code>
141+
* // Shows off getting an array of the positions of all cubes on the field
142+
* Pose3d[] gamePiecePositions = arena.gamePieces()
143+
* .filter(GamePieceState.ON_FIELD::isInState) // Only get the game pieces that are on the field
144+
* .filter(CUBE_GP::isOfVariant) // Only get the game pieces that are cubes
145+
* .filter(GamePiece::isLibraryControlled) // Only get the game pieces that are controlled by the library
146+
* .collect(GamePiece.poseStreamCollector()); // Collect the poses of the game pieces into an array
147+
* </code></pre>
148+
*/
149+
public Stream<ShamGamePiece> gamePieces() {
150+
return List.copyOf(gamePieces).stream();
151+
}
152+
153+
/**
154+
*
155+
*
156+
* <h2>Resets the Field for Autonomous Mode.</h2>
157+
*
158+
* <p>This method clears all current game pieces from the field and places new game pieces in
159+
* their starting positions for the autonomous mode.
160+
*/
161+
public void resetFieldForAuto() {
162+
gamePieces.forEach(gp -> gp.withLib(gp2 -> gp2.delete()));
163+
gamePieces.clear();
164+
placeGamePiecesOnField();
165+
}
166+
167+
/**
168+
*
169+
*
170+
* <h2>Places Game Pieces on the Field for Autonomous Mode.</h2>
171+
*
172+
* <p>This method sets up the game pieces on the field, typically in their starting positions for
173+
* autonomous mode.
174+
*
175+
* <p>It should be implemented differently for each season-specific subclass of {@link
176+
* SimArena} to reflect the unique game piece placements for that season's game.
177+
*/
178+
protected abstract void placeGamePiecesOnField();
179+
180+
/**
181+
*
182+
*
183+
* <h2>Season-Specific Actions to Execute in {@link SimArena#simulationPeriodic()}.</h2>
184+
*
185+
* <p>This method defines season-specific tasks to be executed during the {@link
186+
* SimArena#simulationPeriodic()} method.
187+
*
188+
* <p>For example:
189+
*
190+
* <ul>
191+
* <li>Updating the score counts.
192+
* <li>Simulating human player activities.
193+
* </ul>
194+
*
195+
* <p>This method should be implemented in the season-specific subclass of {@link SimArena}
196+
* to reflect the unique aspects of that season's game.
197+
*/
198+
protected abstract void competitionPeriodic();
199+
200+
/**
201+
*
202+
*
203+
* <h1>Represents an Abstract Field Map</h1>
204+
*
205+
* <p>Stores the layout of obstacles and game pieces.
206+
*
207+
* <p>For each season-specific subclass of {@link SimArena}, there should be a corresponding
208+
* subclass of this class to store the field map for that specific season's game.
209+
*/
210+
public static class FieldMap {
211+
private final List<Body> obstacles = new ArrayList<>();
212+
213+
protected void addBorderLine(Translation2d startingPoint, Translation2d endingPoint) {
214+
addCustomObstacle(
215+
Geometry.createSegment(
216+
GeometryConvertor.toDyn4jVector2(startingPoint),
217+
GeometryConvertor.toDyn4jVector2(endingPoint)),
218+
new Pose2d());
219+
}
220+
221+
protected void addRectangularObstacle(
222+
double width, double height, Pose2d absolutePositionOnField) {
223+
addCustomObstacle(Geometry.createRectangle(width, height), absolutePositionOnField);
224+
}
225+
226+
protected void addCustomObstacle(Convex shape, Pose2d absolutePositionOnField) {
227+
final Body obstacle = createObstacle(shape);
228+
229+
obstacle.getTransform().set(GeometryConvertor.toDyn4jTransform(absolutePositionOnField));
230+
231+
obstacles.add(obstacle);
232+
}
233+
234+
private static Body createObstacle(Convex shape) {
235+
final Body obstacle = new Body();
236+
obstacle.setMass(MassType.INFINITE);
237+
final BodyFixture fixture = obstacle.addFixture(shape);
238+
fixture.setFriction(0.6);
239+
fixture.setRestitution(0.3);
240+
return obstacle;
241+
}
242+
}
243+
}

0 commit comments

Comments
 (0)