diff --git a/src/main/java/frc/trigon/robot/Robot.java b/src/main/java/frc/trigon/robot/Robot.java index e4bb7a25..9284bbff 100644 --- a/src/main/java/frc/trigon/robot/Robot.java +++ b/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 diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 98975d77..2265ac21 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -6,11 +6,15 @@ 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; @@ -18,6 +22,7 @@ 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) { diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/CoralAutoDriveCommand.java similarity index 81% rename from src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java rename to src/main/java/frc/trigon/robot/commands/commandclasses/CoralAutoDriveCommand.java index e0663de3..026e0680 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java +++ b/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 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; diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java index a57f3472..9fdae10e 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/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 { - 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; diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java index ce9fc10d..f4d70ecb 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/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() { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java index 6ca495b4..ebefb93d 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java +++ b/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() { diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 1ea770ae..9f7b8daa 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/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 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; diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 70e4f652..147f64e6 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/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(), diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java index a7bd8346..3905b409 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java +++ b/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> calculateVisibleGamePieces(Pose3d cameraPose, int objectID) { - final ArrayList gamePiecesOnField = SimulationFieldHandler.getSimulatedGamePieces(); + final ArrayList gamePiecesOnField = objectID == SimulatedGamePieceConstants.GamePieceType.CORAL.id ? SimulationFieldHandler.getSimulatedCoral() : SimulationFieldHandler.getSimulatedAlgae(); final ArrayList> visibleTargetObjects = new ArrayList<>(); for (SimulatedGamePiece currentObject : gamePiecesOnField) { if (currentObject.isScored()) diff --git a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePiece.java b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePiece.java index a27b8752..5a54bfbc 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePiece.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePiece.java @@ -30,9 +30,13 @@ 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(); @@ -40,6 +44,13 @@ public void release(Translation3d fieldRelativeReleaseVelocity) { updateIsTouchingGround(); } + public void release() { + poseAtRelease = fieldRelativePose; + timestampAtRelease = Timer.getTimestamp(); + + updateIsTouchingGround(); + } + public void updatePose(Pose3d fieldRelativePose) { this.fieldRelativePose = fieldRelativePose; } diff --git a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java index 6f7ead54..214357de 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java @@ -1,6 +1,8 @@ package frc.trigon.robot.misc.simulatedfield; -import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.util.Units; +import frc.trigon.robot.constants.FieldConstants; import lib.utilities.flippable.FlippablePose3d; import lib.utilities.flippable.FlippableTranslation2d; @@ -9,25 +11,117 @@ public class SimulatedGamePieceConstants { public static final double G_FORCE = 9.806; - public static final double - FEEDER_INTAKE_TOLERANCE_METERS = 0.1, - INTAKE_TOLERANCE_METERS = 0.1, - SCORING_TOLERANCE_METERS = 0.1; + CORAL_INTAKE_TOLERANCE_METERS = 0.3, + ALGAE_INTAKE_TOLERANCE_METERS = 0.3, + CORAL_SCORING_TOLERANCE_METERS = 0.1, + ALGAE_SCORING_TOLERANCE_METERS = 0.2; + + private static final double + FIELD_WIDTH_METERS = FieldConstants.FIELD_WIDTH_METERS, + FIELD_LENGTH_METERS = FieldConstants.FIELD_LENGTH_METERS; + private static final Rotation3d CORAL_TO_VERTICAL_POSITION_ROTATION = new Rotation3d(0, Math.PI / 2, 0); /** * Stores all the game pieces. * Starts out with the game pieces the start on the field. */ public static final ArrayList - STARTING_GAME_PIECES = new ArrayList<>(List.of( - )); + CORAL_ON_FIELD = new ArrayList<>(List.of( + createNewCoral(new Pose3d(1.22, FIELD_WIDTH_METERS / 2, 0.15, CORAL_TO_VERTICAL_POSITION_ROTATION)), + createNewCoral(new Pose3d(1.22, FIELD_WIDTH_METERS / 2 - 1.83, 0.15, CORAL_TO_VERTICAL_POSITION_ROTATION)), + createNewCoral(new Pose3d(1.22, FIELD_WIDTH_METERS / 2 + 1.83, 0.15, CORAL_TO_VERTICAL_POSITION_ROTATION)), + createNewCoral(new Pose3d(FIELD_LENGTH_METERS - 1.22, FIELD_WIDTH_METERS / 2, 0.15, CORAL_TO_VERTICAL_POSITION_ROTATION)), + createNewCoral(new Pose3d(FIELD_LENGTH_METERS - 1.22, FIELD_WIDTH_METERS / 2 - 1.83, 0.15, CORAL_TO_VERTICAL_POSITION_ROTATION)), + createNewCoral(new Pose3d(FIELD_LENGTH_METERS - 1.22, FIELD_WIDTH_METERS / 2 + 1.83, 0.15, CORAL_TO_VERTICAL_POSITION_ROTATION)) + )), + ALGAE_ON_FIELD = new ArrayList<>(List.of( + createNewAlgae(new Pose3d(1.22, FIELD_WIDTH_METERS / 2, 0.5, new Rotation3d())), + createNewAlgae(new Pose3d(1.22, FIELD_WIDTH_METERS / 2 - 1.83, 0.5, new Rotation3d())), + createNewAlgae(new Pose3d(1.22, FIELD_WIDTH_METERS / 2 + 1.83, 0.5, new Rotation3d())), + createNewAlgae(new Pose3d(FIELD_LENGTH_METERS - 1.22, FIELD_WIDTH_METERS / 2, 0.5, new Rotation3d())), + createNewAlgae(new Pose3d(FIELD_LENGTH_METERS - 1.22, FIELD_WIDTH_METERS / 2 - 1.83, 0.5, new Rotation3d())), + createNewAlgae(new Pose3d(FIELD_LENGTH_METERS - 1.22, FIELD_WIDTH_METERS / 2 + 1.83, 0.5, new Rotation3d())) + )); + + private static final Translation3d + REEF_CENTER_TO_L1_VECTOR = new Translation3d(0.652, 0.1643, 0.46), + REEF_CENTER_TO_L2_VECTOR = new Translation3d(0.652, 0.1643, 0.6983), + REEF_CENTER_TO_L3_VECTOR = new Translation3d(0.652, 0.1643, 1.1101), + REEF_CENTER_TO_L4_VECTOR = new Translation3d(0.7796, 0.1643, 1.7345); + private static final Rotation3d + REEF_TO_2_OCLOCK_ROTATION = new Rotation3d(0, 0, Math.PI / 3), + REEF_TO_4_OCLOCK_ROTATION = new Rotation3d(0, 0, Math.PI / 3 * 2), + REEF_TO_6_OCLOCK_ROTATION = new Rotation3d(0, 0, Math.PI), + REEF_TO_8_OCLOCK_ROTATION = new Rotation3d(0, 0, Math.PI / 3 * 4), + REEF_TO_10_OCLOCK_ROTATION = new Rotation3d(0, 0, Math.PI / 3 * 5), + REEF_TO_12_OCLOCK_ROTATION = new Rotation3d(0, 0, 0); + private static final Transform3d + CORAL_TO_L1_ALIGNMENT = new Transform3d(new Translation3d(0, 0, 0), new Rotation3d(0, Units.degreesToRadians(15), 0)), + CORAL_TO_L2_AND_L3_ALIGNMENT = new Transform3d(new Translation3d(0, 0, 0), new Rotation3d(0, -Units.degreesToRadians(35), 0)), + CORAL_TO_L4_ALIGNMENT = new Transform3d(new Translation3d(0, 0, 0), CORAL_TO_VERTICAL_POSITION_ROTATION); + private static final Transform3d + RIGHT_BRANCH_TO_LEFT_BRANCH = new Transform3d(new Translation3d(0, -0.33, 0), new Rotation3d()); + public static final ArrayList CORAL_SCORING_LOCATIONS = calculatedCoralScoringLocations(); + public static final Pose3d PROCESSOR_LOCATION = new Pose3d(0, 0, 0, new Rotation3d()); + public static final FlippableTranslation2d + LEFT_FEEDER_POSITION = new FlippableTranslation2d(0.923, 7.370, true), + RIGHT_FEEDER_POSITION = new FlippableTranslation2d(0.923, 0.668, true); + static final FlippablePose3d + RIGHT_CORAL_SPAWN_POSE = new FlippablePose3d(new Pose3d(1.5, 1.5, 0, new Rotation3d()), true), + LEFT_CORAL_SPAWN_POSE = new FlippablePose3d(new Pose3d(1.5, 6.5, 0, new Rotation3d()), true); + + private static SimulatedGamePiece createNewCoral(Pose3d startingPose) { + return new SimulatedGamePiece(startingPose, GamePieceType.CORAL); + } + + private static SimulatedGamePiece createNewAlgae(Pose3d startingPose) { + return new SimulatedGamePiece(startingPose, GamePieceType.ALGAE); + } - public static final FlippablePose3d SCORING_LOCATION = new FlippablePose3d(0, 0, 0, new Rotation3d(), true); - public static final FlippableTranslation2d FEEDER_POSITION = new FlippableTranslation2d(0, 0, true); + private static ArrayList calculatedCoralScoringLocations() { + final ArrayList coralScoringPoses = new ArrayList<>(); + for (int level = 1; level <= 4; level++) { + for (int clockFace = 2; clockFace <= 12; clockFace += 2) { + coralScoringPoses.add(calculateCoralScorePose(level, clockFace, false)); + coralScoringPoses.add(calculateCoralScorePose(level, clockFace, true)); + } + } + return coralScoringPoses; + } + + private static FlippablePose3d calculateCoralScorePose(int level, int clockFace, boolean isLeftBranch) { + final Translation3d reefCenterToLevelVector; + final Rotation3d reefToClockFaceRotation; + final Transform3d coralAlignment; + switch (level) { + case 1 -> reefCenterToLevelVector = REEF_CENTER_TO_L1_VECTOR; + case 2 -> reefCenterToLevelVector = REEF_CENTER_TO_L2_VECTOR; + case 3 -> reefCenterToLevelVector = REEF_CENTER_TO_L3_VECTOR; + case 4 -> reefCenterToLevelVector = REEF_CENTER_TO_L4_VECTOR; + default -> reefCenterToLevelVector = new Translation3d(); + } + switch (clockFace) { + case 2 -> reefToClockFaceRotation = REEF_TO_2_OCLOCK_ROTATION; + case 4 -> reefToClockFaceRotation = REEF_TO_4_OCLOCK_ROTATION; + case 6 -> reefToClockFaceRotation = REEF_TO_6_OCLOCK_ROTATION; + case 8 -> reefToClockFaceRotation = REEF_TO_8_OCLOCK_ROTATION; + case 10 -> reefToClockFaceRotation = REEF_TO_10_OCLOCK_ROTATION; + case 12 -> reefToClockFaceRotation = REEF_TO_12_OCLOCK_ROTATION; + default -> reefToClockFaceRotation = new Rotation3d(); + } + switch (level) { + case 1 -> coralAlignment = CORAL_TO_L1_ALIGNMENT; + case 2, 3 -> coralAlignment = CORAL_TO_L2_AND_L3_ALIGNMENT; + case 4 -> coralAlignment = CORAL_TO_L4_ALIGNMENT; + default -> coralAlignment = new Transform3d(); + } + return new FlippablePose3d(new Pose3d(new Pose2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, new Rotation2d())).transformBy(new Transform3d(reefCenterToLevelVector.rotateBy(reefToClockFaceRotation), reefToClockFaceRotation)).transformBy(isLeftBranch ? RIGHT_BRANCH_TO_LEFT_BRANCH : new Transform3d()).transformBy(coralAlignment), true); + } public enum GamePieceType { - GAME_PIECE_TYPE(0, 0); + ALGAE(0.2, 0), + CORAL(0.06, 1); public final double originPointHeightOffGroundMeters; public final int id; diff --git a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java index 9f40f2e4..355fca6f 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -1,11 +1,12 @@ package frc.trigon.robot.misc.simulatedfield; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.subsystems.arm.ArmConstants; +import frc.trigon.robot.subsystems.intake.IntakeConstants; +import frc.trigon.robot.subsystems.transporter.TransporterConstants; +import lib.utilities.flippable.FlippablePose3d; import org.littletonrobotics.junction.Logger; import java.util.ArrayList; @@ -14,15 +15,32 @@ * Handles the simulation of game pieces. */ public class SimulationFieldHandler { - private static final ArrayList GAME_PIECES_ON_FIELD = SimulatedGamePieceConstants.STARTING_GAME_PIECES; - private static Integer HELD_GAME_PIECE_INDEX = null; + private static final ArrayList + CORAL_ON_FIELD = SimulatedGamePieceConstants.CORAL_ON_FIELD, + ALGAE_ON_FIELD = SimulatedGamePieceConstants.ALGAE_ON_FIELD; + private static Integer + HELD_CORAL_INDEX = null, + HELD_ALGAE_INDEX = null; + private static boolean IS_CORAL_IN_END_EFFECTOR = true; - public static ArrayList getSimulatedGamePieces() { - return GAME_PIECES_ON_FIELD; + public static ArrayList getSimulatedCoral() { + return CORAL_ON_FIELD; } - public static boolean isHoldingGamePiece() { - return HELD_GAME_PIECE_INDEX != null; + public static ArrayList getSimulatedAlgae() { + return ALGAE_ON_FIELD; + } + + public static boolean isHoldingCoral() { + return HELD_CORAL_INDEX != null; + } + + public static boolean isCoralInEndEffector() { + return IS_CORAL_IN_END_EFFECTOR; + } + + public static boolean isHoldingAlgae() { + return HELD_ALGAE_INDEX != null; } public static void update() { @@ -38,38 +56,52 @@ private static void updateGamePieces() { updateCollection(); updateEjection(); updateHeldGamePiecePoses(); + updateLoad(); } /** * Logs the position of all the game pieces. */ private static void logGamePieces() { - Logger.recordOutput("Poses/GamePieces/GamePieces", mapSimulatedGamePieceListToPoseArray(GAME_PIECES_ON_FIELD)); + Logger.recordOutput("Poses/GamePieces/Corals", mapSimulatedGamePieceListToPoseArray(CORAL_ON_FIELD)); + Logger.recordOutput("Poses/GamePieces/Algae", mapSimulatedGamePieceListToPoseArray(ALGAE_ON_FIELD)); } private static void updateGamePiecesPeriodically() { - for (SimulatedGamePiece gamePiece : GAME_PIECES_ON_FIELD) - gamePiece.updatePeriodically(HELD_GAME_PIECE_INDEX != null && HELD_GAME_PIECE_INDEX == GAME_PIECES_ON_FIELD.indexOf(gamePiece)); + for (SimulatedGamePiece coral : CORAL_ON_FIELD) + coral.updatePeriodically(HELD_CORAL_INDEX != null && HELD_CORAL_INDEX == CORAL_ON_FIELD.indexOf(coral)); + for (SimulatedGamePiece algae : ALGAE_ON_FIELD) + algae.updatePeriodically(HELD_ALGAE_INDEX != null && HELD_ALGAE_INDEX == ALGAE_ON_FIELD.indexOf(algae)); } private static void updateCollection() { final Pose3d robotPose = new Pose3d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose()); - final Pose3d robotRelativeCollectionPose = new Pose3d(); - final Pose3d collectionPose = robotPose.plus(toTransform(robotRelativeCollectionPose)); + final Pose3d + coralCollectionPose = robotPose.plus(toTransform(IntakeConstants.CORAL_COLLECTION_POSE)), + algaeCollectionPose = robotPose.plus(toTransform(RobotContainer.ARM.calculateGamePieceCollectionPose())); - updateFeederCollection(robotPose); + if (isCollectingCoral() && HELD_CORAL_INDEX == null) { + HELD_CORAL_INDEX = getIndexOfCollectedGamePiece(coralCollectionPose, CORAL_ON_FIELD, SimulatedGamePieceConstants.CORAL_INTAKE_TOLERANCE_METERS); - if (isCollectingGamePiece() && HELD_GAME_PIECE_INDEX == null) - HELD_GAME_PIECE_INDEX = getIndexOfCollectedGamePiece(collectionPose, GAME_PIECES_ON_FIELD, SimulatedGamePieceConstants.INTAKE_TOLERANCE_METERS); + IS_CORAL_IN_END_EFFECTOR = false; + } + if (isCollectingAlgae() && HELD_ALGAE_INDEX == null) + HELD_ALGAE_INDEX = getIndexOfCollectedGamePiece(algaeCollectionPose, ALGAE_ON_FIELD, SimulatedGamePieceConstants.ALGAE_INTAKE_TOLERANCE_METERS); } - private static void updateFeederCollection(Pose3d robotPose) { - final double distanceFromFeeder = robotPose.toPose2d().getTranslation().getDistance(SimulatedGamePieceConstants.FEEDER_POSITION.get()); + public static void updateCoralSpawning(Pose3d robotPose) { + final double + distanceFromLeftFeeder = robotPose.toPose2d().getTranslation().getDistance(SimulatedGamePieceConstants.LEFT_FEEDER_POSITION.get()), + distanceFromRightFeeder = robotPose.toPose2d().getTranslation().getDistance(SimulatedGamePieceConstants.RIGHT_FEEDER_POSITION.get()); + final FlippablePose3d coralSpawnPose = distanceFromLeftFeeder < distanceFromRightFeeder + ? SimulatedGamePieceConstants.LEFT_CORAL_SPAWN_POSE + : SimulatedGamePieceConstants.RIGHT_CORAL_SPAWN_POSE; + CORAL_ON_FIELD.add(new SimulatedGamePiece(coralSpawnPose.get(), SimulatedGamePieceConstants.GamePieceType.CORAL)); + } - if (isCollectingGamePieceFromFeeder() && HELD_GAME_PIECE_INDEX == null && - distanceFromFeeder < SimulatedGamePieceConstants.FEEDER_INTAKE_TOLERANCE_METERS) { - GAME_PIECES_ON_FIELD.add(new SimulatedGamePiece(new Pose3d(), SimulatedGamePieceConstants.GamePieceType.GAME_PIECE_TYPE)); - HELD_GAME_PIECE_INDEX = GAME_PIECES_ON_FIELD.size() - 1; + private static void updateLoad() { + if (isCoralLoading()) { + IS_CORAL_IN_END_EFFECTOR = true; } } @@ -87,40 +119,51 @@ private static Integer getIndexOfCollectedGamePiece(Pose3d collectionPose, Array return null; } - private static boolean isCollectingGamePiece() { - return false;//TODO: Implement + private static boolean isCollectingCoral() { + return RobotContainer.INTAKE.atState(IntakeConstants.IntakeState.COLLECT); } - private static boolean isCollectingGamePieceFromFeeder() { - return false;//TODO: Implement + private static boolean isCollectingAlgae() { + return RobotContainer.ARM.atState(ArmConstants.ArmState.COLLECT_ALGAE_L2) || RobotContainer.ARM.atState(ArmConstants.ArmState.COLLECT_ALGAE_L3); } - private static void updateEjection() { - if (HELD_GAME_PIECE_INDEX != null && isEjectingGamePiece()) { - final SimulatedGamePiece heldGamePiece = GAME_PIECES_ON_FIELD.get(HELD_GAME_PIECE_INDEX); - ejectGamePiece(heldGamePiece); - HELD_GAME_PIECE_INDEX = null; - } + private static boolean isCoralLoading() { + return RobotContainer.ARM.atState(ArmConstants.ArmState.LOAD_CORAL); } - private static void ejectGamePiece(SimulatedGamePiece ejectedGamePiece) { - final ChassisSpeeds swerveWheelSpeeds = RobotContainer.SWERVE.getSelfRelativeVelocity(); - final Translation3d robotSelfRelativeVelocity = new Translation3d(swerveWheelSpeeds.vxMetersPerSecond, swerveWheelSpeeds.vyMetersPerSecond, 0); - final Translation3d robotRelativeReleaseVelocity = new Translation3d(); + private static void updateEjection() { + if (HELD_CORAL_INDEX != null) + updateCoralEjection(); - ejectedGamePiece.release(robotSelfRelativeVelocity.plus(robotRelativeReleaseVelocity).rotateBy(new Rotation3d(RobotContainer.SWERVE.getHeading()))); + if (HELD_ALGAE_INDEX != null && RobotContainer.ARM.isEjecting()) { + final SimulatedGamePiece heldAlgae = ALGAE_ON_FIELD.get(HELD_ALGAE_INDEX); + heldAlgae.release(RobotContainer.ARM.calculateLinearArmAndEndEffectorVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d()); + HELD_ALGAE_INDEX = null; + } } - private static boolean isEjectingGamePiece() { - return false;//TODO: Implement + private static void updateCoralEjection() { + final SimulatedGamePiece heldCoral = CORAL_ON_FIELD.get(HELD_CORAL_INDEX); + + if (!isCoralInEndEffector() && RobotContainer.INTAKE.atState(IntakeConstants.IntakeState.EJECT)) { + heldCoral.release(RobotContainer.INTAKE.calculateLinearIntakeVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d(), IntakeConstants.CORAL_COLLECTION_POSE.getTranslation()); + HELD_CORAL_INDEX = null; + } + if (isCoralInEndEffector() && RobotContainer.ARM.isEjecting()) { + heldCoral.release(RobotContainer.ARM.calculateLinearArmAndEndEffectorVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d(), RobotContainer.ARM.calculateGamePieceCollectionPose().getTranslation()); + HELD_CORAL_INDEX = null; + } } /** * Updates the position of the held game pieces so that they stay inside the robot. */ private static void updateHeldGamePiecePoses() { - final Pose3d robotRelativeHeldGamePiecePosition = new Pose3d(); - updateHeldGamePiecePose(robotRelativeHeldGamePiecePosition, GAME_PIECES_ON_FIELD, HELD_GAME_PIECE_INDEX); + final Pose3d + robotRelativeHeldCoralPosition = IS_CORAL_IN_END_EFFECTOR ? RobotContainer.ARM.calculateGamePieceCollectionPose() : TransporterConstants.COLLECTED_CORAL_POSE, + robotRelativeHeldAlgaePosition = RobotContainer.ARM.calculateGamePieceCollectionPose(); + updateHeldGamePiecePose(robotRelativeHeldCoralPosition, CORAL_ON_FIELD, HELD_CORAL_INDEX); + updateHeldGamePiecePose(robotRelativeHeldAlgaePosition, ALGAE_ON_FIELD, HELD_ALGAE_INDEX); } private static void updateHeldGamePiecePose(Pose3d robotRelativeHeldGamePiecePose, ArrayList gamePieceList, Integer heldGamePieceIndex) { diff --git a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationScoringHandler.java b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationScoringHandler.java index b614d694..f4891f11 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationScoringHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationScoringHandler.java @@ -4,24 +4,38 @@ import lib.utilities.flippable.FlippablePose3d; import java.util.ArrayList; -import java.util.List; public class SimulationScoringHandler { public static void checkGamePieceScored(SimulatedGamePiece gamePiece) { - final ArrayList scoreLocations = new ArrayList<>(List.of(SimulatedGamePieceConstants.SCORING_LOCATION)); + if (gamePiece.gamePieceType.equals(SimulatedGamePieceConstants.GamePieceType.CORAL)) + checkCoralScored(gamePiece); + if (gamePiece.gamePieceType.equals(SimulatedGamePieceConstants.GamePieceType.ALGAE)) + checkAlgaeScored(gamePiece); + } + + private static void checkCoralScored(SimulatedGamePiece coral) { + final ArrayList scoreLocations = SimulatedGamePieceConstants.CORAL_SCORING_LOCATIONS; for (FlippablePose3d scoreLocation : scoreLocations) { final Pose3d flippedPose = scoreLocation.get(); - if (isGamePieceScored(gamePiece, flippedPose, SimulatedGamePieceConstants.SCORING_TOLERANCE_METERS)) { - gamePiece.isScored = true; - gamePiece.updatePose(flippedPose); + if (isGamePieceScored(coral, flippedPose, SimulatedGamePieceConstants.CORAL_SCORING_TOLERANCE_METERS)) { + coral.isScored = true; + coral.updatePose(flippedPose); scoreLocations.remove(scoreLocation); return; } } } + private static void checkAlgaeScored(SimulatedGamePiece algae) { + if (!isGamePieceScored(algae, SimulatedGamePieceConstants.PROCESSOR_LOCATION, SimulatedGamePieceConstants.ALGAE_SCORING_TOLERANCE_METERS)) + return; + + algae.isScored = true; + SimulationFieldHandler.getSimulatedAlgae().remove(algae); + } + private static boolean isGamePieceScored(SimulatedGamePiece gamePiece, Pose3d scoreLocation, double scoringToleranceMeters) { final double distanceFromScoreZoneMeters = gamePiece.getDistanceFromPoseMeters(scoreLocation); return distanceFromScoreZoneMeters < scoringToleranceMeters; } -} +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index 354dd371..eb7a6b60 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; import frc.trigon.robot.subsystems.MotorSubsystem; import lib.hardware.phoenix6.cancoder.CANcoderEncoder; import lib.hardware.phoenix6.cancoder.CANcoderSignal; @@ -84,6 +85,11 @@ public void sysIDDrive(double targetVoltage) { armMasterMotor.setControl(voltageRequest.withOutput(targetVoltage)); } + public Pose3d calculateGamePieceCollectionPose() { + return calculateVisualizationPose() + .transformBy(ArmConstants.ARM_TO_HELD_GAME_PIECE); + } + public boolean isArmAboveSafeAngle() { return getAngle().getDegrees() >= ArmConstants.MAXIMUM_ARM_SAFE_ANGLE.getDegrees(); } @@ -121,6 +127,36 @@ public Rotation2d getAngle() { return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION)); } + @AutoLogOutput(key = "Arm/InEndEffector") + public boolean inEndEffector() { + return SimulationFieldHandler.isCoralInEndEffector(); + } + + public Translation3d calculateLinearArmAndEndEffectorVelocity() { + double velocityMetersPerSecond = endEffectorMotor.getSignal(TalonFXSignal.VELOCITY) * 2 * Math.PI * ArmConstants.WHEEL_RADIUS_METERS; + return calculateLinearArmVelocity().plus( + new Translation3d( + getAngle().getCos() * velocityMetersPerSecond, + getAngle().getSin() * velocityMetersPerSecond, + 0 + ) + ); + } + + public boolean isEjecting() { + return endEffectorMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE) > 2; + } + + public Translation3d calculateLinearArmVelocity() { + double velocityRotationsPerSecond = armMasterMotor.getSignal(TalonFXSignal.VELOCITY); + double velocityMagnitude = velocityRotationsPerSecond * 2 * Math.PI * ArmConstants.ARM_LENGTH_METERS; + return new Translation3d( + getAngle().getCos() * velocityMagnitude, + 0, + getAngle().getSin() * velocityMagnitude + ); + } + void setTargetState(ArmConstants.ArmState targetState) { this.isStateReversed = false; this.targetState = targetState; diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 07893783..53396fd4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -4,10 +4,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.signals.*; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.event.BooleanEvent; @@ -91,7 +88,7 @@ public class ArmConstants { END_EFFECTOR_GEAR_RATIO, END_EFFECTOR_MOMENT_OF_INERTIA ); - private static final DoubleSupplier DISTANCE_SENSOR_SIMULATION_SUPPLIER = () -> SimulationFieldHandler.isHoldingGamePiece() ? 0 : 1; + private static final DoubleSupplier DISTANCE_SENSOR_SIMULATION_SUPPLIER = () -> (SimulationFieldHandler.isHoldingCoral() && SimulationFieldHandler.isCoralInEndEffector()) || SimulationFieldHandler.isHoldingAlgae() ? 1 : 0; static final SysIdRoutine.Config ARM_SYSID_CONFIG = new SysIdRoutine.Config( Units.Volts.of(1.5).per(Units.Seconds), @@ -108,21 +105,27 @@ public class ArmConstants { "EndEffectorMechanism", END_EFFECTOR_MAXIMUM_DISPLAYABLE_VELOCITY ); - static final Pose3d ARM_VISUALIZATION_ORIGIN_POINT = new Pose3d( new Translation3d(0, -0.0954, 0.3573), new Rotation3d(0, 0, 0) ); + + static final Transform3d ARM_TO_HELD_GAME_PIECE = new Transform3d( + new Translation3d(0, 0.1, -0.5855), + new Rotation3d(0, edu.wpi.first.math.util.Units.degreesToRadians(0), 0) + ); + static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(5); /** * The highest point of the arms angular zone where the safety logic applies. */ static final Rotation2d MAXIMUM_ARM_SAFE_ANGLE = Rotation2d.fromDegrees(90); - static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(5); + private static final double COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.2; static final BooleanEvent COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), DISTANCE_SENSOR::getBinaryValue ).debounce(COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS); + static final double WHEEL_RADIUS_METERS = edu.wpi.first.math.util.Units.inchesToMeters(1.5); static { configureArmMasterMotor(); @@ -241,7 +244,7 @@ public enum ArmState { SCORE_L1(Rotation2d.fromDegrees(110), Rotation2d.fromDegrees(110), 4), SCORE_L2(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(100), 4), SCORE_L3(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(100), 4), - SCORE_L4(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(100), 4), + SCORE_L4(Rotation2d.fromDegrees(100), Rotation2d.fromDegrees(120), 4), SCORE_NET(Rotation2d.fromDegrees(160), Rotation2d.fromDegrees(160), 4), SCORE_PROCESSOR(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), 4), COLLECT_ALGAE_L2(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), -4), diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index ca527cf9..afff2a4f 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -103,6 +103,7 @@ public class ElevatorConstants { */ public static final double MAXIMUM_ELEVATOR_SAFE_ZONE_METERS = MINIMUM_ELEVATOR_SAFE_ZONE_METERS + ArmConstants.ARM_LENGTH_METERS; private static final DoubleSupplier REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER = () -> 0; + static final double ELEVATOR_POSITION_TOLERANCE_METERS = 0.02; static { configureMasterMotor(); diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index dfb6a22a..389592f2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -89,6 +89,15 @@ public boolean hasCoral() { return IntakeConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); } + public Translation3d calculateLinearIntakeVelocity() { + double velocityMetersPerSecond = intakeMotor.getSignal(TalonFXSignal.VELOCITY) * 2 * Math.PI * IntakeConstants.WHEEL_RADIUS_METERS; + return new Translation3d( + getCurrentAngle().getCos() * velocityMetersPerSecond, + getCurrentAngle().getSin() * velocityMetersPerSecond, + 0 + ); + } + void setTargetState(IntakeConstants.IntakeState targetState) { this.targetState = targetState; setTargetState(targetState.targetAngle, targetState.targetVoltage); diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index eb47449c..9113d59d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -22,6 +22,7 @@ import lib.hardware.phoenix6.talonfx.TalonFXSignal; import lib.hardware.simulation.SimpleMotorSimulation; import lib.hardware.simulation.SingleJointedArmSimulation; +import lib.utilities.Conversions; import lib.utilities.mechanisms.SingleJointedArmMechanism2d; import lib.utilities.mechanisms.SpeedMechanism2d; @@ -64,8 +65,8 @@ public class IntakeConstants { INTAKE_LENGTH_METERS = 0.365, INTAKE_MASS_KILOGRAMS = 3.26; private static final Rotation2d - MINIMUM_ANGLE = Rotation2d.fromDegrees(8.34), - MAXIMUM_ANGLE = Rotation2d.fromDegrees(70); + MINIMUM_ANGLE = Rotation2d.fromDegrees(-12), + MAXIMUM_ANGLE = Rotation2d.fromDegrees(110); private static final boolean SHOULD_SIMULATE_GRAVITY = true; private static final SimpleMotorSimulation INTAKE_SIMULATION = new SimpleMotorSimulation( INTAKE_GEARBOX, @@ -84,7 +85,7 @@ public class IntakeConstants { private static final DoubleSupplier REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER = () -> 0, FORWARD_LIMIT_SENSOR_SIMULATION_SUPPLIER = () -> 0, - DISTANCE_SENSOR_SIMULATION_SUPPLIER = () -> SimulationFieldHandler.isHoldingGamePiece() ? 0 : 1; + DISTANCE_SENSOR_SIMULATION_SUPPLIER = () -> SimulationFieldHandler.isHoldingCoral() && !SimulationFieldHandler.isCoralInEndEffector() ? 1 : 0; static final SysIdRoutine.Config ANGLE_SYSID_CONFIG = new SysIdRoutine.Config( Units.Volts.of(0.2).per(Units.Second), @@ -94,8 +95,9 @@ public class IntakeConstants { static final Pose3d INTAKE_VISUALIZATION_ORIGIN_POINT = new Pose3d( new Translation3d(0.3234, 0, 0.2944), - new Rotation3d(0, MINIMUM_ANGLE.getRadians(), 0) + new Rotation3d(0, -2, 0) ); + private static final double MAXIMUM_DISPLAYABLE_VELOCITY = 12; static final SpeedMechanism2d INTAKE_MECHANISM = new SpeedMechanism2d( "IntakeMechanism", @@ -125,6 +127,11 @@ public class IntakeConstants { CommandScheduler.getInstance().getActiveButtonLoop(), FORWARD_LIMIT_SENSOR::getBinaryValue ).debounce(FORWARD_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS); + public static Pose3d CORAL_COLLECTION_POSE = new Pose3d( + new Translation3d(0.6827, 0, 0), + new Rotation3d() + ); + static final double WHEEL_RADIUS_METERS = edu.wpi.first.math.util.Units.inchesToMeters(1.5); static { configureIntakeMotor(); @@ -211,9 +218,9 @@ private static void configureDistanceSensor() { } public enum IntakeState { - REST(0, MAXIMUM_ANGLE), - COLLECT(5, MINIMUM_ANGLE), - EJECT(-5, MINIMUM_ANGLE); + REST(0, MINIMUM_ANGLE), + COLLECT(5, MAXIMUM_ANGLE), + EJECT(-5, MAXIMUM_ANGLE); public final double targetVoltage; public final Rotation2d targetAngle; diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java index cad6cfb2..f0b3496c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -5,6 +5,7 @@ import com.pathplanner.lib.util.swerve.SwerveSetpointGenerator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; @@ -18,8 +19,6 @@ import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModule; import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModuleConstants; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; import lib.hardware.RobotHardwareStats; import lib.hardware.phoenix6.Phoenix6SignalThread; import lib.hardware.phoenix6.pigeon2.Pigeon2Gyro; @@ -27,6 +26,8 @@ import lib.utilities.flippable.Flippable; import lib.utilities.flippable.FlippablePose2d; import lib.utilities.flippable.FlippableRotation2d; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; public class Swerve extends MotorSubsystem { private final Pigeon2Gyro gyro = SwerveConstants.GYRO; @@ -93,6 +94,14 @@ public Rotation2d getHeading() { return Rotation2d.fromDegrees(SwerveConstants.GYRO.getSignal(Pigeon2Signal.YAW)); } + public Translation3d getFieldRelativeVelocity3d() { + return new Translation3d( + getFieldRelativeVelocity().vxMetersPerSecond, + getFieldRelativeVelocity().vyMetersPerSecond, + 0 + ); + } + public ChassisSpeeds getFieldRelativeVelocity() { final ChassisSpeeds selfRelativeSpeeds = getSelfRelativeVelocity(); return ChassisSpeeds.fromRobotRelativeSpeeds(selfRelativeSpeeds, RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation()); diff --git a/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java b/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java index 832190ec..3b2c2e57 100644 --- a/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java @@ -3,6 +3,9 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; +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.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.event.BooleanEvent; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -62,6 +65,10 @@ public class TransporterConstants { CommandScheduler.getInstance().getActiveButtonLoop(), BEAM_BREAK::getBinaryValue ).debounce(HAS_CORAL_DEBOUNCE_TIME_SECONDS); + public static final Pose3d COLLECTED_CORAL_POSE = new Pose3d( + new Translation3d(0, 0, 0.2), + new Rotation3d() + ); static { configureMotor(RIGHT_MOTOR, RIGHT_MOTOR_SIMULATION);