From b602dd6c73eb3022381105451f783b9a9ea7ac82 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Fri, 5 Sep 2025 02:53:15 +0300 Subject: [PATCH 01/39] added 2025 game pieces --- .../java/frc/trigon/robot/RobotContainer.java | 2 +- .../robot/constants/FieldConstants.java | 65 +++++++++- .../io/SimulationObjectDetectionCameraIO.java | 2 +- .../simulatedfield/SimulatedGamePiece.java | 7 ++ .../SimulatedGamePieceConstants.java | 114 ++++++++++++++++-- .../SimulationFieldHandler.java | 102 +++++++++------- .../SimulationScoringHandler.java | 26 +++- .../robot/subsystems/intake/Intake.java | 29 +++++ .../subsystems/intake/IntakeConstants.java | 10 +- 9 files changed, 287 insertions(+), 70 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 605eaab4..b9b18058 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -37,7 +37,7 @@ public class RobotContainer { public static final ObjectPoseEstimator OBJECT_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(); diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 9061418c..53139fae 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -4,9 +4,8 @@ import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; -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.*; +import lib.utilities.Conversions; import lib.utilities.FilesHandler; import java.io.IOException; @@ -18,7 +17,7 @@ public class FieldConstants { FIELD_WIDTH_METERS = FlippingUtil.fieldSizeY, FIELD_LENGTH_METERS = FlippingUtil.fieldSizeX; private static final List I_HATE_YOU = List.of( - //Tags to ignore + 13, 12, 16, 15, 14, 4, 5, 3, 2, 1 ); private static final boolean SHOULD_USE_HOME_TAG_LAYOUT = false; @@ -26,6 +25,16 @@ public class FieldConstants { private static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0)); public static final HashMap TAG_ID_TO_POSE = fieldLayoutToTagIDToPoseMap(); + public static final int REEF_CLOCK_POSITIONS = 6; + public static final Rotation2d REEF_CLOCK_POSITION_DIFFERENCE = Rotation2d.fromDegrees(Conversions.DEGREES_PER_ROTATIONS / REEF_CLOCK_POSITIONS); + public static final Rotation2d[] REEF_CLOCK_ANGLES = ReefClockPosition.getClockAngles(); + public static final Translation2d BLUE_REEF_CENTER_TRANSLATION = new Translation2d(4.48945, FIELD_WIDTH_METERS / 2); + public static final double + REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS = 1.3, + REEF_CENTER_TO_TARGET_SCORING_POSITION_Y_TRANSFORM_METERS = 0.17, + REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS = 1.6; + + private static AprilTagFieldLayout createAprilTagFieldLayout() { try { return SHOULD_USE_HOME_TAG_LAYOUT ? @@ -44,4 +53,50 @@ private static HashMap fieldLayoutToTagIDToPoseMap() { return tagIDToPose; } -} + + public enum ReefSide { + RIGHT(true), + LEFT(false); + + public final boolean doesFlipYTransformWhenFacingDriverStation; + + ReefSide(boolean doesFlipYTransformWhenFacingDriverStation) { + this.doesFlipYTransformWhenFacingDriverStation = doesFlipYTransformWhenFacingDriverStation; + } + + public boolean shouldFlipYTransform(ReefClockPosition reefClockPosition) { + return doesFlipYTransformWhenFacingDriverStation ^ reefClockPosition.isFacingDriverStation; // In Java, ^ acts as an XOR (exclusive OR) operator, which fits in this case + } + } + + public enum ReefClockPosition { + REEF_12_OCLOCK(true), + REEF_2_OCLOCK(true), + REEF_4_OCLOCK(true), + REEF_6_OCLOCK(true), + REEF_8_OCLOCK(true), + REEF_10_OCLOCK(true); + + public final Rotation2d clockAngle; + public final boolean isFacingDriverStation; + public final int clockPosition; + + ReefClockPosition(boolean isFacingDriverStation) { + this.clockAngle = calculateClockAngle(); + this.isFacingDriverStation = isFacingDriverStation; + this.clockPosition = ordinal() == 0 ? 12 : ordinal() * 2; + } + + public static Rotation2d[] getClockAngles() { + final Rotation2d[] clockAngles = new Rotation2d[ReefClockPosition.values().length]; + for (int i = 0; i < clockAngles.length; i++) + clockAngles[i] = ReefClockPosition.values()[i].clockAngle; + + return clockAngles; + } + + private Rotation2d calculateClockAngle() { + return REEF_CLOCK_POSITION_DIFFERENCE.times(-ordinal()); + } + } +} \ No newline at end of file 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..01366ca4 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePiece.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePiece.java @@ -40,6 +40,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..75396e51 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,115 @@ 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, + CORAL_FEEDER_INTAKE_TOLERANCE_METERS = 1, + 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); + + 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; @@ -44,4 +136,4 @@ public static String getNameFromID(int id) { return ""; } } -} +} \ No newline at end of file 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..78f5fb47 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,9 @@ 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.intake.IntakeConstants; import org.littletonrobotics.junction.Logger; import java.util.ArrayList; @@ -14,15 +12,27 @@ * 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; - 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 isHoldingAlgae() { + return HELD_ALGAE_INDEX != null; } public static void update() { @@ -44,33 +54,28 @@ private static void updateGamePieces() { * 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_CORAL_INDEX != null && HELD_CORAL_INDEX == CORAL_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(RobotContainer.INTAKE.calculateCoralCollectionPose())); +// algaeCollectionPose = robotPose.plus(toTransform(RobotContainer.ARM.calculateAlgaeCollectionPose())); - 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); - } - - private static void updateFeederCollection(Pose3d robotPose) { - final double distanceFromFeeder = robotPose.toPose2d().getTranslation().getDistance(SimulatedGamePieceConstants.FEEDER_POSITION.get()); - - 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; - } +// if (isCollectingAlgae() && HELD_ALGAE_INDEX == null) +// HELD_ALGAE_INDEX = getIndexOfCollectedGamePiece(algaeCollectionPose, ALGAE_ON_FIELD, SimulatedGamePieceConstants.ALGAE_INTAKE_TOLERANCE_METERS); } /** @@ -87,7 +92,11 @@ private static Integer getIndexOfCollectedGamePiece(Pose3d collectionPose, Array return null; } - private static boolean isCollectingGamePiece() { + private static boolean isCollectingCoral() { + return RobotContainer.INTAKE.atState(IntakeConstants.IntakeState.COLLECT); + } + + private static boolean isCollectingAlgae() { return false;//TODO: Implement } @@ -96,22 +105,30 @@ private static boolean isCollectingGamePieceFromFeeder() { } 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; - } + if (HELD_CORAL_INDEX != null) + updateCoralEjection(); + +// if (HELD_ALGAE_INDEX != null && isEjectingAlgae()) { +// final SimulatedGamePiece heldAlgae = ALGAE_ON_FIELD.get(HELD_ALGAE_INDEX); +// ejectAlgae(heldAlgae); +// HELD_ALGAE_INDEX = null; +// } } - 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 updateCoralEjection() { + final SimulatedGamePiece heldCoral = CORAL_ON_FIELD.get(HELD_CORAL_INDEX); + + if (isCollectingCoral()) { + heldCoral.release(); + HELD_CORAL_INDEX = null; + } + } - ejectedGamePiece.release(robotSelfRelativeVelocity.plus(robotRelativeReleaseVelocity).rotateBy(new Rotation3d(RobotContainer.SWERVE.getHeading()))); + private static boolean isEjectingCoral() { + return RobotContainer.INTAKE.atState(IntakeConstants.IntakeState.EJECT); } - private static boolean isEjectingGamePiece() { + private static boolean isEjectingAlgae() { return false;//TODO: Implement } @@ -119,8 +136,11 @@ private static boolean isEjectingGamePiece() { * 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 = RobotContainer.INTAKE.calculateCollectedCoralPose(); +// robotRelativeHeldAlgaePosition = RobotContainer.ARM.calculateAlgaeCollectionPose(); + 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/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index 32c94df6..1515cbb7 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -88,6 +88,28 @@ public boolean hasGamePiece() { return IntakeConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); } + /** + * Calculates the pose where the coral should actually be collected from relative to the robot. + * + * @return the pose + */ + public Pose3d calculateCoralCollectionPose() { + return calculateVisualizationPose() + .transformBy(getVisualizationToRealPitchTransform()) + .transformBy(IntakeConstants.INTAKE_ORIGIN_POINT_TO_CORAL_COLLECTION_TRANSFORM); + } + + /** + * Calculates the pose where the coral should rest inside the robot after intaking. + * + * @return the pose + */ + public Pose3d calculateCollectedCoralPose() { + return calculateVisualizationPose() + .transformBy(getVisualizationToRealPitchTransform()) + .transformBy(IntakeConstants.INTAKE_ORIGIN_POINT_TO_CORAL_VISUALIZATION_TRANSFORM); + } + void setTargetState(IntakeConstants.IntakeState targetState) { this.targetState = targetState; setTargetState(targetState.targetAngle, targetState.targetVoltage); @@ -115,6 +137,13 @@ private Pose3d calculateVisualizationPose() { return IntakeConstants.INTAKE_VISUALIZATION_ORIGIN_POINT.transformBy(transform); } + private Transform3d getVisualizationToRealPitchTransform() { + return new Transform3d( + new Translation3d(), + IntakeConstants.INTAKE_VISUALIZATION_ORIGIN_POINT.getRotation().unaryMinus() + ); + } + private Rotation2d getCurrentAngle() { return Rotation2d.fromRotations(angleMotor.getSignal(TalonFXSignal.POSITION)); } 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 560d9c9f..3d1056e3 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -5,10 +5,7 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; -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; @@ -84,7 +81,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() ? 0 : 1; static final SysIdRoutine.Config ANGLE_SYSID_CONFIG = new SysIdRoutine.Config( Units.Volts.of(0.2).per(Units.Second), @@ -125,6 +122,9 @@ public class IntakeConstants { CommandScheduler.getInstance().getActiveButtonLoop(), FORWARD_LIMIT_SENSOR::getBinaryValue ).debounce(FORWARD_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS); + static final Transform3d + INTAKE_ORIGIN_POINT_TO_CORAL_COLLECTION_TRANSFORM = new Transform3d(), + INTAKE_ORIGIN_POINT_TO_CORAL_VISUALIZATION_TRANSFORM = new Transform3d(); //TODO: get actual values static { configureIntakeMotor(); From 262c9924f1cc13683512a934219ca4f558394fc8 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Fri, 5 Sep 2025 03:12:57 +0300 Subject: [PATCH 02/39] added intake transforms --- .../robot/subsystems/intake/IntakeConstants.java | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) 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 3d1056e3..0b4db2f6 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -123,8 +123,14 @@ public class IntakeConstants { FORWARD_LIMIT_SENSOR::getBinaryValue ).debounce(FORWARD_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS); static final Transform3d - INTAKE_ORIGIN_POINT_TO_CORAL_COLLECTION_TRANSFORM = new Transform3d(), - INTAKE_ORIGIN_POINT_TO_CORAL_VISUALIZATION_TRANSFORM = new Transform3d(); //TODO: get actual values + INTAKE_ORIGIN_POINT_TO_CORAL_COLLECTION_TRANSFORM = new Transform3d( + new Translation3d(0, 23.73, 31.5), + new Rotation3d() + ), + INTAKE_ORIGIN_POINT_TO_CORAL_VISUALIZATION_TRANSFORM = new Transform3d( + new Translation3d(0, 8.98, 32.2), + new Rotation3d() + ); static { configureIntakeMotor(); From f31e4a8d9b4301c6233c0c9bccddeb0763715171 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Fri, 5 Sep 2025 05:02:34 +0300 Subject: [PATCH 03/39] Removed ENL --- src/main/java/frc/trigon/robot/constants/FieldConstants.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 53139fae..75816920 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -33,8 +33,7 @@ public class FieldConstants { REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS = 1.3, REEF_CENTER_TO_TARGET_SCORING_POSITION_Y_TRANSFORM_METERS = 0.17, REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS = 1.6; - - + private static AprilTagFieldLayout createAprilTagFieldLayout() { try { return SHOULD_USE_HOME_TAG_LAYOUT ? From 9b876eb5d5336b45a2ad73b213fbb510cef65557 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 8 Sep 2025 16:12:06 +0300 Subject: [PATCH 04/39] Sim Update (#13) * updated position for arm and intake * Update IntakeConstants.java --- .../java/frc/trigon/robot/subsystems/arm/ArmConstants.java | 2 +- .../frc/trigon/robot/subsystems/intake/IntakeConstants.java | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) 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 197dfc7e..9ce5361f 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -110,7 +110,7 @@ public class ArmConstants { ); static final Pose3d ARM_VISUALIZATION_ORIGIN_POINT = new Pose3d( - new Translation3d(0, 0.0954, 0.9517), + new Translation3d(0, -0.0954, 0.9517), new Rotation3d(0, 0, 0) ); 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 0b4db2f6..a0d5d94c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -90,9 +90,10 @@ public class IntakeConstants { ); static final Pose3d INTAKE_VISUALIZATION_ORIGIN_POINT = new Pose3d( - new Translation3d(0, 0.29449, 0.32349), - new Rotation3d(0, MINIMUM_ANGLE.getRadians(), 0) + new Translation3d(0.3234, 0, 0.2944), + new Rotation3d(0, -2.28, 0) ); + private static final double MAXIMUM_DISPLAYABLE_VELOCITY = 12; static final SpeedMechanism2d INTAKE_MECHANISM = new SpeedMechanism2d( "IntakeMechanism", From 1bdbe850aff2c68f48dd53ab552dfd652f6ccaee Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Mon, 8 Sep 2025 16:14:28 +0300 Subject: [PATCH 05/39] no errors --- src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 9ce5361f..da529930 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -91,7 +91,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.isHoldingAlgae() ? 0 : 1; static final SysIdRoutine.Config ARM_SYSID_CONFIG = new SysIdRoutine.Config( Units.Volts.of(1.5).per(Units.Seconds), From a73ff02d51785ac382b8cd931e4d9bca33f7d202 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Mon, 8 Sep 2025 17:40:02 +0300 Subject: [PATCH 06/39] name changes and cleanliness --- .../java/frc/trigon/robot/RobotContainer.java | 5 +--- ...ommand.java => CoralAutoDriveCommand.java} | 19 +++++------- .../commandclasses/IntakeAssistCommand.java | 10 +++---- .../robot/constants/FieldConstants.java | 4 +-- .../SimulationFieldHandler.java | 5 ++-- .../robot/subsystems/intake/Intake.java | 29 ------------------- .../subsystems/intake/IntakeConstants.java | 16 +++++----- .../transporter/TransporterConstants.java | 7 +++++ 8 files changed, 33 insertions(+), 62 deletions(-) rename src/main/java/frc/trigon/robot/commands/commandclasses/{GamePieceAutoDriveCommand.java => CoralAutoDriveCommand.java} (81%) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 7fb01866..51798d71 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -19,18 +19,15 @@ import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; - import frc.trigon.robot.subsystems.arm.Arm; import frc.trigon.robot.subsystems.arm.ArmCommands; import frc.trigon.robot.subsystems.arm.ArmConstants; - import frc.trigon.robot.subsystems.climber.Climber; import frc.trigon.robot.subsystems.climber.ClimberCommands; import frc.trigon.robot.subsystems.climber.ClimberConstants; import frc.trigon.robot.subsystems.intake.Intake; import frc.trigon.robot.subsystems.intake.IntakeCommands; import frc.trigon.robot.subsystems.intake.IntakeConstants; - import frc.trigon.robot.subsystems.swerve.Swerve; import frc.trigon.robot.subsystems.transporter.Transporter; import frc.trigon.robot.subsystems.transporter.TransporterCommands; @@ -40,7 +37,7 @@ 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.CORAL, 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..a1e580e8 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 VOTSL_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) + () -> VOTSL_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 = VOTSL_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 = VOTSL_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/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 75816920..a87a09f6 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -17,7 +17,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; @@ -33,7 +33,7 @@ public class FieldConstants { REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS = 1.3, REEF_CENTER_TO_TARGET_SCORING_POSITION_Y_TRANSFORM_METERS = 0.17, REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS = 1.6; - + private static AprilTagFieldLayout createAprilTagFieldLayout() { try { return SHOULD_USE_HOME_TAG_LAYOUT ? 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 78f5fb47..15aa253f 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -4,6 +4,7 @@ import edu.wpi.first.math.geometry.Transform3d; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.subsystems.intake.IntakeConstants; +import frc.trigon.robot.subsystems.transporter.TransporterConstants; import org.littletonrobotics.junction.Logger; import java.util.ArrayList; @@ -68,7 +69,7 @@ private static void updateGamePiecesPeriodically() { private static void updateCollection() { final Pose3d robotPose = new Pose3d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose()); final Pose3d - coralCollectionPose = robotPose.plus(toTransform(RobotContainer.INTAKE.calculateCoralCollectionPose())); + coralCollectionPose = robotPose.plus(toTransform(IntakeConstants.CORAL_COLLECTION_POSE)); // algaeCollectionPose = robotPose.plus(toTransform(RobotContainer.ARM.calculateAlgaeCollectionPose())); if (isCollectingCoral() && HELD_CORAL_INDEX == null) @@ -137,7 +138,7 @@ private static boolean isEjectingAlgae() { */ private static void updateHeldGamePiecePoses() { final Pose3d - robotRelativeHeldCoralPosition = RobotContainer.INTAKE.calculateCollectedCoralPose(); + robotRelativeHeldCoralPosition = TransporterConstants.COLLECTED_CORAL_POSE; // robotRelativeHeldAlgaePosition = RobotContainer.ARM.calculateAlgaeCollectionPose(); updateHeldGamePiecePose(robotRelativeHeldCoralPosition, CORAL_ON_FIELD, HELD_CORAL_INDEX); // updateHeldGamePiecePose(robotRelativeHeldAlgaePosition, ALGAE_ON_FIELD, HELD_ALGAE_INDEX); 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 1515cbb7..32c94df6 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -88,28 +88,6 @@ public boolean hasGamePiece() { return IntakeConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); } - /** - * Calculates the pose where the coral should actually be collected from relative to the robot. - * - * @return the pose - */ - public Pose3d calculateCoralCollectionPose() { - return calculateVisualizationPose() - .transformBy(getVisualizationToRealPitchTransform()) - .transformBy(IntakeConstants.INTAKE_ORIGIN_POINT_TO_CORAL_COLLECTION_TRANSFORM); - } - - /** - * Calculates the pose where the coral should rest inside the robot after intaking. - * - * @return the pose - */ - public Pose3d calculateCollectedCoralPose() { - return calculateVisualizationPose() - .transformBy(getVisualizationToRealPitchTransform()) - .transformBy(IntakeConstants.INTAKE_ORIGIN_POINT_TO_CORAL_VISUALIZATION_TRANSFORM); - } - void setTargetState(IntakeConstants.IntakeState targetState) { this.targetState = targetState; setTargetState(targetState.targetAngle, targetState.targetVoltage); @@ -137,13 +115,6 @@ private Pose3d calculateVisualizationPose() { return IntakeConstants.INTAKE_VISUALIZATION_ORIGIN_POINT.transformBy(transform); } - private Transform3d getVisualizationToRealPitchTransform() { - return new Transform3d( - new Translation3d(), - IntakeConstants.INTAKE_VISUALIZATION_ORIGIN_POINT.getRotation().unaryMinus() - ); - } - private Rotation2d getCurrentAngle() { return Rotation2d.fromRotations(angleMotor.getSignal(TalonFXSignal.POSITION)); } 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 a0d5d94c..5827d69d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -5,7 +5,10 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; -import edu.wpi.first.math.geometry.*; +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.system.plant.DCMotor; import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.event.BooleanEvent; @@ -123,15 +126,10 @@ public class IntakeConstants { CommandScheduler.getInstance().getActiveButtonLoop(), FORWARD_LIMIT_SENSOR::getBinaryValue ).debounce(FORWARD_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS); - static final Transform3d - INTAKE_ORIGIN_POINT_TO_CORAL_COLLECTION_TRANSFORM = new Transform3d( - new Translation3d(0, 23.73, 31.5), + public static Pose3d CORAL_COLLECTION_POSE = new Pose3d( + new Translation3d(-23.58, 23.73, -20.22), new Rotation3d() - ), - INTAKE_ORIGIN_POINT_TO_CORAL_VISUALIZATION_TRANSFORM = new Transform3d( - new Translation3d(0, 8.98, 32.2), - new Rotation3d() - ); + ); static { configureIntakeMotor(); 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..04c42627 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(-24.11, 8.98, -20.68), + new Rotation3d() + ); static { configureMotor(RIGHT_MOTOR, RIGHT_MOTOR_SIMULATION); From a1b18b5e704be40f1f6ac3758f5acf4c2508c833 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Mon, 8 Sep 2025 18:30:18 +0300 Subject: [PATCH 07/39] add velocities --- .../misc/simulatedfield/SimulatedGamePiece.java | 8 ++++++-- .../misc/simulatedfield/SimulationFieldHandler.java | 2 +- .../java/frc/trigon/robot/subsystems/arm/Arm.java | 10 ++++++++++ .../trigon/robot/subsystems/arm/ArmConstants.java | 1 + .../frc/trigon/robot/subsystems/swerve/Swerve.java | 13 +++++++++++-- 5 files changed, 29 insertions(+), 5 deletions(-) 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 01366ca4..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(); 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 15aa253f..e83630e8 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -120,7 +120,7 @@ private static void updateCoralEjection() { final SimulatedGamePiece heldCoral = CORAL_ON_FIELD.get(HELD_CORAL_INDEX); if (isCollectingCoral()) { - heldCoral.release(); + heldCoral.release(RobotContainer.ARM.calculateLinearEndEffectorVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d()); //TODO add arm velocity @Shmaya HELD_CORAL_INDEX = null; } } 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 d191f552..6c9bb503 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -6,6 +6,7 @@ import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.robot.RobotContainer; import frc.trigon.robot.subsystems.MotorSubsystem; import lib.hardware.phoenix6.cancoder.CANcoderEncoder; import lib.hardware.phoenix6.cancoder.CANcoderSignal; @@ -93,6 +94,15 @@ public boolean hasGamePiece() { return ArmConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); } + public Translation3d calculateLinearEndEffectorVelocity() { + double velocityRotationsPerSecond = endEffectorMotor.getSignal(TalonFXSignal.VELOCITY) * ArmConstants.WHEEL_RADIUS_METERS; + return new Translation3d( + Math.cos(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getRadians()) * velocityRotationsPerSecond, + Math.sin(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getRadians()) * velocityRotationsPerSecond, + 0 + ); + } + void setTargetState(ArmConstants.ArmState targetState, boolean isStateReversed) { if (isStateReversed) { setTargetState( 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 da529930..920dccd1 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -120,6 +120,7 @@ public class ArmConstants { 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(); 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()); From 636dc72120882fc49737bdf75800f348d9d713d4 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Wed, 10 Sep 2025 18:35:46 +0300 Subject: [PATCH 08/39] set arm to move with elevator --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 2 +- src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java | 2 +- .../java/frc/trigon/robot/subsystems/elevator/Elevator.java | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) 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 6c9bb503..370ee969 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -142,7 +142,7 @@ private void setTargetVoltage(double targetVoltage) { private Pose3d calculateVisualizationPose() { final Transform3d armTransform = new Transform3d( - new Translation3d(), + new Translation3d(0, 0, RobotContainer.ELEVATOR.getPositionMeters()), new Rotation3d(0, getAngle().getRadians(), 0) ); return ArmConstants.ARM_VISUALIZATION_ORIGIN_POINT.transformBy(armTransform); 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 920dccd1..417d3540 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -110,7 +110,7 @@ public class ArmConstants { ); static final Pose3d ARM_VISUALIZATION_ORIGIN_POINT = new Pose3d( - new Translation3d(0, -0.0954, 0.9517), + new Translation3d(0, -0.0954, 0.35), new Rotation3d(0, 0, 0) ); diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index 7063a1d5..0209f807 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -117,7 +117,7 @@ private void scalePositionRequestSpeed(double speedScalar) { positionRequest.Jerk = positionRequest.Acceleration * 10; } - private double getPositionMeters() { + public double getPositionMeters() { return rotationsToMeters(getPositionRotations()); } From a8987e0f3149796bc9d1207c07ad911841e4bc66 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Wed, 10 Sep 2025 19:27:24 +0300 Subject: [PATCH 09/39] added algae sim stuff --- .../SimulationFieldHandler.java | 29 ++++++++++--------- .../frc/trigon/robot/subsystems/arm/Arm.java | 5 ++++ .../robot/subsystems/arm/ArmConstants.java | 12 ++++---- 3 files changed, 27 insertions(+), 19 deletions(-) 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 e83630e8..a8178e94 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform3d; 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 org.littletonrobotics.junction.Logger; @@ -69,14 +70,14 @@ private static void updateGamePiecesPeriodically() { private static void updateCollection() { final Pose3d robotPose = new Pose3d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose()); final Pose3d - coralCollectionPose = robotPose.plus(toTransform(IntakeConstants.CORAL_COLLECTION_POSE)); -// algaeCollectionPose = robotPose.plus(toTransform(RobotContainer.ARM.calculateAlgaeCollectionPose())); + coralCollectionPose = robotPose.plus(toTransform(IntakeConstants.CORAL_COLLECTION_POSE)), + algaeCollectionPose = robotPose.plus(toTransform(RobotContainer.ARM.calculateAlgaeCollectionPose())); if (isCollectingCoral() && HELD_CORAL_INDEX == null) HELD_CORAL_INDEX = getIndexOfCollectedGamePiece(coralCollectionPose, CORAL_ON_FIELD, SimulatedGamePieceConstants.CORAL_INTAKE_TOLERANCE_METERS); -// if (isCollectingAlgae() && HELD_ALGAE_INDEX == null) -// HELD_ALGAE_INDEX = getIndexOfCollectedGamePiece(algaeCollectionPose, ALGAE_ON_FIELD, SimulatedGamePieceConstants.ALGAE_INTAKE_TOLERANCE_METERS); + if (isCollectingAlgae() && HELD_ALGAE_INDEX == null) + HELD_ALGAE_INDEX = getIndexOfCollectedGamePiece(algaeCollectionPose, ALGAE_ON_FIELD, SimulatedGamePieceConstants.ALGAE_INTAKE_TOLERANCE_METERS); } /** @@ -98,7 +99,7 @@ private static boolean isCollectingCoral() { } private static boolean isCollectingAlgae() { - return false;//TODO: Implement + return RobotContainer.ARM.atState(ArmConstants.ArmState.COLLECT_ALGAE_L2) || RobotContainer.ARM.atState(ArmConstants.ArmState.COLLECT_ALGAE_L3); } private static boolean isCollectingGamePieceFromFeeder() { @@ -109,11 +110,11 @@ private static void updateEjection() { if (HELD_CORAL_INDEX != null) updateCoralEjection(); -// if (HELD_ALGAE_INDEX != null && isEjectingAlgae()) { -// final SimulatedGamePiece heldAlgae = ALGAE_ON_FIELD.get(HELD_ALGAE_INDEX); -// ejectAlgae(heldAlgae); -// HELD_ALGAE_INDEX = null; -// } + if (HELD_ALGAE_INDEX != null && isEjectingAlgae()) { + final SimulatedGamePiece heldAlgae = ALGAE_ON_FIELD.get(HELD_ALGAE_INDEX); + heldAlgae.release(RobotContainer.ARM.calculateLinearEndEffectorVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d()); + HELD_ALGAE_INDEX = null; + } } private static void updateCoralEjection() { @@ -130,7 +131,7 @@ private static boolean isEjectingCoral() { } private static boolean isEjectingAlgae() { - return false;//TODO: Implement + return RobotContainer.ARM.atState(ArmConstants.ArmState.REST);//TODO: switch with eject state once its merged into main } /** @@ -138,10 +139,10 @@ private static boolean isEjectingAlgae() { */ private static void updateHeldGamePiecePoses() { final Pose3d - robotRelativeHeldCoralPosition = TransporterConstants.COLLECTED_CORAL_POSE; -// robotRelativeHeldAlgaePosition = RobotContainer.ARM.calculateAlgaeCollectionPose(); + robotRelativeHeldCoralPosition = TransporterConstants.COLLECTED_CORAL_POSE, + robotRelativeHeldAlgaePosition = RobotContainer.ARM.calculateAlgaeCollectionPose(); updateHeldGamePiecePose(robotRelativeHeldCoralPosition, CORAL_ON_FIELD, HELD_CORAL_INDEX); -// updateHeldGamePiecePose(robotRelativeHeldAlgaePosition, ALGAE_ON_FIELD, HELD_ALGAE_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/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index 370ee969..542d2ee6 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -81,6 +81,11 @@ public void sysIDDrive(double targetVoltage) { armMasterMotor.setControl(voltageRequest.withOutput(targetVoltage)); } + public Pose3d calculateAlgaeCollectionPose() { + return calculateVisualizationPose() + .transformBy(ArmConstants.ARM_TO_HELD_ALGAE); + } + public boolean atState(ArmConstants.ArmState targetState) { return this.targetState == targetState && atTargetAngle(); } 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 417d3540..50194f69 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; @@ -110,10 +107,15 @@ public class ArmConstants { ); static final Pose3d ARM_VISUALIZATION_ORIGIN_POINT = new Pose3d( - new Translation3d(0, -0.0954, 0.35), + new Translation3d(0, -0.0954, 0.3573), new Rotation3d(0, 0, 0) ); + static final Transform3d ARM_TO_HELD_ALGAE = new Transform3d( + new Translation3d(0, 0, -0.5855), + new Rotation3d(0, edu.wpi.first.math.util.Units.degreesToRadians(0), 0) + ); + static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(0); private static final double COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.2; static final BooleanEvent COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( From 375c160bf8f3405311417e11f3525974004f8def Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Wed, 10 Sep 2025 19:37:35 +0300 Subject: [PATCH 10/39] added arn velocity --- .../misc/simulatedfield/SimulationFieldHandler.java | 4 ++-- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 9 +++++++++ 2 files changed, 11 insertions(+), 2 deletions(-) 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 a8178e94..0f7e64b6 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -112,7 +112,7 @@ private static void updateEjection() { if (HELD_ALGAE_INDEX != null && isEjectingAlgae()) { final SimulatedGamePiece heldAlgae = ALGAE_ON_FIELD.get(HELD_ALGAE_INDEX); - heldAlgae.release(RobotContainer.ARM.calculateLinearEndEffectorVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d()); + heldAlgae.release(RobotContainer.ARM.calculateLinearEndEffectorVelocity(), RobotContainer.ARM.calculateLinearArmVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d()); HELD_ALGAE_INDEX = null; } } @@ -121,7 +121,7 @@ private static void updateCoralEjection() { final SimulatedGamePiece heldCoral = CORAL_ON_FIELD.get(HELD_CORAL_INDEX); if (isCollectingCoral()) { - heldCoral.release(RobotContainer.ARM.calculateLinearEndEffectorVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d()); //TODO add arm velocity @Shmaya + heldCoral.release(RobotContainer.ARM.calculateLinearEndEffectorVelocity(), RobotContainer.ARM.calculateLinearArmVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d()); //TODO add arm velocity @Shmaya HELD_CORAL_INDEX = null; } } 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 542d2ee6..7f564aa4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -108,6 +108,15 @@ public Translation3d calculateLinearEndEffectorVelocity() { ); } + public Translation3d calculateLinearArmVelocity() { + double velocityRotationsPerSecond = armMasterMotor.getSignal(TalonFXSignal.VELOCITY); + return new Translation3d( + Math.cos(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getRadians()) * velocityRotationsPerSecond, + Math.sin(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getRadians()) * velocityRotationsPerSecond, + 0 + ); + } + void setTargetState(ArmConstants.ArmState targetState, boolean isStateReversed) { if (isStateReversed) { setTargetState( From f4cb92148cec65286e9130dc9b9c7f3523ecd91e Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Wed, 10 Sep 2025 19:39:03 +0300 Subject: [PATCH 11/39] removed reduntant function --- .../robot/misc/simulatedfield/SimulationFieldHandler.java | 4 ---- 1 file changed, 4 deletions(-) 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 0f7e64b6..59f6dc73 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -102,10 +102,6 @@ private static boolean isCollectingAlgae() { return RobotContainer.ARM.atState(ArmConstants.ArmState.COLLECT_ALGAE_L2) || RobotContainer.ARM.atState(ArmConstants.ArmState.COLLECT_ALGAE_L3); } - private static boolean isCollectingGamePieceFromFeeder() { - return false;//TODO: Implement - } - private static void updateEjection() { if (HELD_CORAL_INDEX != null) updateCoralEjection(); From ff15c976382523e883bc1ce0bef56ac4385e8ad4 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Wed, 10 Sep 2025 22:01:54 +0300 Subject: [PATCH 12/39] weird stuff --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) 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 0a372fcf..5d01dbb8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -85,7 +85,8 @@ public void sysIDDrive(double targetVoltage) { public Pose3d calculateAlgaeCollectionPose() { return calculateVisualizationPose() .transformBy(ArmConstants.ARM_TO_HELD_ALGAE); - + } + public boolean atState(ArmConstants.ArmState targetState, boolean isStateReversed) { if (isStateReversed) return this.targetState == targetState && atTargetAngle(isStateReversed); @@ -129,6 +130,7 @@ public Translation3d calculateLinearArmVelocity() { Math.sin(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getRadians()) * velocityRotationsPerSecond, 0 ); + } void setTargetState(ArmConstants.ArmState targetState) { this.isStateReversed = false; @@ -164,7 +166,7 @@ void prepareForState(ArmConstants.ArmState targetState) { void prepareForState(ArmConstants.ArmState targetState, boolean isStateReversed) { this.isStateReversed = isStateReversed; this.targetState = targetState; - + if (isStateReversed) { setTargetAngle(ArmConstants.FULL_ROTATION.minus(targetState.targetAngle)); return; From d1c70d4a00ed8f92c3cfaea3394376ee4bced14e Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Wed, 10 Sep 2025 22:03:33 +0300 Subject: [PATCH 13/39] added eject state --- .../robot/misc/simulatedfield/SimulationFieldHandler.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 59f6dc73..0b940d26 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -117,7 +117,7 @@ private static void updateCoralEjection() { final SimulatedGamePiece heldCoral = CORAL_ON_FIELD.get(HELD_CORAL_INDEX); if (isCollectingCoral()) { - heldCoral.release(RobotContainer.ARM.calculateLinearEndEffectorVelocity(), RobotContainer.ARM.calculateLinearArmVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d()); //TODO add arm velocity @Shmaya + heldCoral.release(RobotContainer.ARM.calculateLinearEndEffectorVelocity(), RobotContainer.ARM.calculateLinearArmVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d()); HELD_CORAL_INDEX = null; } } @@ -127,7 +127,7 @@ private static boolean isEjectingCoral() { } private static boolean isEjectingAlgae() { - return RobotContainer.ARM.atState(ArmConstants.ArmState.REST);//TODO: switch with eject state once its merged into main + return RobotContainer.ARM.atState(ArmConstants.ArmState.EJECT); } /** From 115a5997dbc3f7ccab19c1cfae6ecae1d3b0fd45 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Thu, 11 Sep 2025 14:21:20 +0300 Subject: [PATCH 14/39] wthelly --- .../commands/commandclasses/CoralAutoDriveCommand.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/CoralAutoDriveCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/CoralAutoDriveCommand.java index a1e580e8..026e0680 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/CoralAutoDriveCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/CoralAutoDriveCommand.java @@ -15,7 +15,7 @@ import java.util.function.Supplier; public class CoralAutoDriveCommand extends ParallelCommandGroup { - private static final ObjectPoseEstimator VOTSL_POSE_ESTIMATOR = RobotContainer.CORAL_POSE_ESTIMATOR; + private static final ObjectPoseEstimator CORAL_POSE_ESTIMATOR = RobotContainer.CORAL_POSE_ESTIMATOR; private Translation2d distanceFromTrackedGamePiece; public CoralAutoDriveCommand() { @@ -24,7 +24,7 @@ public CoralAutoDriveCommand() { GeneralCommands.getContinuousConditionalCommand( getDriveToGamePieceCommand(() -> distanceFromTrackedGamePiece), GeneralCommands.getFieldRelativeDriveCommand(), - () -> VOTSL_POSE_ESTIMATOR.getClosestObjectToRobot() != null && shouldMoveTowardsGamePiece(distanceFromTrackedGamePiece) + () -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null && shouldMoveTowardsGamePiece(distanceFromTrackedGamePiece) ) ); } @@ -37,7 +37,7 @@ private Command getTrackGamePieceCommand() { public static Translation2d calculateDistanceFromTrackedGamePiece() { final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); - final Translation2d trackedObjectPositionOnField = VOTSL_POSE_ESTIMATOR.getClosestObjectToRobot(); + final Translation2d trackedObjectPositionOnField = CORAL_POSE_ESTIMATOR.getClosestObjectToRobot(); if (trackedObjectPositionOnField == null) return null; @@ -66,7 +66,7 @@ public static boolean shouldMoveTowardsGamePiece(Translation2d distanceFromTrack public static FlippableRotation2d calculateTargetAngle() { final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); - final Translation2d trackedObjectFieldRelativePosition = VOTSL_POSE_ESTIMATOR.getClosestObjectToRobot(); + final Translation2d trackedObjectFieldRelativePosition = CORAL_POSE_ESTIMATOR.getClosestObjectToRobot(); if (trackedObjectFieldRelativePosition == null) return null; From 27ab709cc258f6ede28d591c4d01c1d017153d51 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 11 Sep 2025 14:22:39 +0300 Subject: [PATCH 15/39] moved public func to where it should be --- .../trigon/robot/subsystems/elevator/Elevator.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index 0209f807..c18ea213 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -10,10 +10,10 @@ import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.subsystems.MotorSubsystem; -import org.littletonrobotics.junction.Logger; import lib.hardware.phoenix6.talonfx.TalonFXMotor; import lib.hardware.phoenix6.talonfx.TalonFXSignal; import lib.utilities.Conversions; +import org.littletonrobotics.junction.Logger; public class Elevator extends MotorSubsystem { private final TalonFXMotor masterMotor = ElevatorConstants.MASTER_MOTOR; @@ -23,7 +23,7 @@ public class Elevator extends MotorSubsystem { ElevatorConstants.DEFAULT_MAXIMUM_VELOCITY, ElevatorConstants.DEFAULT_MAXIMUM_ACCELERATION, ElevatorConstants.DEFAULT_MAXIMUM_ACCELERATION * 10 - ).withEnableFOC(ElevatorConstants.FOC_ENABLED); + ).withEnableFOC(ElevatorConstants.FOC_ENABLED); private ElevatorConstants.ElevatorState targetState = ElevatorConstants.ElevatorState.REST; public Elevator() { @@ -75,6 +75,10 @@ public void updatePeriodically() { Logger.recordOutput("Elevator/CurrentPositionMeters", getPositionMeters()); } + public double getPositionMeters() { + return rotationsToMeters(getPositionRotations()); + } + void setTargetState(ElevatorConstants.ElevatorState targetState) { this.targetState = targetState; scalePositionRequestSpeed(targetState.speedScalar); @@ -117,10 +121,6 @@ private void scalePositionRequestSpeed(double speedScalar) { positionRequest.Jerk = positionRequest.Acceleration * 10; } - public double getPositionMeters() { - return rotationsToMeters(getPositionRotations()); - } - private double rotationsToMeters(double positionsRotations) { return Conversions.rotationsToDistance(positionsRotations, ElevatorConstants.DRUM_DIAMETER_METERS); } From a92c81e697ce022c02dc61c69d7a6932b217ba48 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 11 Sep 2025 14:33:26 +0300 Subject: [PATCH 16/39] Update SimulatedGamePieceConstants.java --- .../robot/misc/simulatedfield/SimulatedGamePieceConstants.java | 1 - 1 file changed, 1 deletion(-) 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 75396e51..1cb26840 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java @@ -13,7 +13,6 @@ public class SimulatedGamePieceConstants { public static final double G_FORCE = 9.806; public static final double CORAL_INTAKE_TOLERANCE_METERS = 0.3, - CORAL_FEEDER_INTAKE_TOLERANCE_METERS = 1, ALGAE_INTAKE_TOLERANCE_METERS = 0.3, CORAL_SCORING_TOLERANCE_METERS = 0.1, ALGAE_SCORING_TOLERANCE_METERS = 0.2; From 76a0a8ed2c53e2cef65562f82a43bfd1cd33802d Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 11 Sep 2025 19:08:46 +0300 Subject: [PATCH 17/39] updated vekocity functions --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) 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 5d01dbb8..1903d00a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -115,16 +115,18 @@ public boolean hasGamePiece() { } public Translation3d calculateLinearEndEffectorVelocity() { - double velocityRotationsPerSecond = endEffectorMotor.getSignal(TalonFXSignal.VELOCITY) * ArmConstants.WHEEL_RADIUS_METERS; + double velocityMetersPerSecond = endEffectorMotor.getSignal(TalonFXSignal.VELOCITY) * 2 * Math.PI * ArmConstants.WHEEL_RADIUS_METERS; + Rotation2d robotRotation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation(); return new Translation3d( - Math.cos(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getRadians()) * velocityRotationsPerSecond, - Math.sin(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getRadians()) * velocityRotationsPerSecond, + robotRotation.getCos() * velocityMetersPerSecond, + robotRotation.getSin() * velocityMetersPerSecond, 0 ); } public Translation3d calculateLinearArmVelocity() { double velocityRotationsPerSecond = armMasterMotor.getSignal(TalonFXSignal.VELOCITY); + Rotation2d robotRotation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation(); return new Translation3d( Math.cos(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getRadians()) * velocityRotationsPerSecond, Math.sin(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getRadians()) * velocityRotationsPerSecond, From 5d9a963471f0e6c1f565308ec9ca32ab480fc0ae Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 11 Sep 2025 19:42:19 +0300 Subject: [PATCH 18/39] set endeffector velocity to already include arm velocity --- .../SimulationFieldHandler.java | 4 ++-- .../frc/trigon/robot/subsystems/arm/Arm.java | 19 ++++++++++--------- .../robot/subsystems/arm/ArmConstants.java | 2 +- 3 files changed, 13 insertions(+), 12 deletions(-) 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 0b940d26..c0ec7e06 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -108,7 +108,7 @@ private static void updateEjection() { if (HELD_ALGAE_INDEX != null && isEjectingAlgae()) { final SimulatedGamePiece heldAlgae = ALGAE_ON_FIELD.get(HELD_ALGAE_INDEX); - heldAlgae.release(RobotContainer.ARM.calculateLinearEndEffectorVelocity(), RobotContainer.ARM.calculateLinearArmVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d()); + heldAlgae.release(RobotContainer.ARM.calculateLinearArmAndEndEffectorVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d()); HELD_ALGAE_INDEX = null; } } @@ -117,7 +117,7 @@ private static void updateCoralEjection() { final SimulatedGamePiece heldCoral = CORAL_ON_FIELD.get(HELD_CORAL_INDEX); if (isCollectingCoral()) { - heldCoral.release(RobotContainer.ARM.calculateLinearEndEffectorVelocity(), RobotContainer.ARM.calculateLinearArmVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d()); + heldCoral.release(RobotContainer.ARM.calculateLinearArmAndEndEffectorVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d()); HELD_CORAL_INDEX = null; } } 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 1903d00a..7b387289 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -114,22 +114,23 @@ public boolean hasGamePiece() { return ArmConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); } - public Translation3d calculateLinearEndEffectorVelocity() { + public Translation3d calculateLinearArmAndEndEffectorVelocity() { double velocityMetersPerSecond = endEffectorMotor.getSignal(TalonFXSignal.VELOCITY) * 2 * Math.PI * ArmConstants.WHEEL_RADIUS_METERS; - Rotation2d robotRotation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation(); - return new Translation3d( - robotRotation.getCos() * velocityMetersPerSecond, - robotRotation.getSin() * velocityMetersPerSecond, - 0 + return calculateLinearArmVelocity().plus( + new Translation3d( + getAngle().getCos() * velocityMetersPerSecond, + getAngle().getSin() * velocityMetersPerSecond, + 0 + ) ); } public Translation3d calculateLinearArmVelocity() { double velocityRotationsPerSecond = armMasterMotor.getSignal(TalonFXSignal.VELOCITY); - Rotation2d robotRotation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation(); + double velocityMagnitude = velocityRotationsPerSecond * 2 * Math.PI * ArmConstants.ARM_LENGTH_METERS; return new Translation3d( - Math.cos(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getRadians()) * velocityRotationsPerSecond, - Math.sin(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().getRadians()) * velocityRotationsPerSecond, + getAngle().getCos() * velocityMagnitude, + getAngle().getSin() * velocityMagnitude, 0 ); } 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 2ab2db53..8cee9b02 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -59,6 +59,7 @@ public class ArmConstants { ARM_DEFAULT_MAXIMUM_JERK = ARM_DEFAULT_MAXIMUM_ACCELERATION * 10; static final boolean FOC_ENABLED = true; + static final double ARM_LENGTH_METERS = 0.67; private static final int ARM_MOTOR_AMOUNT = 2, END_EFFECTOR_MOTOR_AMOUNT = 1; @@ -66,7 +67,6 @@ public class ArmConstants { ARM_GEARBOX = DCMotor.getKrakenX60Foc(ARM_MOTOR_AMOUNT), END_EFFECTOR_GEARBOX = DCMotor.getKrakenX60Foc(END_EFFECTOR_MOTOR_AMOUNT); private static final double - ARM_LENGTH_METERS = 0.67, ARM_MASS_KILOGRAMS = 3.5, END_EFFECTOR_MOMENT_OF_INERTIA = 0.003, END_EFFECTOR_MAXIMUM_DISPLAYABLE_VELOCITY = 12; From f14aeb9099733d62efdbf1f8b6e2366038058d33 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 11 Sep 2025 20:07:18 +0300 Subject: [PATCH 19/39] broke down return statement --- .../simulatedfield/SimulatedGamePieceConstants.java | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) 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 1cb26840..293809b9 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java @@ -113,7 +113,18 @@ private static FlippablePose3d calculateCoralScorePose(int level, int clockFace, 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); + + final Pose3d reefCenterPose = new Pose3d(new Pose2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, new Rotation2d())); + final Transform3d reefToClockAndLevel = new Transform3d( + reefCenterToLevelVector.rotateBy(reefToClockFaceRotation), + reefToClockFaceRotation + ); + final Transform3d branch = isLeftBranch ? RIGHT_BRANCH_TO_LEFT_BRANCH : new Transform3d(); + final Pose3d target = reefCenterPose + .transformBy(reefToClockAndLevel) + .transformBy(branch) + .transformBy(coralAlignment); + return new FlippablePose3d(target, true); } public enum GamePieceType { From e98d01688873c03a71b32da14730e472de124a6b Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 11 Sep 2025 20:10:25 +0300 Subject: [PATCH 20/39] Update SimulationFieldHandler.java --- .../robot/misc/simulatedfield/SimulationFieldHandler.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 c0ec7e06..6e319d19 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -64,7 +64,7 @@ private static void updateGamePiecesPeriodically() { 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_CORAL_INDEX != null && HELD_CORAL_INDEX == CORAL_ON_FIELD.indexOf(algae)); + algae.updatePeriodically(HELD_ALGAE_INDEX != null && HELD_ALGAE_INDEX == ALGAE_ON_FIELD.indexOf(algae)); } private static void updateCollection() { From eb5d7a8a48a4216d8368146f18205319e3c61297 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 11 Sep 2025 20:19:19 +0300 Subject: [PATCH 21/39] quick fix --- .../robot/misc/simulatedfield/SimulationFieldHandler.java | 2 +- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) 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 6e319d19..d6b4bd61 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -116,7 +116,7 @@ private static void updateEjection() { private static void updateCoralEjection() { final SimulatedGamePiece heldCoral = CORAL_ON_FIELD.get(HELD_CORAL_INDEX); - if (isCollectingCoral()) { + if (isEjectingCoral()) { heldCoral.release(RobotContainer.ARM.calculateLinearArmAndEndEffectorVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d()); HELD_CORAL_INDEX = null; } 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 7b387289..52da094c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -130,8 +130,8 @@ public Translation3d calculateLinearArmVelocity() { double velocityMagnitude = velocityRotationsPerSecond * 2 * Math.PI * ArmConstants.ARM_LENGTH_METERS; return new Translation3d( getAngle().getCos() * velocityMagnitude, - getAngle().getSin() * velocityMagnitude, - 0 + 0, + getAngle().getSin() * velocityMagnitude ); } From 3b34a090e27a33bc4e259731d8d9db12d7aa055e Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Thu, 11 Sep 2025 23:46:14 +0300 Subject: [PATCH 22/39] Update SimulatedGamePieceConstants.java --- .../SimulatedGamePieceConstants.java | 15 ++------------- 1 file changed, 2 insertions(+), 13 deletions(-) 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 293809b9..e191e506 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java @@ -113,18 +113,7 @@ private static FlippablePose3d calculateCoralScorePose(int level, int clockFace, case 4 -> coralAlignment = CORAL_TO_L4_ALIGNMENT; default -> coralAlignment = new Transform3d(); } - - final Pose3d reefCenterPose = new Pose3d(new Pose2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, new Rotation2d())); - final Transform3d reefToClockAndLevel = new Transform3d( - reefCenterToLevelVector.rotateBy(reefToClockFaceRotation), - reefToClockFaceRotation - ); - final Transform3d branch = isLeftBranch ? RIGHT_BRANCH_TO_LEFT_BRANCH : new Transform3d(); - final Pose3d target = reefCenterPose - .transformBy(reefToClockAndLevel) - .transformBy(branch) - .transformBy(coralAlignment); - return new FlippablePose3d(target, true); + 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 { @@ -146,4 +135,4 @@ public static String getNameFromID(int id) { return ""; } } -} \ No newline at end of file +} From 1c74f9e00d06d7a95156c3c56187b1c47b83fb1d Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 12 Sep 2025 00:11:05 +0300 Subject: [PATCH 23/39] Update Robot.java --- src/main/java/frc/trigon/robot/Robot.java | 2 ++ 1 file changed, 2 insertions(+) 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 From d65d0ce828fec04202b4b029c5b78b3fafe013ba Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 12 Sep 2025 00:25:52 +0300 Subject: [PATCH 24/39] fixed mistake --- src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java | 2 +- .../frc/trigon/robot/subsystems/intake/IntakeConstants.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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 8cee9b02..20d77a67 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -88,7 +88,7 @@ public class ArmConstants { END_EFFECTOR_GEAR_RATIO, END_EFFECTOR_MOMENT_OF_INERTIA ); - private static final DoubleSupplier DISTANCE_SENSOR_SIMULATION_SUPPLIER = () -> SimulationFieldHandler.isHoldingCoral() || SimulationFieldHandler.isHoldingAlgae() ? 0 : 1; + private static final DoubleSupplier DISTANCE_SENSOR_SIMULATION_SUPPLIER = () -> SimulationFieldHandler.isHoldingCoral() || SimulationFieldHandler.isHoldingAlgae() ? 1 : 0; static final SysIdRoutine.Config ARM_SYSID_CONFIG = new SysIdRoutine.Config( Units.Volts.of(1.5).per(Units.Seconds), 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 5827d69d..3809323c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -84,7 +84,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.isHoldingCoral() ? 0 : 1; + DISTANCE_SENSOR_SIMULATION_SUPPLIER = () -> SimulationFieldHandler.isHoldingCoral() ? 1 : 0; static final SysIdRoutine.Config ANGLE_SYSID_CONFIG = new SysIdRoutine.Config( Units.Volts.of(0.2).per(Units.Second), From a988c046054a3795d46d76a98f2d0957543a45b0 Mon Sep 17 00:00:00 2001 From: Nummun14 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 11 Sep 2025 18:24:03 -0400 Subject: [PATCH 25/39] made intaking in the sim work --- src/main/java/frc/trigon/robot/RobotContainer.java | 2 ++ .../commandfactories/CoralCollectionCommands.java | 4 ++-- .../commandfactories/CoralEjectionCommands.java | 2 +- .../misc/simulatedfield/SimulationFieldHandler.java | 2 +- .../frc/trigon/robot/subsystems/arm/ArmConstants.java | 2 +- .../frc/trigon/robot/subsystems/intake/Intake.java | 9 +++++++++ .../robot/subsystems/intake/IntakeConstants.java | 11 ++++++----- .../subsystems/transporter/TransporterConstants.java | 2 +- 8 files changed, 23 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 3e150831..ffde439d 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -11,6 +11,7 @@ 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.GeneralCommands; import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.LEDConstants; @@ -101,6 +102,7 @@ private void bindControllerCommands() { OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); OperatorConstants.CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getCoralCollectionCommand()); + OperatorConstants.DRIVER_CONTROLLER.rightTrigger().whileTrue(CoralEjectionCommands.getCoralEjectionCommand()); } private void configureSysIDBindings(MotorSubsystem subsystem) { 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 e3786616..463ce48d 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -17,8 +17,8 @@ public class CoralCollectionCommands { public static Command getCoralCollectionCommand() { return new ParallelCommandGroup( - getIntakeSequenceCommand(), - new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE) + getIntakeSequenceCommand() + // new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE) ); } 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 cb52f00d..a2cdc9ed 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java @@ -16,7 +16,7 @@ public static Command getCoralEjectionCommand() { return GeneralCommands.getContinuousConditionalCommand( getEjectCoralFromIntakeCommand(), getEjectCoralFromArmCommand(), - () -> RobotContainer.TRANSPORTER.hasCoral() || RobotContainer.INTAKE.hasCoral() + () -> RobotContainer.TRANSPORTER.hasCoral() || RobotContainer.INTAKE.hasCoral() || !RobotContainer.ARM.hasGamePiece() ); } 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 d6b4bd61..77133698 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -117,7 +117,7 @@ private static void updateCoralEjection() { final SimulatedGamePiece heldCoral = CORAL_ON_FIELD.get(HELD_CORAL_INDEX); if (isEjectingCoral()) { - heldCoral.release(RobotContainer.ARM.calculateLinearArmAndEndEffectorVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d()); + heldCoral.release(RobotContainer.INTAKE.calculateLinearIntakeVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d(), IntakeConstants.CORAL_COLLECTION_POSE.getTranslation()); HELD_CORAL_INDEX = null; } } 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 8cee9b02..20d77a67 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -88,7 +88,7 @@ public class ArmConstants { END_EFFECTOR_GEAR_RATIO, END_EFFECTOR_MOMENT_OF_INERTIA ); - private static final DoubleSupplier DISTANCE_SENSOR_SIMULATION_SUPPLIER = () -> SimulationFieldHandler.isHoldingCoral() || SimulationFieldHandler.isHoldingAlgae() ? 0 : 1; + private static final DoubleSupplier DISTANCE_SENSOR_SIMULATION_SUPPLIER = () -> SimulationFieldHandler.isHoldingCoral() || SimulationFieldHandler.isHoldingAlgae() ? 1 : 0; static final SysIdRoutine.Config ARM_SYSID_CONFIG = new SysIdRoutine.Config( Units.Volts.of(1.5).per(Units.Seconds), 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 5827d69d..95785251 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -84,7 +84,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.isHoldingCoral() ? 0 : 1; + DISTANCE_SENSOR_SIMULATION_SUPPLIER = () -> SimulationFieldHandler.isHoldingCoral() ? 1 : 0; static final SysIdRoutine.Config ANGLE_SYSID_CONFIG = new SysIdRoutine.Config( Units.Volts.of(0.2).per(Units.Second), @@ -127,9 +127,10 @@ public class IntakeConstants { FORWARD_LIMIT_SENSOR::getBinaryValue ).debounce(FORWARD_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS); public static Pose3d CORAL_COLLECTION_POSE = new Pose3d( - new Translation3d(-23.58, 23.73, -20.22), + 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(); @@ -216,9 +217,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/transporter/TransporterConstants.java b/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java index 04c42627..3b2c2e57 100644 --- a/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java @@ -66,7 +66,7 @@ public class TransporterConstants { BEAM_BREAK::getBinaryValue ).debounce(HAS_CORAL_DEBOUNCE_TIME_SECONDS); public static final Pose3d COLLECTED_CORAL_POSE = new Pose3d( - new Translation3d(-24.11, 8.98, -20.68), + new Translation3d(0, 0, 0.2), new Rotation3d() ); From 31ac0372494cd63210646b287d62c548179201fe Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 12 Sep 2025 01:53:59 +0300 Subject: [PATCH 26/39] added transporting command --- .../commandfactories/CoralCollectionCommands.java | 14 ++++++++++++-- .../trigon/robot/subsystems/arm/ArmConstants.java | 1 + .../subsystems/elevator/ElevatorConstants.java | 1 + 3 files changed, 14 insertions(+), 2 deletions(-) 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 463ce48d..0266e409 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -5,9 +5,12 @@ 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; +import frc.trigon.robot.subsystems.arm.ArmConstants; +import frc.trigon.robot.subsystems.elevator.ElevatorCommands; +import frc.trigon.robot.subsystems.elevator.ElevatorConstants; import frc.trigon.robot.subsystems.intake.IntakeCommands; import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.transporter.TransporterCommands; @@ -15,10 +18,17 @@ import lib.hardware.misc.leds.LEDCommands; public class CoralCollectionCommands { + public static Command getCoralTransportCommand() { + return new ParallelCommandGroup( + ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.COLLECT_CORAL), + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.COLLECT_CORAL) + ).until(RobotContainer.ARM::hasGamePiece); + } + public static Command getCoralCollectionCommand() { return new ParallelCommandGroup( getIntakeSequenceCommand() - // new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE) + // new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE) ); } 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 20d77a67..f45a3b43 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -234,6 +234,7 @@ public enum ArmState { REST_FOR_CLIMB(Rotation2d.fromDegrees(0), 0), HOLD_ALGAE(Rotation2d.fromDegrees(90), -4), EJECT(Rotation2d.fromDegrees(60), 4), + COLLECT_CORAL(Rotation2d.fromDegrees(0), -4), PREPARE_SCORE_L1(Rotation2d.fromDegrees(80), 0), SCORE_L1(Rotation2d.fromDegrees(75), 4), PREPARE_SCORE_L2(Rotation2d.fromDegrees(95), 0), 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 b56f6d03..0d1df701 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -159,6 +159,7 @@ private static void configureReverseLimitSensor() { public enum ElevatorState { REST(0.603, 0.7), + COLLECT_CORAL(0.5519, 0.7), SCORE_L1(0.603, 1), SCORE_L2(0.603, 1), SCORE_L3(1.003, 1), From cbdada2f0008d5e63cd38c8f16feb52e4bc767a6 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 12 Sep 2025 02:11:48 +0300 Subject: [PATCH 27/39] added simulation logic for transporting coral to arm --- .../CoralCollectionCommands.java | 2 +- .../simulatedfield/SimulationFieldHandler.java | 17 +++++++++++++++-- .../frc/trigon/robot/subsystems/arm/Arm.java | 4 ++-- .../robot/subsystems/arm/ArmConstants.java | 4 ++-- 4 files changed, 20 insertions(+), 7 deletions(-) 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 0266e409..2c2761d9 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -20,7 +20,7 @@ public class CoralCollectionCommands { public static Command getCoralTransportCommand() { return new ParallelCommandGroup( - ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.COLLECT_CORAL), + ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.TRANSPORT_CORAL), ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.COLLECT_CORAL) ).until(RobotContainer.ARM::hasGamePiece); } 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 77133698..359ea317 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -50,6 +50,7 @@ private static void updateGamePieces() { updateCollection(); updateEjection(); updateHeldGamePiecePoses(); + updateTransport(); } /** @@ -71,7 +72,7 @@ private static void updateCollection() { final Pose3d robotPose = new Pose3d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose()); final Pose3d coralCollectionPose = robotPose.plus(toTransform(IntakeConstants.CORAL_COLLECTION_POSE)), - algaeCollectionPose = robotPose.plus(toTransform(RobotContainer.ARM.calculateAlgaeCollectionPose())); + algaeCollectionPose = robotPose.plus(toTransform(RobotContainer.ARM.calculateGamePieceCollectionPose())); if (isCollectingCoral() && HELD_CORAL_INDEX == null) HELD_CORAL_INDEX = getIndexOfCollectedGamePiece(coralCollectionPose, CORAL_ON_FIELD, SimulatedGamePieceConstants.CORAL_INTAKE_TOLERANCE_METERS); @@ -80,6 +81,14 @@ private static void updateCollection() { HELD_ALGAE_INDEX = getIndexOfCollectedGamePiece(algaeCollectionPose, ALGAE_ON_FIELD, SimulatedGamePieceConstants.ALGAE_INTAKE_TOLERANCE_METERS); } + private static void updateTransport() { + final Pose3d robotPose = new Pose3d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose()); + final Pose3d coralTransportPose = robotPose.plus(toTransform(RobotContainer.ARM.calculateGamePieceCollectionPose())); + + if (isTransportingCoral() && HELD_CORAL_INDEX != null) + HELD_CORAL_INDEX = getIndexOfCollectedGamePiece(coralTransportPose, CORAL_ON_FIELD, SimulatedGamePieceConstants.CORAL_INTAKE_TOLERANCE_METERS); + } + /** * Gets the index of the game piece that is being collected. * @@ -102,6 +111,10 @@ private static boolean isCollectingAlgae() { return RobotContainer.ARM.atState(ArmConstants.ArmState.COLLECT_ALGAE_L2) || RobotContainer.ARM.atState(ArmConstants.ArmState.COLLECT_ALGAE_L3); } + private static boolean isTransportingCoral() { + return RobotContainer.ARM.atState(ArmConstants.ArmState.TRANSPORT_CORAL); + } + private static void updateEjection() { if (HELD_CORAL_INDEX != null) updateCoralEjection(); @@ -136,7 +149,7 @@ private static boolean isEjectingAlgae() { private static void updateHeldGamePiecePoses() { final Pose3d robotRelativeHeldCoralPosition = TransporterConstants.COLLECTED_CORAL_POSE, - robotRelativeHeldAlgaePosition = RobotContainer.ARM.calculateAlgaeCollectionPose(); + robotRelativeHeldAlgaePosition = RobotContainer.ARM.calculateGamePieceCollectionPose(); updateHeldGamePiecePose(robotRelativeHeldCoralPosition, CORAL_ON_FIELD, HELD_CORAL_INDEX); updateHeldGamePiecePose(robotRelativeHeldAlgaePosition, ALGAE_ON_FIELD, HELD_ALGAE_INDEX); } 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 52da094c..0fa9c21a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -82,9 +82,9 @@ public void sysIDDrive(double targetVoltage) { armMasterMotor.setControl(voltageRequest.withOutput(targetVoltage)); } - public Pose3d calculateAlgaeCollectionPose() { + public Pose3d calculateGamePieceCollectionPose() { return calculateVisualizationPose() - .transformBy(ArmConstants.ARM_TO_HELD_ALGAE); + .transformBy(ArmConstants.ARM_TO_HELD_GAME_PIECE); } public boolean atState(ArmConstants.ArmState targetState, boolean isStateReversed) { 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 f45a3b43..709d8d6e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -111,7 +111,7 @@ public class ArmConstants { new Rotation3d(0, 0, 0) ); - static final Transform3d ARM_TO_HELD_ALGAE = new Transform3d( + static final Transform3d ARM_TO_HELD_GAME_PIECE = new Transform3d( new Translation3d(0, 0, -0.5855), new Rotation3d(0, edu.wpi.first.math.util.Units.degreesToRadians(0), 0) ); @@ -234,7 +234,7 @@ public enum ArmState { REST_FOR_CLIMB(Rotation2d.fromDegrees(0), 0), HOLD_ALGAE(Rotation2d.fromDegrees(90), -4), EJECT(Rotation2d.fromDegrees(60), 4), - COLLECT_CORAL(Rotation2d.fromDegrees(0), -4), + TRANSPORT_CORAL(Rotation2d.fromDegrees(0), -4), PREPARE_SCORE_L1(Rotation2d.fromDegrees(80), 0), SCORE_L1(Rotation2d.fromDegrees(75), 4), PREPARE_SCORE_L2(Rotation2d.fromDegrees(95), 0), From b795297655d01588b1dd772963ac2ac0637d9904 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 12 Sep 2025 02:29:09 +0300 Subject: [PATCH 28/39] fixed logic with distance sensors --- .../simulatedfield/SimulationFieldHandler.java | 14 +++++++++++--- .../java/frc/trigon/robot/subsystems/arm/Arm.java | 2 ++ .../trigon/robot/subsystems/arm/ArmConstants.java | 2 +- .../robot/subsystems/intake/IntakeConstants.java | 2 +- 4 files changed, 15 insertions(+), 5 deletions(-) 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 359ea317..91a87f44 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -20,6 +20,7 @@ public class SimulationFieldHandler { private static Integer HELD_CORAL_INDEX = null, HELD_ALGAE_INDEX = null; + private static boolean IS_CORAL_IN_END_EFFECTOR = true; public static ArrayList getSimulatedCoral() { return CORAL_ON_FIELD; @@ -33,6 +34,10 @@ 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; } @@ -74,9 +79,10 @@ private static void updateCollection() { coralCollectionPose = robotPose.plus(toTransform(IntakeConstants.CORAL_COLLECTION_POSE)), algaeCollectionPose = robotPose.plus(toTransform(RobotContainer.ARM.calculateGamePieceCollectionPose())); - if (isCollectingCoral() && HELD_CORAL_INDEX == null) + if (isCollectingCoral() && HELD_CORAL_INDEX == null) { HELD_CORAL_INDEX = getIndexOfCollectedGamePiece(coralCollectionPose, CORAL_ON_FIELD, SimulatedGamePieceConstants.CORAL_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); } @@ -85,8 +91,10 @@ private static void updateTransport() { final Pose3d robotPose = new Pose3d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose()); final Pose3d coralTransportPose = robotPose.plus(toTransform(RobotContainer.ARM.calculateGamePieceCollectionPose())); - if (isTransportingCoral() && HELD_CORAL_INDEX != null) + if (isTransportingCoral() && HELD_CORAL_INDEX != null) { HELD_CORAL_INDEX = getIndexOfCollectedGamePiece(coralTransportPose, CORAL_ON_FIELD, SimulatedGamePieceConstants.CORAL_INTAKE_TOLERANCE_METERS); + IS_CORAL_IN_END_EFFECTOR = true; + } } /** 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 0fa9c21a..3f87ecb1 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -12,6 +12,7 @@ import lib.hardware.phoenix6.cancoder.CANcoderSignal; import lib.hardware.phoenix6.talonfx.TalonFXMotor; import lib.hardware.phoenix6.talonfx.TalonFXSignal; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Arm extends MotorSubsystem { @@ -110,6 +111,7 @@ public boolean atTargetAngle() { return currentToTargetStateDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); } + @AutoLogOutput(key = "Arm/HasCoral") public boolean hasGamePiece() { return ArmConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); } 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 709d8d6e..552d0ebf 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -88,7 +88,7 @@ public class ArmConstants { END_EFFECTOR_GEAR_RATIO, END_EFFECTOR_MOMENT_OF_INERTIA ); - private static final DoubleSupplier DISTANCE_SENSOR_SIMULATION_SUPPLIER = () -> SimulationFieldHandler.isHoldingCoral() || SimulationFieldHandler.isHoldingAlgae() ? 1 : 0; + 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), 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 95785251..8a0907f2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -84,7 +84,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.isHoldingCoral() ? 1 : 0; + 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), From 0886c0057115d25361002cce6696eb371fb1fad2 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 12 Sep 2025 02:53:20 +0300 Subject: [PATCH 29/39] Update SimulationFieldHandler.java --- .../robot/misc/simulatedfield/SimulationFieldHandler.java | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) 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 91a87f44..0b3f98f0 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -88,11 +88,7 @@ private static void updateCollection() { } private static void updateTransport() { - final Pose3d robotPose = new Pose3d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose()); - final Pose3d coralTransportPose = robotPose.plus(toTransform(RobotContainer.ARM.calculateGamePieceCollectionPose())); - if (isTransportingCoral() && HELD_CORAL_INDEX != null) { - HELD_CORAL_INDEX = getIndexOfCollectedGamePiece(coralTransportPose, CORAL_ON_FIELD, SimulatedGamePieceConstants.CORAL_INTAKE_TOLERANCE_METERS); IS_CORAL_IN_END_EFFECTOR = true; } } @@ -156,7 +152,7 @@ private static boolean isEjectingAlgae() { */ private static void updateHeldGamePiecePoses() { final Pose3d - robotRelativeHeldCoralPosition = TransporterConstants.COLLECTED_CORAL_POSE, + 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); From 93cc32df9d3e1c9c5a638517bc37ecd82d6294e4 Mon Sep 17 00:00:00 2001 From: shmayaR <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 11 Sep 2025 20:31:49 -0400 Subject: [PATCH 30/39] made it work --- src/main/java/frc/trigon/robot/RobotContainer.java | 2 ++ .../robot/misc/simulatedfield/SimulationFieldHandler.java | 4 ++-- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 6 ++++++ .../java/frc/trigon/robot/subsystems/arm/ArmConstants.java | 4 ++-- 4 files changed, 12 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index ffde439d..7aab0280 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -103,6 +103,8 @@ private void bindControllerCommands() { OperatorConstants.CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getCoralCollectionCommand()); OperatorConstants.DRIVER_CONTROLLER.rightTrigger().whileTrue(CoralEjectionCommands.getCoralEjectionCommand()); + OperatorConstants.DRIVER_CONTROLLER.a().whileTrue(CoralCollectionCommands.getCoralTransportCommand()); + OperatorConstants.DRIVER_CONTROLLER.x().whileTrue(ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.SCORE_L4)); } private void configureSysIDBindings(MotorSubsystem subsystem) { 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 0b3f98f0..07a1420e 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -88,7 +88,7 @@ private static void updateCollection() { } private static void updateTransport() { - if (isTransportingCoral() && HELD_CORAL_INDEX != null) { + if (isTransportingCoral()) { IS_CORAL_IN_END_EFFECTOR = true; } } @@ -140,7 +140,7 @@ private static void updateCoralEjection() { } private static boolean isEjectingCoral() { - return RobotContainer.INTAKE.atState(IntakeConstants.IntakeState.EJECT); + return RobotContainer.INTAKE.atState(IntakeConstants.IntakeState.EJECT) || RobotContainer.ARM.atState(ArmConstants.ArmState.EJECT); } private static boolean isEjectingAlgae() { 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 3f87ecb1..7f20624b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -7,6 +7,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; @@ -116,6 +117,11 @@ public boolean hasGamePiece() { return ArmConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); } + @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( 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 552d0ebf..c445263e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -112,11 +112,11 @@ public class ArmConstants { ); static final Transform3d ARM_TO_HELD_GAME_PIECE = new Transform3d( - new Translation3d(0, 0, -0.5855), + 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(0); + 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(), From 3fb9f2345f322919e5da4b5df023baf26336c67c Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 12 Sep 2025 04:03:51 +0300 Subject: [PATCH 31/39] fixed logic issue with ejectung commands --- .../commands/commandfactories/CoralEjectionCommands.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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 a2cdc9ed..f6b6827d 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; @@ -16,8 +17,8 @@ public static Command getCoralEjectionCommand() { return GeneralCommands.getContinuousConditionalCommand( getEjectCoralFromIntakeCommand(), getEjectCoralFromArmCommand(), - () -> RobotContainer.TRANSPORTER.hasCoral() || RobotContainer.INTAKE.hasCoral() || !RobotContainer.ARM.hasGamePiece() - ); + () -> RobotContainer.TRANSPORTER.hasCoral() || RobotContainer.INTAKE.hasCoral() + ).onlyIf(SimulationFieldHandler::isHoldingCoral); } private static Command getEjectCoralFromIntakeCommand() { From 6834ec6a8c679a27b974062bca0f7bfb7075a1a2 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Fri, 12 Sep 2025 04:06:26 +0300 Subject: [PATCH 32/39] renamed coral loading for consistansy --- src/main/java/frc/trigon/robot/RobotContainer.java | 2 +- .../commandfactories/CoralCollectionCommands.java | 6 +++--- .../misc/simulatedfield/SimulationFieldHandler.java | 10 +++++----- .../frc/trigon/robot/subsystems/arm/ArmConstants.java | 2 +- .../robot/subsystems/elevator/ElevatorConstants.java | 2 +- 5 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 7aab0280..b35c61db 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -103,7 +103,7 @@ private void bindControllerCommands() { OperatorConstants.CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getCoralCollectionCommand()); OperatorConstants.DRIVER_CONTROLLER.rightTrigger().whileTrue(CoralEjectionCommands.getCoralEjectionCommand()); - OperatorConstants.DRIVER_CONTROLLER.a().whileTrue(CoralCollectionCommands.getCoralTransportCommand()); + OperatorConstants.DRIVER_CONTROLLER.a().whileTrue(CoralCollectionCommands.getLoadCoralCommand()); OperatorConstants.DRIVER_CONTROLLER.x().whileTrue(ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.SCORE_L4)); } 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 2c2761d9..efd29ce0 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -18,10 +18,10 @@ import lib.hardware.misc.leds.LEDCommands; public class CoralCollectionCommands { - public static Command getCoralTransportCommand() { + public static Command getLoadCoralCommand() { return new ParallelCommandGroup( - ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.TRANSPORT_CORAL), - ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.COLLECT_CORAL) + ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.LOAD_CORAL), + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.LOAD_CORAL) ).until(RobotContainer.ARM::hasGamePiece); } 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 07a1420e..fbea4a12 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -55,7 +55,7 @@ private static void updateGamePieces() { updateCollection(); updateEjection(); updateHeldGamePiecePoses(); - updateTransport(); + updateLoad(); } /** @@ -87,8 +87,8 @@ private static void updateCollection() { HELD_ALGAE_INDEX = getIndexOfCollectedGamePiece(algaeCollectionPose, ALGAE_ON_FIELD, SimulatedGamePieceConstants.ALGAE_INTAKE_TOLERANCE_METERS); } - private static void updateTransport() { - if (isTransportingCoral()) { + private static void updateLoad() { + if (isCoralLoading()) { IS_CORAL_IN_END_EFFECTOR = true; } } @@ -115,8 +115,8 @@ private static boolean isCollectingAlgae() { return RobotContainer.ARM.atState(ArmConstants.ArmState.COLLECT_ALGAE_L2) || RobotContainer.ARM.atState(ArmConstants.ArmState.COLLECT_ALGAE_L3); } - private static boolean isTransportingCoral() { - return RobotContainer.ARM.atState(ArmConstants.ArmState.TRANSPORT_CORAL); + private static boolean isCoralLoading() { + return RobotContainer.ARM.atState(ArmConstants.ArmState.LOAD_CORAL); } private static void updateEjection() { 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 c445263e..fb8ae813 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -234,7 +234,7 @@ public enum ArmState { REST_FOR_CLIMB(Rotation2d.fromDegrees(0), 0), HOLD_ALGAE(Rotation2d.fromDegrees(90), -4), EJECT(Rotation2d.fromDegrees(60), 4), - TRANSPORT_CORAL(Rotation2d.fromDegrees(0), -4), + LOAD_CORAL(Rotation2d.fromDegrees(0), -4), PREPARE_SCORE_L1(Rotation2d.fromDegrees(80), 0), SCORE_L1(Rotation2d.fromDegrees(75), 4), PREPARE_SCORE_L2(Rotation2d.fromDegrees(95), 0), 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 0d1df701..83c32002 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -159,7 +159,7 @@ private static void configureReverseLimitSensor() { public enum ElevatorState { REST(0.603, 0.7), - COLLECT_CORAL(0.5519, 0.7), + LOAD_CORAL(0.5519, 0.7), SCORE_L1(0.603, 1), SCORE_L2(0.603, 1), SCORE_L3(1.003, 1), From ff60b187075966bb91badf0f2a3fa1fea2e7b533 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Tue, 16 Sep 2025 12:37:04 +0300 Subject: [PATCH 33/39] added sim for ejecting from arm --- .../robot/misc/simulatedfield/SimulationFieldHandler.java | 6 +++++- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 2 +- 2 files changed, 6 insertions(+), 2 deletions(-) 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 fbea4a12..a9185c46 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -133,10 +133,14 @@ private static void updateEjection() { private static void updateCoralEjection() { final SimulatedGamePiece heldCoral = CORAL_ON_FIELD.get(HELD_CORAL_INDEX); - if (isEjectingCoral()) { + if (isEjectingCoral() && !isCoralInEndEffector()) { heldCoral.release(RobotContainer.INTAKE.calculateLinearIntakeVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d(), IntakeConstants.CORAL_COLLECTION_POSE.getTranslation()); HELD_CORAL_INDEX = null; } + if (isEjectingCoral() && isCoralInEndEffector()) { + heldCoral.release(RobotContainer.ARM.calculateLinearArmAndEndEffectorVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d(), RobotContainer.ARM.calculateGamePieceCollectionPose().getTranslation()); + HELD_CORAL_INDEX = null; + } } private static boolean isEjectingCoral() { 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 7f20624b..7e9bd882 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -118,7 +118,7 @@ public boolean hasGamePiece() { } @AutoLogOutput(key = "Arm/InEndeffector") - public boolean inEndeffector() { + public boolean inEndEffector() { return SimulationFieldHandler.isCoralInEndEffector(); } From 1f364f1fabb46369578825a5f4495ce9e243b424 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Fri, 19 Sep 2025 05:26:47 +0300 Subject: [PATCH 34/39] added coral spawning --- src/main/java/frc/trigon/robot/RobotContainer.java | 11 +++++++---- .../trigon/robot/constants/OperatorConstants.java | 4 +++- .../simulatedfield/SimulatedGamePieceConstants.java | 3 +++ .../misc/simulatedfield/SimulationFieldHandler.java | 12 ++++++++++++ 4 files changed, 25 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index b35c61db..a743642b 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -6,8 +6,10 @@ 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; @@ -19,18 +21,18 @@ 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.elevator.Elevator; -import frc.trigon.robot.subsystems.elevator.ElevatorCommands; -import frc.trigon.robot.subsystems.elevator.ElevatorConstants; import frc.trigon.robot.subsystems.arm.Arm; import frc.trigon.robot.subsystems.arm.ArmCommands; import frc.trigon.robot.subsystems.arm.ArmConstants; import frc.trigon.robot.subsystems.climber.Climber; import frc.trigon.robot.subsystems.climber.ClimberCommands; import frc.trigon.robot.subsystems.climber.ClimberConstants; +import frc.trigon.robot.subsystems.elevator.Elevator; +import frc.trigon.robot.subsystems.elevator.ElevatorCommands; +import frc.trigon.robot.subsystems.elevator.ElevatorConstants; import frc.trigon.robot.subsystems.intake.Intake; import frc.trigon.robot.subsystems.intake.IntakeCommands; import frc.trigon.robot.subsystems.intake.IntakeConstants; @@ -101,6 +103,7 @@ 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.DRIVER_CONTROLLER.rightTrigger().whileTrue(CoralEjectionCommands.getCoralEjectionCommand()); OperatorConstants.DRIVER_CONTROLLER.a().whileTrue(CoralCollectionCommands.getLoadCoralCommand()); diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index f7506110..6114a543 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -38,5 +38,7 @@ public class OperatorConstants { 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 + CORAL_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftTrigger().or(OPERATOR_CONTROLLER.c()), + SPAWN_CORAL_TRIGGER = OPERATOR_CONTROLLER.equals(); } \ No newline at end of file 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 e191e506..214357de 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java @@ -67,6 +67,9 @@ public class SimulatedGamePieceConstants { 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); 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 a9185c46..83da3110 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -6,6 +6,7 @@ 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; @@ -81,12 +82,23 @@ private static void updateCollection() { if (isCollectingCoral() && HELD_CORAL_INDEX == null) { HELD_CORAL_INDEX = getIndexOfCollectedGamePiece(coralCollectionPose, CORAL_ON_FIELD, SimulatedGamePieceConstants.CORAL_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); } + 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)); + } + private static void updateLoad() { if (isCoralLoading()) { IS_CORAL_IN_END_EFFECTOR = true; From ee3c7cba12c3430980942f5e57da93ffe327c4fa Mon Sep 17 00:00:00 2001 From: shmayaR <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 21 Sep 2025 00:17:27 -0400 Subject: [PATCH 35/39] important changes --- .../java/frc/trigon/robot/RobotContainer.java | 7 ++- .../robot/constants/FieldConstants.java | 55 ------------------- .../robot/constants/OperatorConstants.java | 12 ++-- .../SimulationFieldHandler.java | 14 +---- .../frc/trigon/robot/subsystems/arm/Arm.java | 4 ++ .../robot/subsystems/arm/ArmConstants.java | 4 +- .../subsystems/intake/IntakeConstants.java | 7 ++- 7 files changed, 23 insertions(+), 80 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 5594b1c8..6a40531a 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -14,6 +14,7 @@ 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; @@ -105,8 +106,10 @@ private void bindControllerCommands() { OperatorConstants.SPAWN_CORAL_TRIGGER.onTrue(new InstantCommand(() -> SimulationFieldHandler.updateCoralSpawning(new Pose3d(ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose())))); OperatorConstants.CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getCoralCollectionCommand()); OperatorConstants.DRIVER_CONTROLLER.rightTrigger().whileTrue(CoralEjectionCommands.getCoralEjectionCommand()); - OperatorConstants.DRIVER_CONTROLLER.a().whileTrue(CoralCollectionCommands.getLoadCoralCommand()); - OperatorConstants.DRIVER_CONTROLLER.x().whileTrue(ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.SCORE_L4)); + OperatorConstants.DRIVER_CONTROLLER.rightStick().whileTrue(CoralCollectionCommands.getLoadCoralCommand()); + + OperatorConstants.DRIVER_CONTROLLER.leftBumper().whileTrue(CoralPlacingCommands.getScoreInReefCommand(false)); + OperatorConstants.DRIVER_CONTROLLER.rightBumper().whileTrue(CoralPlacingCommands.getScoreInReefCommand(true)); } private void configureSysIDBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index c3b6954a..9f7b8daa 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -47,15 +47,6 @@ public class FieldConstants { REEF_CENTER_TO_TARGET_SCORING_POSITION_Y_TRANSFORM_METERS = 0.17, REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS = 1.6; - public static final int REEF_CLOCK_POSITIONS = 6; - public static final Rotation2d REEF_CLOCK_POSITION_DIFFERENCE = Rotation2d.fromDegrees(Conversions.DEGREES_PER_ROTATIONS / REEF_CLOCK_POSITIONS); - public static final Rotation2d[] REEF_CLOCK_ANGLES = ReefClockPosition.getClockAngles(); - public static final Translation2d BLUE_REEF_CENTER_TRANSLATION = new Translation2d(4.48945, FIELD_WIDTH_METERS / 2); - public static final double - REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS = 1.3, - REEF_CENTER_TO_TARGET_SCORING_POSITION_Y_TRANSFORM_METERS = 0.17, - REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS = 1.6; - private static AprilTagFieldLayout createAprilTagFieldLayout() { try { return SHOULD_USE_HOME_TAG_LAYOUT ? @@ -121,50 +112,4 @@ private Rotation2d calculateClockAngle() { return REEF_CLOCK_POSITION_DIFFERENCE.times(-ordinal()); } } - - public enum ReefSide { - RIGHT(true), - LEFT(false); - - public final boolean doesFlipYTransformWhenFacingDriverStation; - - ReefSide(boolean doesFlipYTransformWhenFacingDriverStation) { - this.doesFlipYTransformWhenFacingDriverStation = doesFlipYTransformWhenFacingDriverStation; - } - - public boolean shouldFlipYTransform(ReefClockPosition reefClockPosition) { - return doesFlipYTransformWhenFacingDriverStation ^ reefClockPosition.isFacingDriverStation; // In Java, ^ acts as an XOR (exclusive OR) operator, which fits in this case - } - } - - public enum ReefClockPosition { - REEF_12_OCLOCK(true), - REEF_2_OCLOCK(true), - REEF_4_OCLOCK(true), - REEF_6_OCLOCK(true), - REEF_8_OCLOCK(true), - REEF_10_OCLOCK(true); - - public final Rotation2d clockAngle; - public final boolean isFacingDriverStation; - public final int clockPosition; - - ReefClockPosition(boolean isFacingDriverStation) { - this.clockAngle = calculateClockAngle(); - this.isFacingDriverStation = isFacingDriverStation; - this.clockPosition = ordinal() == 0 ? 12 : ordinal() * 2; - } - - public static Rotation2d[] getClockAngles() { - final Rotation2d[] clockAngles = new Rotation2d[ReefClockPosition.values().length]; - for (int i = 0; i < clockAngles.length; i++) - clockAngles[i] = ReefClockPosition.values()[i].clockAngle; - - return clockAngles; - } - - private Rotation2d calculateClockAngle() { - return REEF_CLOCK_POSITION_DIFFERENCE.times(-ordinal()); - } - } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 98f7ea45..eb7526ad 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -33,11 +33,11 @@ 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 = OPERATOR_CONTROLLER.y(), 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(), @@ -48,10 +48,10 @@ public class OperatorConstants { SPAWN_CORAL_TRIGGER = OPERATOR_CONTROLLER.equals(); 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()), + 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/simulatedfield/SimulationFieldHandler.java b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java index 83da3110..355fca6f 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -135,7 +135,7 @@ private static void updateEjection() { if (HELD_CORAL_INDEX != null) updateCoralEjection(); - if (HELD_ALGAE_INDEX != null && isEjectingAlgae()) { + 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; @@ -145,24 +145,16 @@ private static void updateEjection() { private static void updateCoralEjection() { final SimulatedGamePiece heldCoral = CORAL_ON_FIELD.get(HELD_CORAL_INDEX); - if (isEjectingCoral() && !isCoralInEndEffector()) { + 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 (isEjectingCoral() && isCoralInEndEffector()) { + if (isCoralInEndEffector() && RobotContainer.ARM.isEjecting()) { heldCoral.release(RobotContainer.ARM.calculateLinearArmAndEndEffectorVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d(), RobotContainer.ARM.calculateGamePieceCollectionPose().getTranslation()); HELD_CORAL_INDEX = null; } } - private static boolean isEjectingCoral() { - return RobotContainer.INTAKE.atState(IntakeConstants.IntakeState.EJECT) || RobotContainer.ARM.atState(ArmConstants.ArmState.EJECT); - } - - private static boolean isEjectingAlgae() { - return RobotContainer.ARM.atState(ArmConstants.ArmState.EJECT); - } - /** * Updates the position of the held game pieces so that they stay inside the robot. */ 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 a664fbe2..616a7d2b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -143,6 +143,10 @@ public Translation3d calculateLinearArmAndEndEffectorVelocity() { ); } + 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; 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 9278c6a1..53396fd4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -119,8 +119,6 @@ public class ArmConstants { * 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( @@ -246,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/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index d44995f0..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, @@ -94,7 +95,7 @@ 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; From 6f953977359b1abd0b3d580273cff63392f9f9ae Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 21 Sep 2025 19:25:27 +0300 Subject: [PATCH 36/39] Update RobotContainer.java --- src/main/java/frc/trigon/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 6a40531a..d294a325 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -105,7 +105,7 @@ private void bindControllerCommands() { OperatorConstants.SPAWN_CORAL_TRIGGER.onTrue(new InstantCommand(() -> SimulationFieldHandler.updateCoralSpawning(new Pose3d(ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose())))); OperatorConstants.CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getCoralCollectionCommand()); - OperatorConstants.DRIVER_CONTROLLER.rightTrigger().whileTrue(CoralEjectionCommands.getCoralEjectionCommand()); + OperatorConstants.DRIVER_CONTROLLER.back().whileTrue(CoralEjectionCommands.getCoralEjectionCommand()); OperatorConstants.DRIVER_CONTROLLER.rightStick().whileTrue(CoralCollectionCommands.getLoadCoralCommand()); OperatorConstants.DRIVER_CONTROLLER.leftBumper().whileTrue(CoralPlacingCommands.getScoreInReefCommand(false)); From d909d7fe48aae358e4b76f9afd24169410a34848 Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Sun, 21 Sep 2025 19:26:41 +0300 Subject: [PATCH 37/39] Update Arm.java --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 616a7d2b..eb7a6b60 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -127,7 +127,7 @@ public Rotation2d getAngle() { return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION)); } - @AutoLogOutput(key = "Arm/InEndeffector") + @AutoLogOutput(key = "Arm/InEndEffector") public boolean inEndEffector() { return SimulationFieldHandler.isCoralInEndEffector(); } From f654394adeeaf13a45701d51adb40bec3ebaf7de Mon Sep 17 00:00:00 2001 From: shmayaR <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 21 Sep 2025 13:24:49 -0400 Subject: [PATCH 38/39] coral now loads autonomously --- .../java/frc/trigon/robot/RobotContainer.java | 1 - .../CoralCollectionCommands.java | 24 +++++++++++-------- .../robot/constants/OperatorConstants.java | 2 +- 3 files changed, 15 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 6a40531a..04b10baf 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -106,7 +106,6 @@ private void bindControllerCommands() { OperatorConstants.SPAWN_CORAL_TRIGGER.onTrue(new InstantCommand(() -> SimulationFieldHandler.updateCoralSpawning(new Pose3d(ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose())))); OperatorConstants.CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getCoralCollectionCommand()); OperatorConstants.DRIVER_CONTROLLER.rightTrigger().whileTrue(CoralEjectionCommands.getCoralEjectionCommand()); - OperatorConstants.DRIVER_CONTROLLER.rightStick().whileTrue(CoralCollectionCommands.getLoadCoralCommand()); OperatorConstants.DRIVER_CONTROLLER.leftBumper().whileTrue(CoralPlacingCommands.getScoreInReefCommand(false)); OperatorConstants.DRIVER_CONTROLLER.rightBumper().whileTrue(CoralPlacingCommands.getScoreInReefCommand(true)); 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 efd29ce0..f4d70ecb 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -18,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( - getIntakeSequenceCommand() - // new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE) + return new SequentialCommandGroup( + getIntakeSequenceCommand(), + 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/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index eb7526ad..5dfe7a37 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -33,7 +33,7 @@ public class OperatorConstants { public static final IntakeAssistCommand.AssistMode DEFAULT_INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.ALTERNATE_ASSIST; public static final Trigger - RESET_HEADING_TRIGGER = OPERATOR_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(), From 52cf4349c53ed463d876590f19f0acbaa9ac0e31 Mon Sep 17 00:00:00 2001 From: shmayaR <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 21 Sep 2025 14:21:15 -0400 Subject: [PATCH 39/39] moved triggers into operator constants --- src/main/java/frc/trigon/robot/RobotContainer.java | 7 +++---- .../frc/trigon/robot/constants/OperatorConstants.java | 9 ++++++--- .../frc/trigon/robot/subsystems/elevator/Elevator.java | 1 - 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index e7bcdbe4..2265ac21 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -106,10 +106,9 @@ private void bindControllerCommands() { OperatorConstants.SPAWN_CORAL_TRIGGER.onTrue(new InstantCommand(() -> SimulationFieldHandler.updateCoralSpawning(new Pose3d(ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose())))); OperatorConstants.CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getCoralCollectionCommand()); - OperatorConstants.DRIVER_CONTROLLER.back().whileTrue(CoralEjectionCommands.getCoralEjectionCommand()); - - OperatorConstants.DRIVER_CONTROLLER.leftBumper().whileTrue(CoralPlacingCommands.getScoreInReefCommand(false)); - OperatorConstants.DRIVER_CONTROLLER.rightBumper().whileTrue(CoralPlacingCommands.getScoreInReefCommand(true)); + 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/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 5dfe7a37..147f64e6 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -42,11 +42,14 @@ public class OperatorConstants { 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()), - SPAWN_CORAL_TRIGGER = OPERATOR_CONTROLLER.equals(); - + 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()), diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index 607201c0..080739d1 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -77,7 +77,6 @@ public void updatePeriodically() { Logger.recordOutput("Elevator/CurrentPositionMeters", getPositionMeters()); } - public boolean atState(ElevatorConstants.ElevatorState targetState) { return targetState == this.targetState && atTargetState(); }