Merge pull request #9 from twisterjafla/main

Author: konnorreynolds

yagsl/java/swervelib/simulation/ironmaple/simulation/Goal.java

@@ -1,18 +1,18 @@
 package swervelib.simulation.ironmaple.simulation;
 
+import static edu.wpi.first.units.Units.Degrees;
+
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.Distance;
-import org.dyn4j.geometry.Rectangle;
-import org.dyn4j.geometry.Vector2;
 import swervelib.simulation.ironmaple.simulation.gamepieces.GamePiece;
 
 import java.util.List;
-
-import static edu.wpi.first.units.Units.Degrees;
+import org.dyn4j.geometry.Rectangle;
+import org.dyn4j.geometry.Vector2;
 
 /**
  *
@@ -35,6 +35,11 @@ public abstract class Goal implements SimulatedArena.Simulatable {
     protected Rotation3d pieceAngle = null;
     protected Angle pieceAngleTolerance = Angle.ofBaseUnits(15, Degrees);
 
+    protected final double minZ;
+    protected final double maxZ;
+
+    protected final boolean allowGrounded;
+
     /**
      *
      *
@@ -52,14 +57,15 @@ public static Rotation3d flipRotation(Rotation3d toFlip) {
      *
      * <h2>Creates a goal object </h2>
      *
-     * @param arena         The host arena of this goal
-     * @param xDimension    The x dimension of the default box collider.
-     * @param yDimension    The y dimension of the default box collider.
-     * @param height        The height or z dimension of the default box collider.
+     * @param arena The host arena of this goal
+     * @param xDimension The x dimension of the default box collider.
+     * @param yDimension The y dimension of the default box collider.
+     * @param height The height or z dimension of the default box collider.
      * @param gamePieceType the string game piece type to be handled by this goal.
-     * @param position      The position of this goal.
-     * @param isBlue        Wether this is a blue goal or a red one.
-     * @param max           How many pieces can be scored in this goal.
+     * @param position The position of this goal.
+     * @param isBlue Wether this is a blue goal or a red one.
+     * @param max How many pieces can be scored in this goal.
+     * @param allowsGrounded Wether or not grounded pieces can be scored in this goal
      */
     public Goal(
             SimulatedArena arena,
@@ -69,7 +75,8 @@ public Goal(
             String gamePieceType,
             Translation3d position,
             boolean isBlue,
-            int max) {
+            int max,
+            boolean allowGrounded) {
 
         xyBox = new Rectangle(xDimension.in(Units.Meters), yDimension.in(Units.Meters));
         this.height = height;
@@ -80,6 +87,11 @@ public Goal(
         this.elevation = position.getMeasureZ();
         this.isBlue = isBlue;
 
+        this.allowGrounded = allowGrounded;
+
+        minZ = elevation.in(Units.Meters);
+        maxZ = minZ + height.in(Units.Meters);
+
         xyBox.translate(new Vector2(position.getX(), position.getY()));
     }
 
@@ -88,13 +100,14 @@ public Goal(
      *
      * <h2>Creates a goal object with no scoring max.</h2>
      *
-     * @param arena         The host arena of this goal.
-     * @param xDimension    The x dimension of the default box collider.
-     * @param yDimension    The y dimension of the default box collider.
-     * @param height        The height or z dimension of the default box collider.
+     * @param arena The host arena of this goal.
+     * @param xDimension The x dimension of the default box collider.
+     * @param yDimension The y dimension of the default box collider.
+     * @param height The height or z dimension of the default box collider.
      * @param gamePieceType the string game piece type to be handled by this goal.
-     * @param position      The position of this goal.
-     * @param isBlue        Wether this is a blue goal or a red one.
+     * @param position The position of this goal.
+     * @param isBlue Wether this is a blue goal or a red one.
+     * @param allowsGrounded Wether or not grounded pieces can be scored in this goal
      */
     public Goal(
             SimulatedArena arena,
@@ -103,8 +116,9 @@ public Goal(
             Distance height,
             String gamePieceType,
             Translation3d position,
-            boolean isBlue) {
-        this(arena, xDimension, yDimension, height, gamePieceType, position, isBlue, 99999);
+            boolean isBlue,
+            boolean allowsGrounded) {
+        this(arena, xDimension, yDimension, height, gamePieceType, position, isBlue, 99999, allowsGrounded);
     }
 
     /**
@@ -118,7 +132,7 @@ public void simulationSubTick(int subTickNum) {
         /// Use list filtering for more efficient bulk checking.
         // Get all pieces of our type as a list.
         arena.getGamePiecesByType(gamePieceType).stream()
-                .filter(gamePiece -> !checkGrounded(gamePiece))
+                .filter(gamePiece -> checkGrounded(gamePiece))
                 .filter(this::checkValidity)
                 .limit(max - gamePieceCount) // Only score what we can
                 .forEach(
@@ -134,7 +148,7 @@ public void simulationSubTick(int subTickNum) {
      *
      * <h2>Sets the angle to be used when checking game piece rotation.
      *
-     * @param angle          The angle that pieces should have when interacting with this goal
+     * @param angle The angle that pieces should have when interacting with this goal
      * @param angleTolerance The tolerance to be used in checking said angle.
      */
     public void setNeededAngle(Rotation3d angle, Angle angleTolerance) {
@@ -176,20 +190,20 @@ protected boolean checkValidity(GamePiece gamePiece) {
     /**
      *
      *
-     * <h2>Checks whether the submitted game piece is grounded </h2>
+     * <h2>Checks whether the submitted game piece is grounded and if this is acceptable </h2>
      *
      * @param gamePiece The game piece to have its groundedness checked.
-     * @return Whether the piece is grounded.
+     * @return Whether the piece is acceptable for this goal.
      */
     protected boolean checkGrounded(GamePiece gamePiece) {
-        return gamePiece.isGrounded();
+        return allowGrounded || !gamePiece.isGrounded();
     }
 
     /**
      *
      *
      * <h2>Checks wether or not the submitted game piece has a rotation able to be scored in this goal </h2>
-     * <p>
+     *
      * By default the rotation needed and tolerance may be set using the {@link Goal#setNeededAngle(Rotation3d, Angle)}
      * function. However rotation checks may be handled differently by some children making this not apply. Additionally
      * be aware that this function only supports pitch and yaw, not role. If support for roll is needed a custom
@@ -208,13 +222,13 @@ protected boolean checkRotation(GamePiece gamePiece) {
                 flipRotation(gamePiece.getPose3d().getRotation()).minus(pieceAngle);
 
         return new Rotation3d(Degrees.of(0), normalDiff.getMeasureZ(), normalDiff.getMeasureZ())
-                .getMeasureAngle()
-                .in(Degrees)
-                < pieceAngleTolerance.in(Degrees)
+                                .getMeasureAngle()
+                                .in(Degrees)
+                        < pieceAngleTolerance.in(Degrees)
                 || new Rotation3d(Degrees.of(0), flippedDiff.getMeasureZ(), flippedDiff.getMeasureZ())
-                .getMeasureAngle()
-                .in(Degrees)
-                < pieceAngleTolerance.in(Degrees);
+                                .getMeasureAngle()
+                                .in(Degrees)
+                        < pieceAngleTolerance.in(Degrees);
     }
 
     /**
@@ -228,8 +242,6 @@ protected boolean checkRotation(GamePiece gamePiece) {
     protected boolean checkCollision(GamePiece gamePiece) {
         // Call our values just once.
         var pose = gamePiece.getPose3d();
-        double minZ = elevation.in(Units.Meters);
-        double maxZ = minZ + height.in(Units.Meters);
 
         return xyBox.contains(new Vector2(pose.getX(), pose.getY())) && pose.getZ() >= minZ && pose.getZ() <= maxZ;
     }
@@ -238,7 +250,7 @@ protected boolean checkCollision(GamePiece gamePiece) {
      *
      *
      * <h2>Function to check wether the velocity of potential game pieces is acceptable.</h2>
-     * <p>
+     *
      * The default implementation of this function always returns true so any velocity checks will need to be
      * implemented by children classes.
      *
@@ -273,7 +285,7 @@ public boolean isFull() {
      *
      *
      * <h2>Adds points when a piece has been successfully scored in this goal</h2>
-     * <p>
+     *
      * Since this function is the only trigger called when a piece is scored it may handle other small things outside of
      * adding points.
      */
@@ -285,7 +297,7 @@ public boolean isFull() {
      * <h2>Displays game pieces to advantage scope if applicable.</h2>
      *
      * @param drawList a list of {@link Pose3d} objects used to visualize the positions of the game pieces on
-     *                 AdvantageScope
+     *     AdvantageScope
      */
     public abstract void draw(List<Pose3d> drawList);
 

yagsl/java/swervelib/simulation/ironmaple/simulation/IntakeSimulation.java

@@ -1,7 +1,17 @@
 package swervelib.simulation.ironmaple.simulation;
 
+import static edu.wpi.first.units.Units.Meters;
+
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.units.measure.Distance;
+import swervelib.simulation.ironmaple.simulation.drivesims.AbstractDriveTrainSimulation;
+import swervelib.simulation.ironmaple.simulation.gamepieces.GamePieceOnFieldSimulation;
+import swervelib.simulation.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeCoralAlgaeStack;
+
+import java.util.ArrayDeque;
+import java.util.Objects;
+import java.util.Queue;
+import java.util.function.Predicate;
 import org.dyn4j.collision.CollisionBody;
 import org.dyn4j.collision.Fixture;
 import org.dyn4j.dynamics.Body;
@@ -13,16 +23,6 @@
 import org.dyn4j.geometry.Vector2;
 import org.dyn4j.world.ContactCollisionData;
 import org.dyn4j.world.listener.ContactListener;
-import swervelib.simulation.ironmaple.simulation.drivesims.AbstractDriveTrainSimulation;
-import swervelib.simulation.ironmaple.simulation.gamepieces.GamePieceOnFieldSimulation;
-import swervelib.simulation.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeCoralAlgaeStack;
-
-import java.util.ArrayDeque;
-import java.util.Objects;
-import java.util.Queue;
-import java.util.function.Predicate;
-
-import static edu.wpi.first.units.Units.Meters;
 
 /**
  *
@@ -43,9 +43,9 @@
  * <p>A game piece is collected if the following conditions are met:
  *
  * <ul>
- *   <li>1. The type of the game piece ({@link swervelib.simulation.ironmaple.simulation.gamepieces.GamePieceOnFieldSimulation#type})
+ *   <li>1. The type of the game piece ({@link org.ironmaple.simulation.gamepieces.GamePieceOnFieldSimulation#type})
  *       matches {@link #targetedGamePieceType}.
- *   <li>2. The {@link swervelib.simulation.ironmaple.simulation.gamepieces.GamePieceOnFieldSimulation} is in contact with the intake
+ *   <li>2. The {@link org.ironmaple.simulation.gamepieces.GamePieceOnFieldSimulation} is in contact with the intake
  *       (and not other parts of the robot).
  *   <li>3. The intake is turned on via {@link #startIntake()}.
  *   <li>4. The number of game pieces in the intake ({@link #gamePiecesInIntakeCount}) is less than {@link #capacity}.
@@ -79,10 +79,10 @@ public enum IntakeSide {
      * <p>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
+     * @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,
@@ -101,11 +101,11 @@ public static IntakeSimulation InTheFrameIntake(
      * <p>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
+     * @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(
@@ -154,9 +154,9 @@ private static Rectangle getIntakeRectangle(
      * <p>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
+     * @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,
@@ -257,6 +257,22 @@ public boolean addGamePieceToIntake() {
 
         return toReturn;
     }
+    /**
+     *
+     *
+     * <h2>Adds a number of game pieces to the intake.</h2>
+     *
+     * 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.max(gamePiecesInIntakeCount + piecesToAdd, capacity);
+        return toReturn;
+    }
 
     /**
      *
@@ -318,28 +334,22 @@ private void flagGamePieceForRemoval(GamePieceOnFieldSimulation gamePiece) {
 
         /* functions not used */
         @Override
-        public void persist(ContactCollisionData collision, Contact oldContact, Contact newContact) {
-        }
+        public void persist(ContactCollisionData collision, Contact oldContact, Contact newContact) {}
 
         @Override
-        public void end(ContactCollisionData collision, Contact contact) {
-        }
+        public void end(ContactCollisionData collision, Contact contact) {}
 
         @Override
-        public void destroyed(ContactCollisionData collision, Contact contact) {
-        }
+        public void destroyed(ContactCollisionData collision, Contact contact) {}
 
         @Override
-        public void collision(ContactCollisionData collision) {
-        }
+        public void collision(ContactCollisionData collision) {}
 
         @Override
-        public void preSolve(ContactCollisionData collision, Contact contact) {
-        }
+        public void preSolve(ContactCollisionData collision, Contact contact) {}
 
         @Override
-        public void postSolve(ContactCollisionData collision, SolvedContact contact) {
-        }
+        public void postSolve(ContactCollisionData collision, SolvedContact contact) {}
     }
 
     /**
@@ -405,7 +415,7 @@ public boolean isRunning() {
      * 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
+     *     pieces on the field
      */
     public void setCustomIntakeCondition(Predicate<GamePieceOnFieldSimulation> customIntakeCondition) {
         this.customIntakeCondition = customIntakeCondition;