Skip to content

Commit

Permalink
added human player simulation
Browse files Browse the repository at this point in the history
  • Loading branch information
@catr1xLiu
catr1xLiu committed Sep 7, 2024
1 parent a07bd15 commit 5c7c0ee
Show file tree
Hide file tree
Showing 2 changed files with 75 additions and 37 deletions.
20 changes: 14 additions & 6 deletions ...in/java/frc/robot/utils/CompetitionFieldUtils/Simulations/CompetitionFieldSimulation.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,11 +27,11 @@
* should only be created during a robot simulation (not in real or replay mode)
* */
public abstract class CompetitionFieldSimulation {
private final World<Body> physicsWorld;
private final CompetitionFieldVisualizer competitionField;
private final Set<HolonomicChassisSimulation> robotSimulations = new HashSet<>();
private final HolonomicChassisSimulation mainRobot;
private final Set<GamePieceInSimulation> gamePieces;
protected final World<Body> physicsWorld;
protected final CompetitionFieldVisualizer competitionField;
protected final Set<HolonomicChassisSimulation> robotSimulations = new HashSet<>();
protected final HolonomicChassisSimulation mainRobot;
protected final Set<GamePieceInSimulation> gamePieces;

private List<IntakeSimulation> intakeSimulations = new ArrayList<>();

Expand All @@ -49,6 +49,7 @@ public CompetitionFieldSimulation(HolonomicChassisSimulation mainRobot, FieldObs
}

public void updateSimulationWorld() {
competitionPeriodic();
final double subPeriodSeconds = Robot.defaultPeriodSecs / SIMULATION_TICKS_IN_1_PERIOD;
// move through 5 sub-periods in each update
for (int i = 0; i < SIMULATION_TICKS_IN_1_PERIOD; i++) {
Expand Down Expand Up @@ -77,6 +78,7 @@ public void registerIntake(IntakeSimulation intakeSimulation) {
public void addGamePiece(GamePieceInSimulation gamePieceInSimulation) {
this.physicsWorld.addBody(gamePieceInSimulation);
this.competitionField.addObject(gamePieceInSimulation);
this.gamePieces.add(gamePieceInSimulation);
}

public void removeGamePiece(GamePieceInSimulation gamePieceInSimulation) {
Expand All @@ -100,10 +102,16 @@ public void resetFieldForAuto() {
placeGamePiecesOnField();
}
/**
* place all game pieces on the field (for autonomous)
* do field reset by placing all the game-pieces on field(for autonomous)
* */
public abstract void placeGamePiecesOnField();

/**
* update the score counts & human players periodically
* implement this method in current year's simulation
* */
public abstract void competitionPeriodic();

/**
* stores the obstacles on a competition field, which includes the border and the game pieces
* */
Expand Down
92 changes: 61 additions & 31 deletions .../java/frc/robot/utils/CompetitionFieldUtils/Simulations/Crescendo2024FieldSimulation.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,46 +3,24 @@
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.wpilibj.DriverStation;
import frc.robot.utils.CompetitionFieldUtils.Objects.Crescendo2024FieldObjects;
import frc.robot.utils.CompetitionFieldUtils.Objects.GamePieceInSimulation;
import frc.robot.utils.MapleTimeUtils;

import static frc.robot.constants.FieldConstants.*;

/**
* field simulation for 2024 competition
* */
public class Crescendo2024FieldSimulation extends CompetitionFieldSimulation {
public Crescendo2024FieldSimulation(HolonomicChassisSimulation robot) {
super(robot, new CrescendoFieldObstaclesMap());
}

private static final Translation2d[] NOTE_INITIAL_POSITIONS = new Translation2d[] {
new Translation2d(2.9, 4.1),
new Translation2d(2.9, 5.55),
new Translation2d(2.9, 7),

new Translation2d(8.27, 0.75),
new Translation2d(8.27, 2.43),
new Translation2d(8.27, 4.1),
new Translation2d(8.27, 5.78),
new Translation2d(8.27, 7.46),

new Translation2d(13.64, 4.1),
new Translation2d(13.64, 5.55),
new Translation2d(13.64, 7),
};
@Override
public void placeGamePiecesOnField() {
for (Translation2d notePosition:NOTE_INITIAL_POSITIONS)
super.addGamePiece(new Crescendo2024FieldObjects.NoteOnFieldSimulated(notePosition));
}

/**
* the obstacles on the 2024 competition field
* */
public static final class CrescendoFieldObstaclesMap extends FieldObstaclesMap {
public CrescendoFieldObstaclesMap() {
super();

//left wall
super.addBorderLine(
new Translation2d(0, 1),
Expand All @@ -57,7 +35,7 @@ public CrescendoFieldObstaclesMap() {
new Translation2d(0.9, 5),
new Translation2d(0.9, 6.05)
);

super.addBorderLine(
new Translation2d(0.9, 6.05),
new Translation2d(0, 6.5)
Expand All @@ -66,15 +44,15 @@ public CrescendoFieldObstaclesMap() {
new Translation2d(0, 6.5),
new Translation2d(0, 8.2)
);


// upper wall
super.addBorderLine(
new Translation2d(0, 8.12),
new Translation2d(FIELD_WIDTH, 8.12)
);
// righter wall

// righter wall
super.addBorderLine(
new Translation2d(FIELD_WIDTH, 1),
new Translation2d(FIELD_WIDTH, 4.51)
Expand Down Expand Up @@ -143,4 +121,56 @@ public CrescendoFieldObstaclesMap() {
);
}
}

public Crescendo2024FieldSimulation(HolonomicChassisSimulation robot) {
super(robot, new CrescendoFieldObstaclesMap());
}

private static final Translation2d[] NOTE_INITIAL_POSITIONS = new Translation2d[] {
new Translation2d(2.9, 4.1),
new Translation2d(2.9, 5.55),
new Translation2d(2.9, 7),

new Translation2d(8.27, 0.75),
new Translation2d(8.27, 2.43),
new Translation2d(8.27, 4.1),
new Translation2d(8.27, 5.78),
new Translation2d(8.27, 7.46),

new Translation2d(13.64, 4.1),
new Translation2d(13.64, 5.55),
new Translation2d(13.64, 7),
};

@Override
public void placeGamePiecesOnField() {
for (Translation2d notePosition:NOTE_INITIAL_POSITIONS)
super.addGamePiece(new Crescendo2024FieldObjects.NoteOnFieldSimulated(notePosition));
}

private static final Translation2d BLUE_SOURCE_POSITION = new Translation2d(15.6, 0.8);
@Override
public void competitionPeriodic() {
simulateHumanPlayer();
}

private double previousThrowTimeSeconds = 0;
private void simulateHumanPlayer() {
if (!DriverStation.isTeleopEnabled()) return;

if (MapleTimeUtils.getLogTimeSeconds() - previousThrowTimeSeconds < 1) return;

final Translation2d sourcePosition = toCurrentAllianceTranslation(BLUE_SOURCE_POSITION);
/* if there is any game-piece 0.5 meters within the human player station, we don't throw a new note */
for (GamePieceInSimulation gamePiece:super.gamePieces)
if (gamePiece
.getObjectOnFieldPose2d()
.getTranslation()
.getDistance(sourcePosition)
< 1) return;

/* otherwise, place a note */
addGamePiece(new Crescendo2024FieldObjects.NoteOnFieldSimulated(sourcePosition));
previousThrowTimeSeconds = MapleTimeUtils.getLogTimeSeconds();
}
}

0 comments on commit 5c7c0ee

Please sign in to comment.