Simulation

src/main/java/frc/trigon/robot/Robot.java

@@ -9,6 +9,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.trigon.robot.constants.RobotConstants;
+import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler;
 import lib.hardware.RobotHardwareStats;
 import lib.hardware.phoenix6.Phoenix6Inputs;
 import org.littletonrobotics.junction.LogFileUtil;
@@ -59,6 +60,7 @@ public void testInit() {
 
     @Override
     public void simulationPeriodic() {
+        SimulationFieldHandler.update();
     }
 
     @Override

src/main/java/frc/trigon/robot/RobotContainer.java

@@ -6,18 +6,23 @@
 package frc.trigon.robot;
 
 import com.pathplanner.lib.auto.AutoBuilder;
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.trigon.robot.commands.CommandConstants;
 import frc.trigon.robot.commands.commandfactories.CoralCollectionCommands;
+import frc.trigon.robot.commands.commandfactories.CoralEjectionCommands;
+import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands;
 import frc.trigon.robot.commands.commandfactories.GeneralCommands;
 import frc.trigon.robot.constants.CameraConstants;
 import frc.trigon.robot.constants.LEDConstants;
 import frc.trigon.robot.constants.OperatorConstants;
 import frc.trigon.robot.constants.PathPlannerConstants;
 import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator;
 import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants;
+import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler;
 import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator;
 import frc.trigon.robot.subsystems.MotorSubsystem;
 import frc.trigon.robot.subsystems.arm.Arm;
@@ -40,10 +45,10 @@
 
 public class RobotContainer {
     public static final PoseEstimator ROBOT_POSE_ESTIMATOR = new PoseEstimator();
-    public static final ObjectPoseEstimator OBJECT_POSE_ESTIMATOR = new ObjectPoseEstimator(
+    public static final ObjectPoseEstimator CORAL_POSE_ESTIMATOR = new ObjectPoseEstimator(
             CameraConstants.OBJECT_POSE_ESTIMATOR_DELETION_THRESHOLD_SECONDS,
             ObjectPoseEstimator.DistanceCalculationMethod.ROTATION_AND_TRANSLATION,
-            SimulatedGamePieceConstants.GamePieceType.GAME_PIECE_TYPE,
+            SimulatedGamePieceConstants.GamePieceType.CORAL,
             CameraConstants.OBJECT_DETECTION_CAMERA
     );
     public static final Swerve SWERVE = new Swerve();
@@ -98,7 +103,12 @@ private void bindControllerCommands() {
         OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND);
         OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand());
 
+        OperatorConstants.SPAWN_CORAL_TRIGGER.onTrue(new InstantCommand(() -> SimulationFieldHandler.updateCoralSpawning(new Pose3d(ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose()))));
         OperatorConstants.CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getCoralCollectionCommand());
+
+        OperatorConstants.EJECT_CORAL_TRIGGER.whileTrue(CoralEjectionCommands.getCoralEjectionCommand());
+        OperatorConstants.SCORE_CORAL_LEFT_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(false));
+        OperatorConstants.SCORE_CORAL_RIGHT_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(true));
     }
 
     private void configureSysIDBindings(MotorSubsystem subsystem) {

src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java → src/main/java/frc/trigon/robot/commands/commandclasses/CoralAutoDriveCommand.java

@@ -8,26 +8,23 @@
 import frc.trigon.robot.commands.commandfactories.GeneralCommands;
 import frc.trigon.robot.constants.PathPlannerConstants;
 import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator;
-import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants;
 import frc.trigon.robot.subsystems.swerve.SwerveCommands;
-import org.littletonrobotics.junction.Logger;
 import lib.utilities.flippable.FlippableRotation2d;
+import org.littletonrobotics.junction.Logger;
 
 import java.util.function.Supplier;
 
-public class GamePieceAutoDriveCommand extends ParallelCommandGroup {
-    private static final ObjectPoseEstimator OBJECT_POSE_ESTIMATOR = RobotContainer.OBJECT_POSE_ESTIMATOR;
-    private final SimulatedGamePieceConstants.GamePieceType targetGamePieceType;
+public class CoralAutoDriveCommand extends ParallelCommandGroup {
+    private static final ObjectPoseEstimator CORAL_POSE_ESTIMATOR = RobotContainer.CORAL_POSE_ESTIMATOR;
     private Translation2d distanceFromTrackedGamePiece;
 
-    public GamePieceAutoDriveCommand(SimulatedGamePieceConstants.GamePieceType targetGamePieceType) {
-        this.targetGamePieceType = targetGamePieceType;
+    public CoralAutoDriveCommand() {
         addCommands(
                 getTrackGamePieceCommand(),
                 GeneralCommands.getContinuousConditionalCommand(
                         getDriveToGamePieceCommand(() -> distanceFromTrackedGamePiece),
                         GeneralCommands.getFieldRelativeDriveCommand(),
-                        () -> OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot() != null && shouldMoveTowardsGamePiece(distanceFromTrackedGamePiece)
+                        () -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null && shouldMoveTowardsGamePiece(distanceFromTrackedGamePiece)
                 )
         );
     }
@@ -40,7 +37,7 @@ private Command getTrackGamePieceCommand() {
 
     public static Translation2d calculateDistanceFromTrackedGamePiece() {
         final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
-        final Translation2d trackedObjectPositionOnField = OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot();
+        final Translation2d trackedObjectPositionOnField = CORAL_POSE_ESTIMATOR.getClosestObjectToRobot();
         if (trackedObjectPositionOnField == null)
             return null;
 
@@ -57,7 +54,7 @@ public static Command getDriveToGamePieceCommand(Supplier<Translation2d> distanc
                 SwerveCommands.getClosedLoopSelfRelativeDriveCommand(
                         () -> PathPlannerConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.get().getX()),
                         () -> PathPlannerConstants.GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.get().getY()),
-                        GamePieceAutoDriveCommand::calculateTargetAngle
+                        CoralAutoDriveCommand::calculateTargetAngle
                 )
         );
     }
@@ -69,7 +66,7 @@ public static boolean shouldMoveTowardsGamePiece(Translation2d distanceFromTrack
 
     public static FlippableRotation2d calculateTargetAngle() {
         final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
-        final Translation2d trackedObjectFieldRelativePosition = OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot();
+        final Translation2d trackedObjectFieldRelativePosition = CORAL_POSE_ESTIMATOR.getClosestObjectToRobot();
         if (trackedObjectFieldRelativePosition == null)
             return null;
 

src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java

@@ -11,8 +11,8 @@
 import frc.trigon.robot.commands.commandfactories.GeneralCommands;
 import frc.trigon.robot.constants.OperatorConstants;
 import frc.trigon.robot.subsystems.swerve.SwerveCommands;
-import org.littletonrobotics.junction.Logger;
 import lib.hardware.RobotHardwareStats;
+import org.littletonrobotics.junction.Logger;
 
 import java.util.function.Supplier;
 
@@ -43,7 +43,7 @@ public IntakeAssistCommand(AssistMode assistMode) {
                 GeneralCommands.getContinuousConditionalCommand(
                         GeneralCommands.getFieldRelativeDriveCommand(),
                         getAssistIntakeCommand(assistMode, () -> distanceFromTrackedGamePiece),
-                        () -> RobotContainer.OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot() == null || distanceFromTrackedGamePiece == null
+                        () -> RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() == null || distanceFromTrackedGamePiece == null
                 )
         );
     }
@@ -53,7 +53,7 @@ public IntakeAssistCommand(AssistMode assistMode) {
      * This command is for intaking a game piece with a specific robot-relative position.
      * To create an intake assist command that selects the closest game piece automatically, use {@link IntakeAssistCommand#IntakeAssistCommand(AssistMode)} instead.
      *
-     * @param assistMode               the type of assistance
+     * @param assistMode                   the type of assistance
      * @param distanceFromTrackedGamePiece the position of the game piece relative to the robot
      * @return the command
      */
@@ -70,14 +70,14 @@ public static Command getAssistIntakeCommand(AssistMode assistMode, Supplier<Tra
 
     private Command getTrackGamePieceCommand() {
         return new RunCommand(() -> {
-            if (RobotContainer.OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot() != null)
+            if (RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null)
                 distanceFromTrackedGamePiece = calculateDistanceFromTrackedCGamePiece();
         });
     }
 
     private static Translation2d calculateDistanceFromTrackedCGamePiece() {
         final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
-        final Translation2d trackedObjectPositionOnField = RobotContainer.OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot();
+        final Translation2d trackedObjectPositionOnField = RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot();
         if (trackedObjectPositionOnField == null)
             return null;
 

src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java

@@ -5,7 +5,6 @@
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.trigon.robot.RobotContainer;
-import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand;
 import frc.trigon.robot.constants.LEDConstants;
 import frc.trigon.robot.constants.OperatorConstants;
 import frc.trigon.robot.subsystems.arm.ArmCommands;
@@ -19,25 +18,29 @@
 import lib.hardware.misc.leds.LEDCommands;
 
 public class CoralCollectionCommands {
-    public static Command getLoadCoralCommand() {
-        return new ParallelCommandGroup(
-                ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.LOAD_CORAL),
-                ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.LOAD_CORAL)
-        ).until(RobotContainer.ARM::hasGamePiece);
-    }
 
     public static Command getCoralCollectionCommand() {
-        return new ParallelCommandGroup(
+        return new SequentialCommandGroup(
                 getIntakeSequenceCommand(),
-                new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE)
+                new InstantCommand(
+                        () -> getLoadCoralCommand().schedule()
+                )
         );
+        // new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE)
+    }
+
+    private static Command getLoadCoralCommand() {
+        return new ParallelCommandGroup(
+                ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.LOAD_CORAL),
+                ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.LOAD_CORAL)
+        ).until(RobotContainer.ARM::hasGamePiece).asProxy();
     }
 
     private static Command getIntakeSequenceCommand() {
         return new SequentialCommandGroup(
                 getInitiateCollectionCommand().until(RobotContainer.INTAKE::hasCoral),
                 new InstantCommand(() -> getAlignCoralCommand().schedule()).alongWith(getCollectionConfirmationCommand())
-        );
+        ).until(RobotContainer.INTAKE::hasCoral);
     }
 
     private static Command getInitiateCollectionCommand() {

src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java

@@ -4,6 +4,7 @@
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.trigon.robot.RobotContainer;
+import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler;
 import frc.trigon.robot.subsystems.arm.ArmCommands;
 import frc.trigon.robot.subsystems.arm.ArmConstants;
 import frc.trigon.robot.subsystems.intake.IntakeCommands;
@@ -17,7 +18,7 @@ public static Command getCoralEjectionCommand() {
                 getEjectCoralFromIntakeCommand(),
                 getEjectCoralFromArmCommand(),
                 () -> RobotContainer.TRANSPORTER.hasCoral() || RobotContainer.INTAKE.hasCoral()
-        );
+        ).onlyIf(SimulationFieldHandler::isHoldingCoral);
     }
 
     private static Command getEjectCoralFromIntakeCommand() {

src/main/java/frc/trigon/robot/constants/FieldConstants.java

@@ -19,7 +19,7 @@ public class FieldConstants {
             FIELD_WIDTH_METERS = FlippingUtil.fieldSizeY,
             FIELD_LENGTH_METERS = FlippingUtil.fieldSizeX;
     private static final List<Integer> I_HATE_YOU = List.of(
-            13, 12, 16, 15, 14, 4, 5, 3, 2, 1
+            1, 2, 3, 4, 5, 12, 13, 14, 15, 16
     );
 
     private static final boolean SHOULD_USE_HOME_TAG_LAYOUT = false;

src/main/java/frc/trigon/robot/constants/OperatorConstants.java

@@ -33,22 +33,28 @@ public class OperatorConstants {
     public static final IntakeAssistCommand.AssistMode DEFAULT_INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.ALTERNATE_ASSIST;
 
     public static final Trigger
-            RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.y(),
+            RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.rightStick(),
             DRIVE_FROM_DPAD_TRIGGER = new Trigger(() -> DRIVER_CONTROLLER.getPov() != -1),
             TOGGLE_BRAKE_TRIGGER = OPERATOR_CONTROLLER.g().or(RobotController::getUserButton),
             DEBUGGING_TRIGGER = OPERATOR_CONTROLLER.f2(),
-            CONTINUE_TRIGGER = DRIVER_CONTROLLER.leftStick().and(DRIVER_CONTROLLER.rightStick()).or(OPERATOR_CONTROLLER.k()),
+            CONTINUE_TRIGGER = DRIVER_CONTROLLER.rightTrigger().or(OPERATOR_CONTROLLER.k()),
             FORWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.right(),
             BACKWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.left(),
             FORWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.up(),
             BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.down();
-    public static final Trigger CORAL_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftTrigger().or(OPERATOR_CONTROLLER.c());
 
     public static final Trigger
-            SET_TARGET_SCORING_REEF_LEVEL_L1_TRIGGER = OPERATOR_CONTROLLER.numpad0().or(DRIVER_CONTROLLER.povDown()),
-            SET_TARGET_SCORING_REEF_LEVEL_L2_TRIGGER = OPERATOR_CONTROLLER.numpad1().or(DRIVER_CONTROLLER.povRight()),
-            SET_TARGET_SCORING_REEF_LEVEL_L3_TRIGGER = OPERATOR_CONTROLLER.numpad2().or(DRIVER_CONTROLLER.povLeft()),
-            SET_TARGET_SCORING_REEF_LEVEL_L4_TRIGGER = OPERATOR_CONTROLLER.numpad3().or(DRIVER_CONTROLLER.povUp()),
+            CORAL_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftTrigger().or(OPERATOR_CONTROLLER.c()),
+            SPAWN_CORAL_TRIGGER = OPERATOR_CONTROLLER.equals(),
+            SCORE_CORAL_LEFT_TRIGGER = DRIVER_CONTROLLER.leftBumper(),
+            SCORE_CORAL_RIGHT_TRIGGER = DRIVER_CONTROLLER.rightBumper(),
+            EJECT_CORAL_TRIGGER = DRIVER_CONTROLLER.back().or(OPERATOR_CONTROLLER.e());
+
+    public static final Trigger
+            SET_TARGET_SCORING_REEF_LEVEL_L1_TRIGGER = OPERATOR_CONTROLLER.numpad0().or(DRIVER_CONTROLLER.a()),
+            SET_TARGET_SCORING_REEF_LEVEL_L2_TRIGGER = OPERATOR_CONTROLLER.numpad1().or(DRIVER_CONTROLLER.b()),
+            SET_TARGET_SCORING_REEF_LEVEL_L3_TRIGGER = OPERATOR_CONTROLLER.numpad2().or(DRIVER_CONTROLLER.x()),
+            SET_TARGET_SCORING_REEF_LEVEL_L4_TRIGGER = OPERATOR_CONTROLLER.numpad3().or(DRIVER_CONTROLLER.y()),
             SET_TARGET_SCORING_REEF_CLOCK_POSITION_2_OCLOCK_TRIGGER = OPERATOR_CONTROLLER.numpad9(),
             SET_TARGET_SCORING_REEF_CLOCK_POSITION_4_OCLOCK_TRIGGER = OPERATOR_CONTROLLER.numpad6(),
             SET_TARGET_SCORING_REEF_CLOCK_POSITION_6_OCLOCK_TRIGGER = OPERATOR_CONTROLLER.numpad5(),

src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java

@@ -80,7 +80,7 @@ private void updateHasNewResultInputs(ObjectDetectionCameraInputsAutoLogged inpu
      * @return the placements of the visible objects, as a pair of the object and the rotation of the object relative to the camera
      */
     private ArrayList<Pair<SimulatedGamePiece, Rotation3d>> calculateVisibleGamePieces(Pose3d cameraPose, int objectID) {
-        final ArrayList<SimulatedGamePiece> gamePiecesOnField = SimulationFieldHandler.getSimulatedGamePieces();
+        final ArrayList<SimulatedGamePiece> gamePiecesOnField = objectID == SimulatedGamePieceConstants.GamePieceType.CORAL.id ? SimulationFieldHandler.getSimulatedCoral() : SimulationFieldHandler.getSimulatedAlgae();
         final ArrayList<Pair<SimulatedGamePiece, Rotation3d>> visibleTargetObjects = new ArrayList<>();
         for (SimulatedGamePiece currentObject : gamePiecesOnField) {
             if (currentObject.isScored())

src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePiece.java

@@ -30,16 +30,27 @@ public void updatePeriodically(boolean isHeld) {
     /**
      * Releases the game piece from the robot.
      *
-     * @param fieldRelativeReleaseVelocity the velocity that the object is released at, relative to the field
+     * @param fieldRelativeReleaseVelocities the velocity that the object is released at, relative to the field
      */
-    public void release(Translation3d fieldRelativeReleaseVelocity) {
+    public void release(Translation3d... fieldRelativeReleaseVelocities) {
+        final Translation3d fieldRelativeReleaseVelocity = new Translation3d();
+        for (Translation3d velocityComponent : fieldRelativeReleaseVelocities)
+            fieldRelativeReleaseVelocity.plus(velocityComponent);
+
         velocityAtRelease = fieldRelativeReleaseVelocity;
         poseAtRelease = fieldRelativePose;
         timestampAtRelease = Timer.getTimestamp();
 
         updateIsTouchingGround();
     }
 
+    public void release() {
+        poseAtRelease = fieldRelativePose;
+        timestampAtRelease = Timer.getTimestamp();
+
+        updateIsTouchingGround();
+    }
+
     public void updatePose(Pose3d fieldRelativePose) {
         this.fieldRelativePose = fieldRelativePose;
     }