From e887b1185a45b94005a9285b931e0ae2727622ad Mon Sep 17 00:00:00 2001 From: Yishai Levy <96019039+levyishai@users.noreply.github.com> Date: Sun, 30 Mar 2025 16:22:44 +0300 Subject: [PATCH 01/65] Open Elevator only when close to reef --- .../commandfactories/AutonomousCommands.java | 22 ++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index d0796818..bbd7376d 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -11,7 +11,6 @@ import frc.trigon.robot.commands.commandclasses.CoralAutoDriveCommand; import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.FieldConstants; -import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.constants.PathPlannerConstants; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.subsystems.coralintake.CoralIntakeCommands; @@ -134,14 +133,27 @@ private static boolean isFirst() { public static Command getPrepareForScoreCommand() { return new ParallelCommandGroup( - ElevatorCommands.getSetTargetStateCommand(OperatorConstants.REEF_CHOOSER.getElevatorState()), + getOpenElevatorWhenCloseToReefCommand(), GripperCommands.getPrepareForScoringInL4Command(() -> TARGET_SCORING_POSE) ); } + private static Command getOpenElevatorWhenCloseToReefCommand() { + return GeneralCommands.runWhen( + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_L4), + () -> calculateDistanceToTargetScoringPose() < PathPlannerConstants.MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR + ); + } + + private static double calculateDistanceToTargetScoringPose() { + final Translation2d currentTranslation = RobotContainer.POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); + final Translation2d targetTranslation = TARGET_SCORING_POSE.get().getTranslation(); + return currentTranslation.getDistance(targetTranslation); + } + public static Command getFeedCoralCommand() { return new ParallelCommandGroup( - ElevatorCommands.getSetTargetStateCommand(OperatorConstants.REEF_CHOOSER.getElevatorState()), + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_L4), GripperCommands.getScoreInL4Command(() -> TARGET_SCORING_POSE), getAddCurrentScoringBranchToScoredBranchesCommand() ).withTimeout(0.25); @@ -194,8 +206,8 @@ public static FlippablePose2d calculateClosestScoringPose(boolean shouldOnlyChec @AutoLogOutput private static boolean canFeed() { - return RobotContainer.ELEVATOR.atState(OperatorConstants.REEF_CHOOSER.getScoringLevel().elevatorState) && - RobotContainer.GRIPPER.atState(OperatorConstants.REEF_CHOOSER.getScoringLevel().gripperState) && + return RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.SCORE_L4) && + RobotContainer.GRIPPER.atState(GripperConstants.GripperState.SCORE_L4_CLOSE) && TARGET_SCORING_POSE != null && Math.abs(RobotContainer.POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getX()) < 0.085 && Math.abs(RobotContainer.POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getY()) < 0.03; From 14d00352744cd4b9e0dce55bb31ad65b0b5d7012 Mon Sep 17 00:00:00 2001 From: Yishai Levy <96019039+levyishai@users.noreply.github.com> Date: Sun, 30 Mar 2025 16:29:13 +0300 Subject: [PATCH 02/65] added cool logic --- .../commandclasses/CoralAutoDriveCommand.java | 4 ++-- .../commandfactories/AutonomousCommands.java | 16 ++++++++-------- .../commandfactories/CoralPlacingCommands.java | 4 ++-- .../robot/subsystems/elevator/Elevator.java | 11 +++++++++-- .../robot/subsystems/swerve/SwerveCommands.java | 11 +++++++++++ 5 files changed, 32 insertions(+), 14 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 93b8e80a..f509abaf 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/CoralAutoDriveCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/CoralAutoDriveCommand.java @@ -70,9 +70,9 @@ public static Command getDriveToCoralCommand(Supplier distanceFro // () -> Y_PID_CONTROLLER.calculate(distanceFromTrackedCoral.get().getY()), // CoralAutoDriveCommand::calculateTargetAngle // ).until(() -> shouldMoveTowardsCoral(distanceFromTrackedCoral.get())), - new InstantCommand(() -> X_PID_CONTROLLER.reset(distanceFromTrackedCoral.get().getX())), + new InstantCommand(() -> X_PID_CONTROLLER.reset(distanceFromTrackedCoral.get().getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().vxMetersPerSecond)), SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> X_PID_CONTROLLER.calculate(distanceFromTrackedCoral.get().getX(), -0.65), + () -> X_PID_CONTROLLER.calculate(distanceFromTrackedCoral.get().getX(), -0.7), () -> Y_PID_CONTROLLER.calculate(distanceFromTrackedCoral.get().getY()), CoralAutoDriveCommand::calculateTargetAngle ) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index bbd7376d..004f319a 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -76,15 +76,15 @@ public static Command getDriveToCoralCommand(boolean isRight) { } public static Command getFindCoralCommand(boolean isRight) { - return new ParallelCommandGroup( - new RunCommand(() -> CameraConstants.OBJECT_DETECTION_CAMERA.trackObject(SimulatedGamePieceConstants.GamePieceType.CORAL)), - SwerveCommands.getDriveToPoseCommand(() -> isRight ? FieldConstants.AUTO_FIND_CORAL_POSE_RIGHT : FieldConstants.AUTO_FIND_CORAL_POSE_LEFT, PathPlannerConstants.DRIVE_TO_REEF_CONSTRAINTS).andThen( - SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> 0, - () -> 0, - () -> 0.2 - ) + return new SequentialCommandGroup( + SwerveCommands.getDriveToPoseCommand(() -> isRight ? FieldConstants.AUTO_FIND_CORAL_POSE_RIGHT : FieldConstants.AUTO_FIND_CORAL_POSE_LEFT, PathPlannerConstants.DRIVE_TO_REEF_CONSTRAINTS, 1.5), + SwerveCommands.getClosedLoopSelfRelativeDriveCommand( + () -> 0, + () -> 0, + () -> 0.2 ) + ).alongWith( + new RunCommand(() -> CameraConstants.OBJECT_DETECTION_CAMERA.trackObject(SimulatedGamePieceConstants.GamePieceType.CORAL)) ); } diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java index 00750141..5bcc462c 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -83,7 +83,7 @@ private static Command getGripperScoringSequenceCommand() { return new SequentialCommandGroup( GripperCommands.getSetTargetStateCommand(GripperConstants.GripperState.OPEN_FOR_NOT_HITTING_REEF) .unless(() -> RobotContainer.ELEVATOR.atState(REEF_CHOOSER.getElevatorState()) || REEF_CHOOSER.getScoringLevel() == ScoringLevel.L2 || REEF_CHOOSER.getScoringLevel() == ScoringLevel.L1_GRIPPER) - .until(() -> RobotContainer.ELEVATOR.atState(REEF_CHOOSER.getElevatorState())), + .until(RobotContainer.ELEVATOR::isCloseEnoughToOpenGripper), scoreFromGripperReefChooserCommand() ); } @@ -92,7 +92,7 @@ private static Command getAutoGripperScoringSequenceCommand() { return new SequentialCommandGroup( GripperCommands.getSetTargetStateCommand(GripperConstants.GripperState.OPEN_FOR_NOT_HITTING_REEF) .unless(() -> RobotContainer.ELEVATOR.atState(REEF_CHOOSER.getElevatorState()) || REEF_CHOOSER.getScoringLevel() == ScoringLevel.L2 || REEF_CHOOSER.getScoringLevel() == ScoringLevel.L1_GRIPPER) - .until(() -> RobotContainer.ELEVATOR.atState(REEF_CHOOSER.getElevatorState())), + .until(RobotContainer.ELEVATOR::isCloseEnoughToOpenGripper), new ConditionalCommand( scoreFromGripperInL4Command(), scoreFromGripperReefChooserCommand(), 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 f7fd7ece..4a2020fd 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -75,6 +75,10 @@ public void sysIdDrive(double targetVoltage) { masterMotor.setControl(voltageRequest.withOutput(targetVoltage)); } + public boolean isCloseEnoughToOpenGripper() { + return calculateTargetStateDistance() < 0.25; + } + public boolean willCurrentMovementMoveThroughHitRange() { return willMovementMoveThroughHitRange(metersToRotations(targetState.targetPositionMeters)); } @@ -93,8 +97,11 @@ public boolean atState(ElevatorConstants.ElevatorState targetState) { @AutoLogOutput(key = "Elevator/AtTargetState") public boolean atTargetState() { - final double currentToTargetStateDifference = Math.abs(targetState.targetPositionMeters - getPositionMeters()); - return currentToTargetStateDifference < ElevatorConstants.POSITION_TOLERANCE_METERS; + return calculateTargetStateDistance() < ElevatorConstants.POSITION_TOLERANCE_METERS; + } + + private double calculateTargetStateDistance() { + return Math.abs(targetState.targetPositionMeters - getPositionMeters()); } public double metersToRotations(double positionMeters) { diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java index 56f95558..9460fb93 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java @@ -135,6 +135,10 @@ public static Command getDriveToPoseCommand(Supplier targetPose return new DeferredCommand(() -> getCurrentDriveToPoseCommand(targetPose.get(), constraints), Set.of(RobotContainer.SWERVE)); } + public static Command getDriveToPoseCommand(Supplier targetPose, PathConstraints constraints, double endVelocity) { + return new DeferredCommand(() -> getCurrentDriveToPoseCommand(targetPose.get(), constraints, endVelocity), Set.of(RobotContainer.SWERVE)); + } + private static Command getCurrentDriveToPoseCommand(FlippablePose2d targetPose, PathConstraints constraints) { return new SequentialCommandGroup( new InstantCommand(() -> RobotContainer.SWERVE.initializeDrive(true)), @@ -143,6 +147,13 @@ private static Command getCurrentDriveToPoseCommand(FlippablePose2d targetPose, ); } + private static Command getCurrentDriveToPoseCommand(FlippablePose2d targetPose, PathConstraints constraints, double endVelocity) { + return new SequentialCommandGroup( + new InstantCommand(() -> RobotContainer.SWERVE.initializeDrive(true)), + AutoBuilder.pathfindToPose(targetPose.get(), constraints, endVelocity) + ); + } + private static Command getPIDToPoseCommand(FlippablePose2d targetPose) { return new FunctionalCommand( RobotContainer.SWERVE::resetRotationController, From bcee93c5a271e60ba174897122408e36503126b5 Mon Sep 17 00:00:00 2001 From: Yishai Levy <96019039+levyishai@users.noreply.github.com> Date: Tue, 1 Apr 2025 00:05:42 +0300 Subject: [PATCH 03/65] cool all works --- .../java/frc/trigon/robot/subsystems/elevator/Elevator.java | 2 +- .../frc/trigon/robot/subsystems/gripper/GripperConstants.java | 2 +- 2 files changed, 2 insertions(+), 2 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 4a2020fd..977076ed 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -76,7 +76,7 @@ public void sysIdDrive(double targetVoltage) { } public boolean isCloseEnoughToOpenGripper() { - return calculateTargetStateDistance() < 0.25; + return calculateTargetStateDistance() < 0.3; } public boolean willCurrentMovementMoveThroughHitRange() { diff --git a/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java b/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java index db9e3835..c6f5b1f6 100644 --- a/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java @@ -249,7 +249,7 @@ public enum GripperState { SCORE_ALGAE_IN_PROCESSOR(Rotation2d.fromDegrees(-35), 11, 1), PREPARE_FOR_SCORING_ALGAE_IN_PROCESSOR(Rotation2d.fromDegrees(-35), COLLECT_ALGAE_FROM_REEF.targetGripperVoltage, 0.3), COLLECT_CORAL_FROM_FEEDER(Rotation2d.fromDegrees(90), 8, 1), - OPEN_FOR_NOT_HITTING_REEF(Rotation2d.fromDegrees(107), 0.4, 1); + OPEN_FOR_NOT_HITTING_REEF(Rotation2d.fromDegrees(107), 0.2, 1); final Rotation2d targetAngle; final double targetGripperVoltage; From 91c6f84f0355c35a892e4d89f647d1745d204090 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Tue, 1 Apr 2025 18:59:47 +0300 Subject: [PATCH 04/65] Joystick relative when intaking (easily removable, dw)) --- .../java/frc/trigon/robot/commands/CommandConstants.java | 8 ++++---- .../commandfactories/CoralCollectionCommands.java | 4 +++- .../java/frc/trigon/robot/subsystems/swerve/Swerve.java | 3 +-- 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index 97165017..aa16059b 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -31,10 +31,10 @@ public class CommandConstants { private static final double JOYSTICK_ORIENTED_ROTATION_DEADBAND = 0.07; public static final Command - FIELD_RELATIVE_DRIVE_WITH_JOYSTICK_ORIENTED_ROTATION_TO_REEF_SECTIONS_COMMAND = SwerveCommands.getClosedLoopFieldRelativeDriveCommand( + FIELD_RELATIVE_DRIVE_WITH_JOYSTICK_ORIENTED_ROTATION_COMMAND = SwerveCommands.getClosedLoopFieldRelativeDriveCommand( () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftY()), () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftX()), - CommandConstants::calculateJoystickOrientedToReefSectionsTargetAngle + CommandConstants::calculateTargetHeadingFromJoystickAngle ), SELF_RELATIVE_DRIVE_COMMAND = SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftY()), @@ -106,9 +106,9 @@ public static double calculateShiftModeValue(double minimumPower) { * Calculates the target rotation value from the joystick's angle. Used for joystick oriented rotation. * Joystick oriented rotation is when the robot rotates directly to the joystick's angle. * - * @return the rotation value + * @return the target heading */ - public static FlippableRotation2d calculateJoystickOrientedToReefSectionsTargetAngle() { + private static FlippableRotation2d calculateTargetHeadingFromJoystickAngle() { final double xPower = DRIVER_CONTROLLER.getRightX(), yPower = DRIVER_CONTROLLER.getRightY(); 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 61dd3d78..8fbf445b 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -2,6 +2,7 @@ import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.coralintake.CoralIntakeCommands; import frc.trigon.robot.subsystems.coralintake.CoralIntakeConstants; @@ -40,7 +41,8 @@ private static Command getInitiateFeederCoralCollectionCommand() { private static Command getInitiateFloorCoralCollectionCommand() { return new ParallelCommandGroup( CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.COLLECT_FROM_FLOOR), - getScheduleCoralLoadingWhenCollectedCommand() + getScheduleCoralLoadingWhenCollectedCommand(), + CommandConstants.FIELD_RELATIVE_DRIVE_WITH_JOYSTICK_ORIENTED_ROTATION_COMMAND ); } 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 b90aa8a8..04af5c92 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -356,8 +356,7 @@ private boolean atTranslationPosition(double currentTranslationPosition, double } private double calculateProfiledAngleSpeedToTargetAngle(FlippableRotation2d targetAngle) { - if (targetAngle != null) - SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.setGoal(targetAngle.get().getDegrees()); + SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.setGoal(targetAngle == null ? getHeading().getDegrees() : targetAngle.get().getDegrees()); final Rotation2d currentAngle = RobotContainer.POSE_ESTIMATOR.getEstimatedRobotPose().getRotation(); final double output = Units.degreesToRadians(SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.calculate(currentAngle.getDegrees())); From 191ca3fb3e8d3f71cf10079d5106834d40342cc7 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 1 Apr 2025 22:10:49 +0300 Subject: [PATCH 05/65] Possibly better algae --- .../AlgaeManipulationCommands.java | 59 +++++++++++++------ .../commandfactories/AutonomousCommands.java | 2 +- .../robot/constants/OperatorConstants.java | 1 + .../robot/subsystems/elevator/Elevator.java | 6 +- .../elevator/ElevatorConstants.java | 4 +- .../subsystems/gripper/GripperConstants.java | 1 + 6 files changed, 51 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java index db0e23dd..850b8562 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -21,13 +21,16 @@ import org.trigon.utilities.flippable.FlippableRotation2d; import org.trigon.utilities.flippable.FlippableTranslation2d; +import java.util.Map; + public class AlgaeManipulationCommands { private static final ReefChooser REEF_CHOOSER = OperatorConstants.REEF_CHOOSER; public static Command getCollectAlgaeFromLollipopCommand() { return new SequentialCommandGroup( CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.GRIPPER::hasGamePiece).asProxy(), - getGripAlgaeCommand(GripperConstants.GripperState.COLLECT_ALGAE_FROM_LOLLIPOP).asProxy().until(() -> OperatorConstants.SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean() || OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean()), + getGripAlgaeCommand(GripperConstants.GripperState.COLLECT_ALGAE_FROM_LOLLIPOP).asProxy() + .until(AlgaeManipulationCommands::isScoreAlgaeButtonPressed), getScoreAlgaeCommand().asProxy() ).raceWith(getScoreNetEndingConditionCommand()).andThen(AlgaeManipulationCommands::reloadAfterScore); } @@ -53,14 +56,14 @@ private static void reloadAfterScore() { } private static Command getScoreNetEndingConditionCommand() { - return new WaitUntilCommand(() -> OperatorConstants.CONTINUE_TRIGGER.getAsBoolean() && RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.SCORE_NET)).andThen(new WaitCommand(0.5)); + return new WaitUntilCommand(() -> RobotContainer.GRIPPER.atState(GripperConstants.GripperState.SCORE_ALGAE_IN_NET) || RobotContainer.GRIPPER.atState(GripperConstants.GripperState.QUICK_SCORE_ALGAE_IN_NET)).andThen(new WaitCommand(0.1)); } private static Command getCollectAlgaeFromReefManuallyCommand() { return new ParallelCommandGroup( getGripAlgaeCommand(GripperConstants.GripperState.COLLECT_ALGAE_FROM_REEF), getOpenElevatorForAlgaeCommand() - ).until(() -> OperatorConstants.SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean() || OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean()).andThen( + ).until(AlgaeManipulationCommands::isScoreAlgaeButtonPressed).andThen( getScoreAlgaeCommand() ); } @@ -83,13 +86,38 @@ private static Command getOpenElevatorForAlgaeCommand() { } private static Command getScoreAlgaeCommand() { - return new ConditionalCommand( - getScoreInNetCommand(), - getScoreInProcessorCommand(), - OperatorConstants.SCORE_ALGAE_IN_NET_TRIGGER + return new SelectCommand<>( + Map.of( + 1, getQuickScoreInNetCommand(), + 2, getScoreInNetCommand(), + 3, getScoreInProcessorCommand() + ), + AlgaeManipulationCommands::getAlgaeScoreMethodSelector ); } + private static int getAlgaeScoreMethodSelector() { + if (OperatorConstants.QUICK_SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean()) + return 1; + else if (OperatorConstants.SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean()) + return 2; + else if (OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean()) + return 3; + return 0; + } + + private static Command getQuickScoreInNetCommand() { + return new ParallelCommandGroup( + new WaitCommand(0.3).andThen(ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.QUICK_SCORE_NET)), + AlgaeManipulatorCommands.getOpenCommand(), + new SequentialCommandGroup( + GripperCommands.getSetTargetStateCommand(GripperConstants.GripperState.PREPARE_FOR_SCORING_ALGAE_IN_NET) + .until(RobotContainer.ELEVATOR::isOverQuickNetScoreReleaseHeight), + GripperCommands.getSetTargetStateCommand(GripperConstants.GripperState.QUICK_SCORE_ALGAE_IN_NET) + ) + ).until(() -> RobotContainer.GRIPPER.atState(GripperConstants.GripperState.QUICK_SCORE_ALGAE_IN_NET)); + } + private static Command getScoreInNetCommand() { return new ParallelCommandGroup( ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_NET), @@ -114,7 +142,6 @@ private static Command getScoreInProcessorCommand() { GripperCommands.getSetTargetStateCommand(GripperConstants.GripperState.SCORE_ALGAE_IN_PROCESSOR) ), SwerveCommands.getClosedLoopFieldRelativeDriveCommand( - () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), () -> new FlippableRotation2d(Rotation2d.fromDegrees(90), true) @@ -136,19 +163,13 @@ private static Command getAlignToReefCommand() { ).until(OperatorConstants.CONTINUE_TRIGGER); } - private static Pose2d calculateReefAlgaeCollectionPose() { - final Translation2d reefCenterPositionOnField = new FlippableTranslation2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, true).get(); - final Rotation2d targetAngle = calculateTargetAngle().get(); - final Transform2d reefCenterToTargetPoseTranslation = new Transform2d(FieldConstants.REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS, 0, new Rotation2d()); - - return new Pose2d(reefCenterPositionOnField, targetAngle).transformBy(reefCenterToTargetPoseTranslation); - } - - private static FlippableRotation2d calculateTargetAngle() { - return new FlippableRotation2d(REEF_CHOOSER.getClockPosition().clockAngle, true); + private static boolean isScoreAlgaeButtonPressed() { + return OperatorConstants.SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean() || + OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean() || + OperatorConstants.QUICK_SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean(); } - public static FlippablePose2d calculateClosestAlgaeCollectionPose() { + private static FlippablePose2d calculateClosestAlgaeCollectionPose() { final Translation2d robotPositionOnField = RobotContainer.POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); final Translation2d reefCenterPosition = new FlippableTranslation2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, true).get(); final Rotation2d[] reefClockAngles = FieldConstants.REEF_CLOCK_ANGLES; diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index 004f319a..85fe2e44 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -99,7 +99,7 @@ public static Command getDriveToReefWithoutHittingAlgaeCommand(FieldConstants.Re return new SequentialCommandGroup( new InstantCommand(() -> TARGET_SCORING_POSE = calculateClosestScoringPose(true, reefClockPositions, false)), new WaitUntilCommand(() -> TARGET_SCORING_POSE != null).raceWith(SwerveCommands.getClosedLoopSelfRelativeDriveCommand(() -> 0, () -> 0, () -> 0)), - SwerveCommands.getDriveToPoseCommand(() -> calculateClosestScoringPose(true, reefClockPositions, true), PathPlannerConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly().until(RobotContainer.ELEVATOR::isElevatorOverAlgaeHitRange), + SwerveCommands.getDriveToPoseCommand(() -> calculateClosestScoringPose(true, reefClockPositions, true), PathPlannerConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly().until(RobotContainer.ELEVATOR::isOverAlgaeHitRange), SwerveCommands.getDriveToPoseCommand(() -> TARGET_SCORING_POSE, PathPlannerConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly() ); } diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index f3b2eb52..b5627797 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -43,6 +43,7 @@ public class OperatorConstants { COLLECT_ALGAE_FROM_REEF_TRIGGER = OPERATOR_CONTROLLER.a().or(DRIVER_CONTROLLER.a()), COLLECT_ALGAE_FROM_LOLLIPOP_TRIGGER = OPERATOR_CONTROLLER.l().or(DRIVER_CONTROLLER.y()), FEEDER_CORAL_COLLECTION_WITH_GRIPPER = OPERATOR_CONTROLLER.d(), + QUICK_SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.x(), SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.n().or(DRIVER_CONTROLLER.b()), SCORE_ALGAE_IN_PROCESSOR_TRIGGER = OPERATOR_CONTROLLER.j(), INCREMENT_TARGET_SCORING_LEVEL_TRIGGER = DRIVER_CONTROLLER.povUp(), 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 977076ed..deeb7f9f 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -83,7 +83,11 @@ public boolean willCurrentMovementMoveThroughHitRange() { return willMovementMoveThroughHitRange(metersToRotations(targetState.targetPositionMeters)); } - public boolean isElevatorOverAlgaeHitRange() { + public boolean isOverQuickNetScoreReleaseHeight() { + return getPositionRotations() > ElevatorConstants.QUICK_SCORE_NET_HEIGHT_LOWER_BOUND_POSITION_ROTATIONS; + } + + public boolean isOverAlgaeHitRange() { return getPositionRotations() > ElevatorConstants.GRIPPER_HITTING_ALGAE_UPPER_BOUND_POSITION_ROTATIONS; } 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 c4e9b707..e613897b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -79,7 +79,8 @@ public class ElevatorConstants { static final double GRIPPER_HITTING_ELEVATOR_BASE_LOWER_BOUND_POSITION_ROTATIONS = 0.29, GRIPPER_HITTING_ELEVATOR_BASE_UPPER_BOUND_POSITION_ROTATIONS = 0.7, - GRIPPER_HITTING_ALGAE_UPPER_BOUND_POSITION_ROTATIONS = 3; + GRIPPER_HITTING_ALGAE_UPPER_BOUND_POSITION_ROTATIONS = 3, + QUICK_SCORE_NET_HEIGHT_LOWER_BOUND_POSITION_ROTATIONS = 3; static { configureMasterMotor(); @@ -158,6 +159,7 @@ public enum ElevatorState { SCORE_L4(1.045, 1), COLLECT_ALGAE_FROM_L3(0.35, 1), REST_WITH_ALGAE(0, 0.3), + QUICK_SCORE_NET(1, 0.3), SCORE_NET(1.045, 0.3); public final double targetPositionMeters; diff --git a/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java b/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java index c6f5b1f6..31a4999a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java @@ -244,6 +244,7 @@ public enum GripperState { COLLECT_ALGAE_FROM_REEF(Rotation2d.fromDegrees(30), -4, 1), COLLECT_ALGAE_FROM_LOLLIPOP(Rotation2d.fromDegrees(-35), COLLECT_ALGAE_FROM_REEF.targetGripperVoltage, 1), HOLD_ALGAE(Rotation2d.fromDegrees(40), -40, 0.3), + QUICK_SCORE_ALGAE_IN_NET(Rotation2d.fromDegrees(60), 11, 1), SCORE_ALGAE_IN_NET(Rotation2d.fromDegrees(60), 11, 1), PREPARE_FOR_SCORING_ALGAE_IN_NET(Rotation2d.fromDegrees(100), COLLECT_ALGAE_FROM_REEF.targetGripperVoltage, 0.3), SCORE_ALGAE_IN_PROCESSOR(Rotation2d.fromDegrees(-35), 11, 1), From a07aa068ea31982f5529971720dd3ada635913e0 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 1 Apr 2025 22:57:48 +0300 Subject: [PATCH 06/65] Quick algae shot for me --- .../java/frc/trigon/robot/constants/OperatorConstants.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index b5627797..d8039b2a 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -2,6 +2,7 @@ import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.trigon.robot.RobotContainer; import frc.trigon.robot.misc.ReefChooser; import org.trigon.hardware.misc.KeyboardController; import org.trigon.hardware.misc.XboxController; @@ -35,7 +36,7 @@ public class OperatorConstants { BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.down(), FLOOR_CORAL_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftTrigger().or(OPERATOR_CONTROLLER.c()), FEEDER_CORAL_COLLECTION_TRIGGER = OPERATOR_CONTROLLER.f().or(DRIVER_CONTROLLER.start()), - SCORE_CORAL_IN_REEF_TRIGGER = DRIVER_CONTROLLER.rightBumper().or(OPERATOR_CONTROLLER.q()), + SCORE_CORAL_IN_REEF_TRIGGER = DRIVER_CONTROLLER.rightBumper().and(() -> !RobotContainer.ALGAE_MANIPULATOR.isOpen()).or(OPERATOR_CONTROLLER.q()), CONTINUE_TRIGGER = OPERATOR_CONTROLLER.k().or(DRIVER_CONTROLLER.leftBumper()), EJECT_CORAL_TRIGGER = OPERATOR_CONTROLLER.e().or(DRIVER_CONTROLLER.x()), DEBUGGING_TRIGGER = OPERATOR_CONTROLLER.f2(), @@ -43,7 +44,7 @@ public class OperatorConstants { COLLECT_ALGAE_FROM_REEF_TRIGGER = OPERATOR_CONTROLLER.a().or(DRIVER_CONTROLLER.a()), COLLECT_ALGAE_FROM_LOLLIPOP_TRIGGER = OPERATOR_CONTROLLER.l().or(DRIVER_CONTROLLER.y()), FEEDER_CORAL_COLLECTION_WITH_GRIPPER = OPERATOR_CONTROLLER.d(), - QUICK_SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.x(), + QUICK_SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.x().or(DRIVER_CONTROLLER.rightBumper().and(RobotContainer.ALGAE_MANIPULATOR::isOpen)), SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.n().or(DRIVER_CONTROLLER.b()), SCORE_ALGAE_IN_PROCESSOR_TRIGGER = OPERATOR_CONTROLLER.j(), INCREMENT_TARGET_SCORING_LEVEL_TRIGGER = DRIVER_CONTROLLER.povUp(), From 224c28a68b641bbeef9c77d7e0fa67b2592dae25 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 2 Apr 2025 14:50:17 +0300 Subject: [PATCH 07/65] It all actually works now --- .../algaemanipulator/AlgaeManipulatorConstants.java | 4 ++-- .../trigon/robot/subsystems/gripper/GripperConstants.java | 6 +++--- .../java/frc/trigon/robot/subsystems/swerve/Swerve.java | 4 +++- 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulatorConstants.java index 5a749685..959d9e5b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulatorConstants.java @@ -57,8 +57,8 @@ public class AlgaeManipulatorConstants { static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(1); static final Rotation2d MAXIMUM_RESTING_GRIPPER_ANGLE = Rotation2d.fromDegrees(0); static final double - OPEN_TORQUE_CURRENT = 20, - CLOSE_TO_LIMIT_TORQUE_CURRENT = -20; + OPEN_TORQUE_CURRENT = 17, + CLOSE_TO_LIMIT_TORQUE_CURRENT = -18; static { final TalonFXConfiguration config = new TalonFXConfiguration(); diff --git a/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java b/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java index 31a4999a..c0375ad7 100644 --- a/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java @@ -243,14 +243,14 @@ public enum GripperState { UNLOAD_CORAL(Rotation2d.fromDegrees(-50), -3, 1), COLLECT_ALGAE_FROM_REEF(Rotation2d.fromDegrees(30), -4, 1), COLLECT_ALGAE_FROM_LOLLIPOP(Rotation2d.fromDegrees(-35), COLLECT_ALGAE_FROM_REEF.targetGripperVoltage, 1), - HOLD_ALGAE(Rotation2d.fromDegrees(40), -40, 0.3), + HOLD_ALGAE(Rotation2d.fromDegrees(40), -2, 0.3), QUICK_SCORE_ALGAE_IN_NET(Rotation2d.fromDegrees(60), 11, 1), SCORE_ALGAE_IN_NET(Rotation2d.fromDegrees(60), 11, 1), PREPARE_FOR_SCORING_ALGAE_IN_NET(Rotation2d.fromDegrees(100), COLLECT_ALGAE_FROM_REEF.targetGripperVoltage, 0.3), SCORE_ALGAE_IN_PROCESSOR(Rotation2d.fromDegrees(-35), 11, 1), - PREPARE_FOR_SCORING_ALGAE_IN_PROCESSOR(Rotation2d.fromDegrees(-35), COLLECT_ALGAE_FROM_REEF.targetGripperVoltage, 0.3), + PREPARE_FOR_SCORING_ALGAE_IN_PROCESSOR(Rotation2d.fromDegrees(-35), -3, 0.3), COLLECT_CORAL_FROM_FEEDER(Rotation2d.fromDegrees(90), 8, 1), - OPEN_FOR_NOT_HITTING_REEF(Rotation2d.fromDegrees(107), 0.2, 1); + OPEN_FOR_NOT_HITTING_REEF(Rotation2d.fromDegrees(107), 0.4, 1); final Rotation2d targetAngle; final double targetGripperVoltage; 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 04af5c92..a6d0a833 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -356,7 +356,9 @@ private boolean atTranslationPosition(double currentTranslationPosition, double } private double calculateProfiledAngleSpeedToTargetAngle(FlippableRotation2d targetAngle) { - SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.setGoal(targetAngle == null ? getHeading().getDegrees() : targetAngle.get().getDegrees()); + if (targetAngle == null) + return 0; + SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.setGoal(targetAngle.get().getDegrees()); final Rotation2d currentAngle = RobotContainer.POSE_ESTIMATOR.getEstimatedRobotPose().getRotation(); final double output = Units.degreesToRadians(SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.calculate(currentAngle.getDegrees())); From c59001106ab6e8025a03fc9f83705792fc9fc8dc Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 2 Apr 2025 14:54:43 +0300 Subject: [PATCH 08/65] =?UTF-8?q?No=20time=20=F0=9F=98=AA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../commands/commandfactories/CoralCollectionCommands.java | 5 ++--- 1 file changed, 2 insertions(+), 3 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 8fbf445b..369ce3a3 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -2,7 +2,6 @@ import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.coralintake.CoralIntakeCommands; import frc.trigon.robot.subsystems.coralintake.CoralIntakeConstants; @@ -41,8 +40,8 @@ private static Command getInitiateFeederCoralCollectionCommand() { private static Command getInitiateFloorCoralCollectionCommand() { return new ParallelCommandGroup( CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.COLLECT_FROM_FLOOR), - getScheduleCoralLoadingWhenCollectedCommand(), - CommandConstants.FIELD_RELATIVE_DRIVE_WITH_JOYSTICK_ORIENTED_ROTATION_COMMAND + getScheduleCoralLoadingWhenCollectedCommand() +// CommandConstants.FIELD_RELATIVE_DRIVE_WITH_JOYSTICK_ORIENTED_ROTATION_COMMAND ); } From a49ce39110ea942d9fdeb0529b2f97fae085eb9c Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 2 Apr 2025 18:17:01 +0300 Subject: [PATCH 09/65] I have more controls --- .../commands/commandfactories/AlgaeManipulationCommands.java | 2 +- .../java/frc/trigon/robot/constants/OperatorConstants.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java index 850b8562..c2e058b5 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -146,7 +146,7 @@ private static Command getScoreInProcessorCommand() { () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), () -> new FlippableRotation2d(Rotation2d.fromDegrees(90), true) ).asProxy() - ); + ).until(() -> RobotContainer.GRIPPER.atState(GripperConstants.GripperState.SCORE_ALGAE_IN_PROCESSOR) && OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean()); } private static Command getAlignToReefCommand() { diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index d8039b2a..3a4c55e8 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -38,7 +38,7 @@ public class OperatorConstants { FEEDER_CORAL_COLLECTION_TRIGGER = OPERATOR_CONTROLLER.f().or(DRIVER_CONTROLLER.start()), SCORE_CORAL_IN_REEF_TRIGGER = DRIVER_CONTROLLER.rightBumper().and(() -> !RobotContainer.ALGAE_MANIPULATOR.isOpen()).or(OPERATOR_CONTROLLER.q()), CONTINUE_TRIGGER = OPERATOR_CONTROLLER.k().or(DRIVER_CONTROLLER.leftBumper()), - EJECT_CORAL_TRIGGER = OPERATOR_CONTROLLER.e().or(DRIVER_CONTROLLER.x()), + EJECT_CORAL_TRIGGER = OPERATOR_CONTROLLER.e().or(() -> DRIVER_CONTROLLER.x().getAsBoolean() && !RobotContainer.ALGAE_MANIPULATOR.isOpen()), DEBUGGING_TRIGGER = OPERATOR_CONTROLLER.f2(), UNLOAD_CORAL_TRIGGER = OPERATOR_CONTROLLER.z().or(DRIVER_CONTROLLER.back()), COLLECT_ALGAE_FROM_REEF_TRIGGER = OPERATOR_CONTROLLER.a().or(DRIVER_CONTROLLER.a()), @@ -46,7 +46,7 @@ public class OperatorConstants { FEEDER_CORAL_COLLECTION_WITH_GRIPPER = OPERATOR_CONTROLLER.d(), QUICK_SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.x().or(DRIVER_CONTROLLER.rightBumper().and(RobotContainer.ALGAE_MANIPULATOR::isOpen)), SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.n().or(DRIVER_CONTROLLER.b()), - SCORE_ALGAE_IN_PROCESSOR_TRIGGER = OPERATOR_CONTROLLER.j(), + SCORE_ALGAE_IN_PROCESSOR_TRIGGER = OPERATOR_CONTROLLER.j().or(() -> DRIVER_CONTROLLER.x().getAsBoolean() && RobotContainer.ALGAE_MANIPULATOR.isOpen()), INCREMENT_TARGET_SCORING_LEVEL_TRIGGER = DRIVER_CONTROLLER.povUp(), DECREMENT_TARGET_SCORING_LEVEL_TRIGGER = DRIVER_CONTROLLER.povDown(), LED_AUTO_SETUP_TRIGGER = OPERATOR_CONTROLLER.backtick(); From 4403172914d04f223e56a8d13ed2033eaa75c18f Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 2 Apr 2025 18:27:15 +0300 Subject: [PATCH 10/65] Refactor --- .../commandfactories/AlgaeManipulationCommands.java | 8 ++++---- .../java/frc/trigon/robot/subsystems/gripper/Gripper.java | 4 ++-- .../trigon/robot/subsystems/gripper/GripperCommands.java | 4 ++-- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java index c2e058b5..6e0bb887 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -70,8 +70,8 @@ private static Command getCollectAlgaeFromReefManuallyCommand() { private static Command getGripAlgaeCommand(GripperConstants.GripperState targetGripState) { return new SequentialCommandGroup( - GripperCommands.getSetTargetStateWithCurrentCommand(targetGripState).raceWith(new WaitCommand(1).andThen(new WaitUntilCommand(RobotContainer.GRIPPER::isMovingSlowly))), - GripperCommands.getSetTargetStateWithCurrentCommand(GripperConstants.GripperState.HOLD_ALGAE) + GripperCommands.getSetTargetStateWhileHoldingAlgaeCommand(targetGripState).raceWith(new WaitCommand(1).andThen(new WaitUntilCommand(RobotContainer.GRIPPER::isMovingSlowly))), + GripperCommands.getSetTargetStateWhileHoldingAlgaeCommand(GripperConstants.GripperState.HOLD_ALGAE) ).alongWith( AlgaeManipulatorCommands.getOpenCommand() ); @@ -123,7 +123,7 @@ private static Command getScoreInNetCommand() { ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_NET), AlgaeManipulatorCommands.getOpenCommand(), new SequentialCommandGroup( - GripperCommands.getSetTargetStateWithCurrentCommand(GripperConstants.GripperState.PREPARE_FOR_SCORING_ALGAE_IN_NET).until(OperatorConstants.CONTINUE_TRIGGER), + GripperCommands.getSetTargetStateWhileHoldingAlgaeCommand(GripperConstants.GripperState.PREPARE_FOR_SCORING_ALGAE_IN_NET).until(OperatorConstants.CONTINUE_TRIGGER), GripperCommands.getSetTargetStateCommand(GripperConstants.GripperState.SCORE_ALGAE_IN_NET) ), SwerveCommands.getClosedLoopFieldRelativeDriveCommand( @@ -138,7 +138,7 @@ private static Command getScoreInProcessorCommand() { return new ParallelCommandGroup( AlgaeManipulatorCommands.getOpenCommand().until(OperatorConstants.CONTINUE_TRIGGER), new SequentialCommandGroup( - GripperCommands.getSetTargetStateWithCurrentCommand(GripperConstants.GripperState.PREPARE_FOR_SCORING_ALGAE_IN_PROCESSOR).until(OperatorConstants.CONTINUE_TRIGGER), + GripperCommands.getSetTargetStateWhileHoldingAlgaeCommand(GripperConstants.GripperState.PREPARE_FOR_SCORING_ALGAE_IN_PROCESSOR).until(OperatorConstants.CONTINUE_TRIGGER), GripperCommands.getSetTargetStateCommand(GripperConstants.GripperState.SCORE_ALGAE_IN_PROCESSOR) ), SwerveCommands.getClosedLoopFieldRelativeDriveCommand( diff --git a/src/main/java/frc/trigon/robot/subsystems/gripper/Gripper.java b/src/main/java/frc/trigon/robot/subsystems/gripper/Gripper.java index db48b6d4..e944ee47 100644 --- a/src/main/java/frc/trigon/robot/subsystems/gripper/Gripper.java +++ b/src/main/java/frc/trigon/robot/subsystems/gripper/Gripper.java @@ -173,11 +173,11 @@ void setTargetState(Rotation2d targetAngle, double targetGrippingVoltage) { setTargetVoltage(targetGrippingVoltage); } - void setTargetStateWithCurrent(Rotation2d targetAngle, double targetGrippingCurrent) { + void setTargetStateWhileHoldingAlgae(Rotation2d targetAngle, double targetGrippingVoltage) { positionRequest.Slot = 1; scalePositionRequestSpeed(targetState.speedScalar); setTargetAngle(targetAngle); - grippingMotor.setControl(voltageRequest.withOutput(targetGrippingCurrent)); + setTargetVoltage(targetGrippingVoltage); } private void scalePositionRequestSpeed(double speedScalar) { diff --git a/src/main/java/frc/trigon/robot/subsystems/gripper/GripperCommands.java b/src/main/java/frc/trigon/robot/subsystems/gripper/GripperCommands.java index 9bb1c802..369a0c57 100644 --- a/src/main/java/frc/trigon/robot/subsystems/gripper/GripperCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/gripper/GripperCommands.java @@ -42,9 +42,9 @@ public static Command getGripperDefaultCommand() { ); } - public static Command getSetTargetStateWithCurrentCommand(GripperConstants.GripperState targetState) { + public static Command getSetTargetStateWhileHoldingAlgaeCommand(GripperConstants.GripperState targetState) { return new StartEndCommand( - () -> RobotContainer.GRIPPER.setTargetStateWithCurrent(targetState.targetAngle, targetState.targetGripperVoltage), + () -> RobotContainer.GRIPPER.setTargetStateWhileHoldingAlgae(targetState.targetAngle, targetState.targetGripperVoltage), RobotContainer.GRIPPER::stop, RobotContainer.GRIPPER ); From 410339d0c3bd14704f0cf3c67139a08c46a1af26 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 3 Apr 2025 23:55:01 +0300 Subject: [PATCH 11/65] Add floor algae to processor which may or may not work --- src/main/java/frc/trigon/robot/RobotContainer.java | 1 + .../commandfactories/AlgaeManipulationCommands.java | 13 +++++++++++++ .../trigon/robot/constants/OperatorConstants.java | 1 + .../robot/subsystems/coralintake/CoralIntake.java | 5 +++++ .../coralintake/CoralIntakeConstants.java | 5 ++++- 5 files changed, 24 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index af09fd6b..76acd475 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -111,6 +111,7 @@ private void bindControllerCommands() { OperatorConstants.UNLOAD_CORAL_TRIGGER.whileTrue(CoralCollectionCommands.getUnloadCoralCommand()); OperatorConstants.COLLECT_ALGAE_FROM_REEF_TRIGGER.toggleOnTrue(AlgaeManipulationCommands.getCollectAlgaeFromReefCommand()); OperatorConstants.COLLECT_ALGAE_FROM_LOLLIPOP_TRIGGER.toggleOnTrue(AlgaeManipulationCommands.getCollectAlgaeFromLollipopCommand()); + OperatorConstants.COLLECT_ALGAE_FROM_FLOOR_TRIGGER.toggleOnTrue(AlgaeManipulationCommands.getCollectAlgaeFromFloorCommand()); OperatorConstants.FEEDER_CORAL_COLLECTION_WITH_GRIPPER.whileTrue(CoralCollectionCommands.getFeederCoralCollectionFromGripperCommand()); } diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java index 6e0bb887..750a0a72 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -12,6 +12,8 @@ import frc.trigon.robot.constants.PathPlannerConstants; import frc.trigon.robot.misc.ReefChooser; import frc.trigon.robot.subsystems.algaemanipulator.AlgaeManipulatorCommands; +import frc.trigon.robot.subsystems.coralintake.CoralIntakeCommands; +import frc.trigon.robot.subsystems.coralintake.CoralIntakeConstants; import frc.trigon.robot.subsystems.elevator.ElevatorCommands; import frc.trigon.robot.subsystems.elevator.ElevatorConstants; import frc.trigon.robot.subsystems.gripper.GripperCommands; @@ -44,6 +46,17 @@ public static Command getCollectAlgaeFromReefCommand() { .raceWith(getScoreNetEndingConditionCommand()).andThen(AlgaeManipulationCommands::reloadAfterScore); } + public static Command getCollectAlgaeFromFloorCommand() { + return new SequentialCommandGroup( + CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.COLLECT_ALGAE_FROM_FLOOR) + .raceWith(new WaitCommand(1).andThen(new WaitUntilCommand(() -> RobotContainer.CORAL_INTAKE.isMovingSlowly() || OperatorConstants.CONTINUE_TRIGGER.getAsBoolean()))), + CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.PREPARE_SCORE_ALGAE_IN_PROCESSOR) + .raceWith(new WaitCommand(1).andThen(new WaitUntilCommand(OperatorConstants.CONTINUE_TRIGGER))), + CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.SCORE_ALGAE_IN_PROCESSOR) + .until(OperatorConstants.CONTINUE_TRIGGER.negate()) + ); + } + public static Command getResetAmpAlignerCommand() { return new ParallelRaceGroup( GripperCommands.getSetTargetStateCommand(GripperConstants.GripperState.REST), diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 3a4c55e8..990afe5b 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -43,6 +43,7 @@ public class OperatorConstants { UNLOAD_CORAL_TRIGGER = OPERATOR_CONTROLLER.z().or(DRIVER_CONTROLLER.back()), COLLECT_ALGAE_FROM_REEF_TRIGGER = OPERATOR_CONTROLLER.a().or(DRIVER_CONTROLLER.a()), COLLECT_ALGAE_FROM_LOLLIPOP_TRIGGER = OPERATOR_CONTROLLER.l().or(DRIVER_CONTROLLER.y()), + COLLECT_ALGAE_FROM_FLOOR_TRIGGER = OPERATOR_CONTROLLER.s(), FEEDER_CORAL_COLLECTION_WITH_GRIPPER = OPERATOR_CONTROLLER.d(), QUICK_SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.x().or(DRIVER_CONTROLLER.rightBumper().and(RobotContainer.ALGAE_MANIPULATOR::isOpen)), SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.n().or(DRIVER_CONTROLLER.b()), diff --git a/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntake.java b/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntake.java index 506169c7..8a1b99d8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntake.java +++ b/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntake.java @@ -122,6 +122,11 @@ public boolean isEarlyCoralCollectionDetected() { return CoralIntakeConstants.EARLY_CORAL_COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); } + @AutoLogOutput + public boolean isMovingSlowly() { + return Math.abs(intakeMotor.getSignal(TalonFXSignal.VELOCITY)) < 4; + } + /** * Calculates the pose where the coral should actually be collected from relative to the robot. * diff --git a/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeConstants.java index 1d370fd6..07696d5b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeConstants.java @@ -189,6 +189,7 @@ private static void configureIntakeMotor() { INTAKE_MOTOR.setPhysicsSimulation(INTAKE_SIMULATION); INTAKE_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); + INTAKE_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100); INTAKE_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); } @@ -293,7 +294,9 @@ public enum CoralIntakeState { REST(0, 0, LOAD_CORAL_TO_GRIPPER_SEEING_GAME_PIECE_WITH_BEAM_BREAK.targetAngle), SCORE_L1_BOOST(-3, -3, Rotation2d.fromDegrees(33)), SCORE_L1(-2, 3, Rotation2d.fromDegrees(33)), - COLLECT_ALGAE_FROM_FLOOR(-6, 0, Rotation2d.fromDegrees(-40)); + COLLECT_ALGAE_FROM_FLOOR(5, 0, Rotation2d.fromDegrees(-15)), + PREPARE_SCORE_ALGAE_IN_PROCESSOR(5, 0, Rotation2d.fromDegrees(15)), + SCORE_ALGAE_IN_PROCESSOR(-2, 0, PREPARE_SCORE_ALGAE_IN_PROCESSOR.targetAngle); public final double targetIntakeVoltage, From 4170f6f5e19a0a659fb7e4387d6ddc384bd57bae Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sat, 5 Apr 2025 22:44:41 +0300 Subject: [PATCH 12/65] Refactor Triggers + roll algae capabilities --- .../java/frc/trigon/robot/RobotContainer.java | 13 ++++++---- .../AlgaeManipulationCommands.java | 5 ++++ .../robot/constants/OperatorConstants.java | 25 +++++++++++-------- .../coralintake/CoralIntakeConstants.java | 1 + 4 files changed, 28 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 76acd475..bdfd5ae8 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -98,21 +98,24 @@ private void bindControllerCommands() { // OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); // OperatorConstants.TOGGLE_ROTATION_MODE_TRIGGER.onTrue(GeneralCommands.getToggleRotationModeCommand()); OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); - OperatorConstants.RESET_AMP_ALIGNER_TRIGGER.whileTrue(AlgaeManipulationCommands.getResetAmpAlignerCommand()); - OperatorConstants.FLOOR_CORAL_COLLECTION_TRIGGER.and(OperatorConstants.LEFT_MULTIFUNCTION_TRIGGER).whileTrue(new CoralAutoDriveCommand()); + OperatorConstants.DEBUGGING_TRIGGER.whileTrue(CommandConstants.WHEEL_RADIUS_CHARACTERIZATION_COMMAND); OperatorConstants.OPERATOR_CONTROLLER.f3().whileTrue(new CoralAlignmentCommand()); + + OperatorConstants.RESET_AMP_ALIGNER_TRIGGER.whileTrue(AlgaeManipulationCommands.getResetAmpAlignerCommand()); + OperatorConstants.FLOOR_CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getFloorCoralCollectionCommand()); + OperatorConstants.FLOOR_CORAL_COLLECTION_TRIGGER.and(OperatorConstants.LEFT_MULTIFUNCTION_TRIGGER).whileTrue(new CoralAutoDriveCommand()); OperatorConstants.FEEDER_CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getFeederCoralCollectionCommand()); + OperatorConstants.FEEDER_CORAL_COLLECTION_WITH_GRIPPER.whileTrue(CoralCollectionCommands.getFeederCoralCollectionFromGripperCommand()); OperatorConstants.SCORE_CORAL_IN_REEF_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand()); - - OperatorConstants.DEBUGGING_TRIGGER.whileTrue(CommandConstants.WHEEL_RADIUS_CHARACTERIZATION_COMMAND); OperatorConstants.EJECT_CORAL_TRIGGER.whileTrue(EjectionCommands.getEjectCoralCommand()); OperatorConstants.UNLOAD_CORAL_TRIGGER.whileTrue(CoralCollectionCommands.getUnloadCoralCommand()); + OperatorConstants.COLLECT_ALGAE_FROM_REEF_TRIGGER.toggleOnTrue(AlgaeManipulationCommands.getCollectAlgaeFromReefCommand()); OperatorConstants.COLLECT_ALGAE_FROM_LOLLIPOP_TRIGGER.toggleOnTrue(AlgaeManipulationCommands.getCollectAlgaeFromLollipopCommand()); OperatorConstants.COLLECT_ALGAE_FROM_FLOOR_TRIGGER.toggleOnTrue(AlgaeManipulationCommands.getCollectAlgaeFromFloorCommand()); - OperatorConstants.FEEDER_CORAL_COLLECTION_WITH_GRIPPER.whileTrue(CoralCollectionCommands.getFeederCoralCollectionFromGripperCommand()); + OperatorConstants.ROLL_ALGAE_ON_FLOOR_TRIGGER.toggleOnTrue(AlgaeManipulationCommands.getRollAlgaeOnFloorCommand()); } private void bindSetters() { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java index 750a0a72..d4d89c91 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -57,6 +57,11 @@ public static Command getCollectAlgaeFromFloorCommand() { ); } + public static Command getRollAlgaeOnFloorCommand() { + return CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.ROLL_ALGAE_ON_FLOOR) + .until(OperatorConstants.CONTINUE_TRIGGER); + } + public static Command getResetAmpAlignerCommand() { return new ParallelRaceGroup( GripperCommands.getSetTargetStateCommand(GripperConstants.GripperState.REST), diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 990afe5b..4dcde4a2 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -24,36 +24,39 @@ public class OperatorConstants { ROTATION_STICK_SPEED_DIVIDER = 1; public static final Trigger + LED_AUTO_SETUP_TRIGGER = OPERATOR_CONTROLLER.backtick(), RESET_HEADING_TRIGGER = OPERATOR_CONTROLLER.r(), DRIVE_FROM_DPAD_TRIGGER = new Trigger(() -> DRIVER_CONTROLLER.getPov() != -1), TOGGLE_BRAKE_TRIGGER = OPERATOR_CONTROLLER.g().or(RobotController::getUserButton), - RESET_AMP_ALIGNER_TRIGGER = OPERATOR_CONTROLLER.v(), + DEBUGGING_TRIGGER = OPERATOR_CONTROLLER.f2(), LEFT_MULTIFUNCTION_TRIGGER = DRIVER_CONTROLLER.leftStick().or(OPERATOR_CONTROLLER.b()), RIGHT_MULTIFUNCTION_TRIGGER = DRIVER_CONTROLLER.rightStick().or(OPERATOR_CONTROLLER.m()), FORWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.right(), BACKWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.left(), FORWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.up(), BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.down(), + CONTINUE_TRIGGER = OPERATOR_CONTROLLER.k().or(DRIVER_CONTROLLER.leftBumper()); + public static final Trigger + RESET_AMP_ALIGNER_TRIGGER = OPERATOR_CONTROLLER.v(), FLOOR_CORAL_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftTrigger().or(OPERATOR_CONTROLLER.c()), FEEDER_CORAL_COLLECTION_TRIGGER = OPERATOR_CONTROLLER.f().or(DRIVER_CONTROLLER.start()), + FEEDER_CORAL_COLLECTION_WITH_GRIPPER = OPERATOR_CONTROLLER.d(), SCORE_CORAL_IN_REEF_TRIGGER = DRIVER_CONTROLLER.rightBumper().and(() -> !RobotContainer.ALGAE_MANIPULATOR.isOpen()).or(OPERATOR_CONTROLLER.q()), - CONTINUE_TRIGGER = OPERATOR_CONTROLLER.k().or(DRIVER_CONTROLLER.leftBumper()), EJECT_CORAL_TRIGGER = OPERATOR_CONTROLLER.e().or(() -> DRIVER_CONTROLLER.x().getAsBoolean() && !RobotContainer.ALGAE_MANIPULATOR.isOpen()), - DEBUGGING_TRIGGER = OPERATOR_CONTROLLER.f2(), UNLOAD_CORAL_TRIGGER = OPERATOR_CONTROLLER.z().or(DRIVER_CONTROLLER.back()), COLLECT_ALGAE_FROM_REEF_TRIGGER = OPERATOR_CONTROLLER.a().or(DRIVER_CONTROLLER.a()), COLLECT_ALGAE_FROM_LOLLIPOP_TRIGGER = OPERATOR_CONTROLLER.l().or(DRIVER_CONTROLLER.y()), COLLECT_ALGAE_FROM_FLOOR_TRIGGER = OPERATOR_CONTROLLER.s(), - FEEDER_CORAL_COLLECTION_WITH_GRIPPER = OPERATOR_CONTROLLER.d(), + ROLL_ALGAE_ON_FLOOR_TRIGGER = OPERATOR_CONTROLLER.w(), QUICK_SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.x().or(DRIVER_CONTROLLER.rightBumper().and(RobotContainer.ALGAE_MANIPULATOR::isOpen)), SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.n().or(DRIVER_CONTROLLER.b()), - SCORE_ALGAE_IN_PROCESSOR_TRIGGER = OPERATOR_CONTROLLER.j().or(() -> DRIVER_CONTROLLER.x().getAsBoolean() && RobotContainer.ALGAE_MANIPULATOR.isOpen()), - INCREMENT_TARGET_SCORING_LEVEL_TRIGGER = DRIVER_CONTROLLER.povUp(), - DECREMENT_TARGET_SCORING_LEVEL_TRIGGER = DRIVER_CONTROLLER.povDown(), - LED_AUTO_SETUP_TRIGGER = OPERATOR_CONTROLLER.backtick(); + SCORE_ALGAE_IN_PROCESSOR_TRIGGER = OPERATOR_CONTROLLER.j().or(() -> DRIVER_CONTROLLER.x().getAsBoolean() && RobotContainer.ALGAE_MANIPULATOR.isOpen()); public static final Trigger + ENABLE_IGNORE_LOLLIPOP_CORAL_TRIGGER = OPERATOR_CONTROLLER.o(), + DISABLE_IGNORE_LOLLIPOP_CORAL_TRIGGER = OPERATOR_CONTROLLER.p(), ENABLE_AUTONOMOUS_REEF_SCORING_TRIGGER = OPERATOR_CONTROLLER.u(), - DISABLE_AUTONOMOUS_REEF_SCORING_TRIGGER = OPERATOR_CONTROLLER.i(), + DISABLE_AUTONOMOUS_REEF_SCORING_TRIGGER = OPERATOR_CONTROLLER.i(); + public static final Trigger SET_TARGET_SCORING_REEF_LEVEL_L1_FROM_GRIPPER_TRIGGER = OPERATOR_CONTROLLER.numpadDecimal(), SET_TARGET_SCORING_REEF_LEVEL_L1_FROM_CORAL_INTAKE_TRIGGER = OPERATOR_CONTROLLER.numpad0(), SET_TARGET_SCORING_REEF_LEVEL_L2_TRIGGER = OPERATOR_CONTROLLER.numpad1(), @@ -67,6 +70,6 @@ public class OperatorConstants { SET_TARGET_SCORING_REEF_CLOCK_POSITION_12_OCLOCK_TRIGGER = OPERATOR_CONTROLLER.numpad8(), SET_TARGET_REEF_SCORING_SIDE_LEFT_TRIGGER = OPERATOR_CONTROLLER.left().or(DRIVER_CONTROLLER.povLeft()), SET_TARGET_REEF_SCORING_SIDE_RIGHT_TRIGGER = OPERATOR_CONTROLLER.right().or(DRIVER_CONTROLLER.povRight()), - ENABLE_IGNORE_LOLLIPOP_CORAL_TRIGGER = OPERATOR_CONTROLLER.o(), - DISABLE_IGNORE_LOLLIPOP_CORAL_TRIGGER = OPERATOR_CONTROLLER.p(); + INCREMENT_TARGET_SCORING_LEVEL_TRIGGER = DRIVER_CONTROLLER.povUp(), + DECREMENT_TARGET_SCORING_LEVEL_TRIGGER = DRIVER_CONTROLLER.povDown(); } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeConstants.java index 07696d5b..a532e471 100644 --- a/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeConstants.java @@ -294,6 +294,7 @@ public enum CoralIntakeState { REST(0, 0, LOAD_CORAL_TO_GRIPPER_SEEING_GAME_PIECE_WITH_BEAM_BREAK.targetAngle), SCORE_L1_BOOST(-3, -3, Rotation2d.fromDegrees(33)), SCORE_L1(-2, 3, Rotation2d.fromDegrees(33)), + ROLL_ALGAE_ON_FLOOR(2, 0, Rotation2d.fromDegrees(15)), COLLECT_ALGAE_FROM_FLOOR(5, 0, Rotation2d.fromDegrees(-15)), PREPARE_SCORE_ALGAE_IN_PROCESSOR(5, 0, Rotation2d.fromDegrees(15)), SCORE_ALGAE_IN_PROCESSOR(-2, 0, PREPARE_SCORE_ALGAE_IN_PROCESSOR.targetAngle); From 8a4fa5828c6fa5cb17848f4684bdcd0236c2ea34 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 6 Apr 2025 02:47:16 +0300 Subject: [PATCH 13/65] Different approach to rolling algae --- .../robot/subsystems/coralintake/CoralIntakeConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeConstants.java index a532e471..a9467636 100644 --- a/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeConstants.java @@ -294,7 +294,7 @@ public enum CoralIntakeState { REST(0, 0, LOAD_CORAL_TO_GRIPPER_SEEING_GAME_PIECE_WITH_BEAM_BREAK.targetAngle), SCORE_L1_BOOST(-3, -3, Rotation2d.fromDegrees(33)), SCORE_L1(-2, 3, Rotation2d.fromDegrees(33)), - ROLL_ALGAE_ON_FLOOR(2, 0, Rotation2d.fromDegrees(15)), + ROLL_ALGAE_ON_FLOOR(-2, 0, Rotation2d.fromDegrees(-45)), COLLECT_ALGAE_FROM_FLOOR(5, 0, Rotation2d.fromDegrees(-15)), PREPARE_SCORE_ALGAE_IN_PROCESSOR(5, 0, Rotation2d.fromDegrees(15)), SCORE_ALGAE_IN_PROCESSOR(-2, 0, PREPARE_SCORE_ALGAE_IN_PROCESSOR.targetAngle); From c2164da87204a28bb0a40f249349f259c1dbf2b7 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 6 Apr 2025 04:23:01 +0300 Subject: [PATCH 14/65] Add algae manipulation --- .../SimulatedGamePieceConstants.java | 8 ++++---- .../SimulationFieldHandler.java | 20 +++++++++++++------ .../algaemanipulator/AlgaeManipulator.java | 17 +++++++++++++++- .../AlgaeManipulatorConstants.java | 12 ++++++++++- .../robot/subsystems/gripper/Gripper.java | 15 +++++++++++++- .../subsystems/gripper/GripperConstants.java | 7 +++++-- 6 files changed, 64 insertions(+), 15 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 bc3bb55a..a1666507 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java @@ -14,7 +14,7 @@ public class SimulatedGamePieceConstants { public static final double CORAL_INTAKE_TOLERANCE_METERS = 0.3, CORAL_FEEDER_INTAKE_TOLERANCE_METERS = 1, - ALGAE_INTAKE_TOLERANCE_METERS = 0.4, + ALGAE_INTAKE_TOLERANCE_METERS = 0.3, CORAL_SCORING_TOLERANCE_METERS = 0.1, ALGAE_SCORING_TOLERANCE_METERS = 0.2; @@ -42,8 +42,8 @@ public class SimulatedGamePieceConstants { 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())) - )); + 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), @@ -118,7 +118,7 @@ private static FlippablePose3d calculateCoralScorePose(int level, int clockFace, } public enum GamePieceType { - ALGAE(0.15, 0), + ALGAE(0.2, 0), CORAL(0.06, 1); public final double originPointHeightOffGroundMeters; 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 ebd810e8..5d834eb4 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -19,7 +19,7 @@ public class SimulationFieldHandler { CORAL_ON_FIELD = SimulatedGamePieceConstants.CORAL_ON_FIELD, ALGAE_ON_FIELD = SimulatedGamePieceConstants.ALGAE_ON_FIELD; private static Integer - HELD_CORAL_INDEX = 0, + HELD_CORAL_INDEX = null, HELD_ALGAE_INDEX = null; private static boolean IS_CORAL_IN_GRIPPER = true; @@ -78,7 +78,7 @@ private static void updateCollection() { final Pose3d robotPose = new Pose3d(RobotContainer.POSE_ESTIMATOR.getEstimatedRobotPose()); final Pose3d coralCollectionPose = robotPose.plus(toTransform(RobotContainer.CORAL_INTAKE.calculateCoralCollectionPose())), - algaeCollectionPose = robotPose.plus(toTransform(new Pose3d())); + algaeCollectionPose = robotPose.plus(toTransform(RobotContainer.GRIPPER.calculateAlgaeCollectionPose())); updateCoralFeederCollection(robotPose); @@ -131,7 +131,7 @@ private static boolean isCollectingCoralFromFeeder() { } private static boolean isCollectingAlgae() { - return false;//TODO: Implement + return RobotContainer.ALGAE_MANIPULATOR.isOpen(); } private static void updateEjection() { @@ -140,7 +140,7 @@ private static void updateEjection() { if (HELD_ALGAE_INDEX != null && isEjectingAlgae()) { final SimulatedGamePiece heldAlgae = ALGAE_ON_FIELD.get(HELD_ALGAE_INDEX); - heldAlgae.release(); + ejectAlgae(heldAlgae); HELD_ALGAE_INDEX = null; } } @@ -157,6 +157,14 @@ private static void updateCoralEjection() { ejectCoralFromGripper(heldCoral); } + private static void ejectAlgae(SimulatedGamePiece ejectedAlgae) { + final ChassisSpeeds swerveWheelSpeeds = RobotContainer.SWERVE.getSelfRelativeVelocity(); + final Translation3d robotSelfRelativeVelocity = new Translation3d(swerveWheelSpeeds.vxMetersPerSecond, swerveWheelSpeeds.vyMetersPerSecond, 0); + final Translation3d robotRelativeReleaseVelocity = RobotContainer.GRIPPER.calculateAlgaeReleaseVelocity(); + + ejectedAlgae.release(robotSelfRelativeVelocity.plus(robotRelativeReleaseVelocity).rotateBy(new Rotation3d(RobotContainer.SWERVE.getHeading()))); + } + private static void ejectCoralFromGripper(SimulatedGamePiece ejectedCoral) { final Pose3d robotPose = new Pose3d(RobotContainer.POSE_ESTIMATOR.getEstimatedRobotPose()); final Pose3d robotRelativeGripperReleasePose = RobotContainer.GRIPPER.calculateCoralReleasePoint(); @@ -180,7 +188,7 @@ private static boolean isIntakeEjectingCoral() { } private static boolean isEjectingAlgae() { - return false;//TODO: Implement + return RobotContainer.GRIPPER.isReleasingAlgae() || !RobotContainer.ALGAE_MANIPULATOR.isOpen(); } /** @@ -189,7 +197,7 @@ private static boolean isEjectingAlgae() { private static void updateHeldGamePiecePoses() { final Pose3d robotRelativeHeldCoralPosition = IS_CORAL_IN_GRIPPER ? RobotContainer.GRIPPER.calculateHeldCoralVisualizationPose() : RobotContainer.CORAL_INTAKE.calculateCollectedCoralPose(), - robotRelativeHeldAlgaePosition = new Pose3d(); + robotRelativeHeldAlgaePosition = RobotContainer.GRIPPER.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/algaemanipulator/AlgaeManipulator.java b/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulator.java index 26658a79..b72437fd 100644 --- a/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulator.java +++ b/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulator.java @@ -3,12 +3,14 @@ import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VoltageOut; -import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.*; 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 org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; import org.trigon.utilities.Conversions; @@ -58,6 +60,8 @@ public void updateMechanism() { getCurrentAngle(), Rotation2d.fromRotations(angleMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE)) ); + + Logger.recordOutput("Poses/Components/AlgaeManipulatorPose", calculateVisualizationPose()); } @Override @@ -100,6 +104,17 @@ void setTargetAngle(Rotation2d targetAngle) { angleMotor.setControl(positionRequest.withPosition(targetAngle.getRotations())); } + private Pose3d calculateVisualizationPose() { + final Pose3d currentGripperPose = RobotContainer.GRIPPER.calculateVisualizationPose(); + final Pose3d gripperOrigin = currentGripperPose.transformBy(AlgaeManipulatorConstants.GRIPPER_TO_ALGAE_MANIPULATOR); + + final Transform3d pitchTransform = new Transform3d( + new Translation3d(0, 0, 0), + new Rotation3d(0, getCurrentAngle().getRadians(), 0) + ); + return gripperOrigin.transformBy(pitchTransform); + } + private Rotation2d getCurrentAngle() { return Rotation2d.fromRotations(angleMotor.getSignal(TalonFXSignal.POSITION)); } diff --git a/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulatorConstants.java index 959d9e5b..d020fc55 100644 --- a/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulatorConstants.java @@ -2,7 +2,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.*; -import edu.wpi.first.math.geometry.Rotation2d; +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.util.Color; @@ -47,6 +47,16 @@ public class AlgaeManipulatorConstants { Units.Second.of(1000) ); + static final Pose3d ALGAE_MANIPULATOR_VISUALIZATION_ORIGIN_POINT = new Pose3d( + new Translation3d(0, 0, 0), + new Rotation3d(0, edu.wpi.first.math.util.Units.degreesToRadians(0), 0) + ); + static final Transform3d + GRIPPER_TO_ALGAE_MANIPULATOR = new Transform3d( + GripperConstants.GRIPPER_VISUALIZATION_ORIGIN_POINT, + ALGAE_MANIPULATOR_VISUALIZATION_ORIGIN_POINT + ); + static final SingleJointedArmMechanism2d ANGLE_MECHANISM = new SingleJointedArmMechanism2d( "AlgaeManipulatorAngleMechanism", ARM_LENGTH_METERS, diff --git a/src/main/java/frc/trigon/robot/subsystems/gripper/Gripper.java b/src/main/java/frc/trigon/robot/subsystems/gripper/Gripper.java index e944ee47..e403198d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/gripper/Gripper.java +++ b/src/main/java/frc/trigon/robot/subsystems/gripper/Gripper.java @@ -106,10 +106,18 @@ public boolean isEjecting() { return grippingMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE) <= GripperConstants.MINIMUM_VOLTAGE_FOR_EJECTING; } + public boolean isReleasingAlgae() { + return grippingMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE) > -2; + } + public boolean isOpenForElevator() { return getCurrentEncoderAngle().getDegrees() > GripperConstants.MINIMUM_OPEN_FOR_ELEVATOR_ANGLE.getDegrees(); } + public Translation3d calculateAlgaeReleaseVelocity() { + return new Translation3d(3, 0, 0).rotateBy(new Rotation3d(0, getCurrentEncoderAngle().getRadians() + 2, 0)); + } + @AutoLogOutput(key = "Gripper/HasGamePiece") public boolean hasGamePiece() { return GripperConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); @@ -146,6 +154,11 @@ public boolean isMovingSlowly() { return Math.abs(grippingMotor.getSignal(TalonFXSignal.VELOCITY)) < 4; } + public Pose3d calculateAlgaeCollectionPose() { + return calculateVisualizationPose() + .transformBy(GripperConstants.GRIPPER_TO_HELD_ALGAE); + } + void prepareForState(GripperConstants.GripperState targetState) { this.targetState = targetState; this.targetAngle = targetState.targetAngle; @@ -195,7 +208,7 @@ void setTargetVoltage(double targetGrippingVoltage) { grippingMotor.setControl(voltageRequest.withOutput(targetGrippingVoltage)); } - private Pose3d calculateVisualizationPose() { + public Pose3d calculateVisualizationPose() { final Pose3d currentElevatorPose = RobotContainer.ELEVATOR.getFirstStageComponentPose(); final Pose3d gripperOrigin = currentElevatorPose.transformBy(GripperConstants.ELEVATOR_TO_GRIPPER); diff --git a/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java b/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java index c0375ad7..75fa74e1 100644 --- a/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java @@ -99,7 +99,7 @@ public class GripperConstants { GRIPPER_LENGTH_METERS, Color.kRed ); - private static final Pose3d GRIPPER_VISUALIZATION_ORIGIN_POINT = new Pose3d( + public static final Pose3d GRIPPER_VISUALIZATION_ORIGIN_POINT = new Pose3d( new Translation3d(-0.224, 0, 0.8622), new Rotation3d(0, edu.wpi.first.math.util.Units.degreesToRadians(62), 0) ); @@ -115,11 +115,14 @@ public class GripperConstants { GRIPPER_TO_HELD_CORAL = new Transform3d( new Translation3d(0.05, 0, -0.1), new Rotation3d(0, 0, 0) + ), + GRIPPER_TO_HELD_ALGAE = new Transform3d( + new Translation3d(-0.1, 0, -0.35), + new Rotation3d(0, edu.wpi.first.math.util.Units.degreesToRadians(0), 0) ); static final double WHEEL_DIAMETER_METERS = edu.wpi.first.math.util.Units.inchesToMeters(2.5); static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(2); - static final double SCORE_IN_REEF_FOR_AUTO_VOLTAGE = -5; static final double MINIMUM_VOLTAGE_FOR_EJECTING = -3; static final Rotation2d MINIMUM_OPEN_FOR_ELEVATOR_ANGLE = Rotation2d.fromDegrees(-34); private static final double COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.10; From c93925783f59282c261cc9861ece6b5f97b13161 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 6 Apr 2025 04:23:22 +0300 Subject: [PATCH 15/65] Oops --- .../misc/simulatedfield/SimulatedGamePieceConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 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 a1666507..8968e145 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java @@ -42,8 +42,8 @@ public class SimulatedGamePieceConstants { 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())), - )); + 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), From d16a743b219b994ae99d4b019cd7972883487df3 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 6 Apr 2025 13:53:59 +0300 Subject: [PATCH 16/65] Bit of an update --- .../robot/subsystems/coralintake/CoralIntakeConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeConstants.java index a9467636..72affd9d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeConstants.java @@ -296,7 +296,7 @@ public enum CoralIntakeState { SCORE_L1(-2, 3, Rotation2d.fromDegrees(33)), ROLL_ALGAE_ON_FLOOR(-2, 0, Rotation2d.fromDegrees(-45)), COLLECT_ALGAE_FROM_FLOOR(5, 0, Rotation2d.fromDegrees(-15)), - PREPARE_SCORE_ALGAE_IN_PROCESSOR(5, 0, Rotation2d.fromDegrees(15)), + PREPARE_SCORE_ALGAE_IN_PROCESSOR(5, 0, Rotation2d.fromDegrees(-15)), SCORE_ALGAE_IN_PROCESSOR(-2, 0, PREPARE_SCORE_ALGAE_IN_PROCESSOR.targetAngle); public final double From 5dfc15f93690356d24b6971291eda50a20fc121d Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Wed, 16 Apr 2025 08:26:45 -0500 Subject: [PATCH 17/65] all good bro --- .../java/frc/trigon/robot/commands/CommandConstants.java | 2 +- .../commands/commandclasses/CoralAlignmentCommand.java | 8 ++++++-- .../commandfactories/CoralCollectionCommands.java | 6 +++++- 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index aa16059b..be7898b8 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -108,7 +108,7 @@ public static double calculateShiftModeValue(double minimumPower) { * * @return the target heading */ - private static FlippableRotation2d calculateTargetHeadingFromJoystickAngle() { + public static FlippableRotation2d calculateTargetHeadingFromJoystickAngle() { final double xPower = DRIVER_CONTROLLER.getRightX(), yPower = DRIVER_CONTROLLER.getRightY(); diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/CoralAlignmentCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/CoralAlignmentCommand.java index 16993ba8..4b769003 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/CoralAlignmentCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/CoralAlignmentCommand.java @@ -1,6 +1,7 @@ package frc.trigon.robot.commands.commandclasses; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.CameraConstants; @@ -13,9 +14,11 @@ public class CoralAlignmentCommand extends ParallelCommandGroup { private static final ObjectDetectionCamera CAMERA = CameraConstants.OBJECT_DETECTION_CAMERA; + private FlippableRotation2d prev = null; public CoralAlignmentCommand() { addCommands( + new InstantCommand(() -> prev = null), getDriveWhileAligningToNoteCommand(), CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.COLLECT_FROM_FLOOR) ); @@ -28,8 +31,9 @@ private Command getDriveWhileAligningToNoteCommand() { () -> { var a = CAMERA.getTargetObjectsRotations(SimulatedGamePieceConstants.GamePieceType.CORAL); if (a.length == 0) - return null; - return new FlippableRotation2d(RobotContainer.POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().plus(a[0].toRotation2d()), false); + return prev; + prev = new FlippableRotation2d(RobotContainer.POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().plus(a[0].toRotation2d()), false); + return prev; } ); } 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 369ce3a3..44836eb6 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -41,7 +41,11 @@ private static Command getInitiateFloorCoralCollectionCommand() { return new ParallelCommandGroup( CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.COLLECT_FROM_FLOOR), getScheduleCoralLoadingWhenCollectedCommand() -// CommandConstants.FIELD_RELATIVE_DRIVE_WITH_JOYSTICK_ORIENTED_ROTATION_COMMAND +// SwerveCommands.getClosedLoopFieldRelativeDriveCommand( +// () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), +// () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), +// CommandConstants::calculateTargetHeadingFromJoystickAngle +// ).asProxy() ); } From 17b9b3878df6a415649804fc0ee45d611c449e67 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Wed, 16 Apr 2025 08:48:19 -0500 Subject: [PATCH 18/65] cool stuff bro --- .../subsystems/algaemanipulator/AlgaeManipulator.java | 4 ++++ .../algaemanipulator/AlgaeManipulatorCommands.java | 9 ++++++--- .../robot/subsystems/gripper/GripperConstants.java | 4 ++-- 3 files changed, 12 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulator.java b/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulator.java index b72437fd..f82221cb 100644 --- a/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulator.java +++ b/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulator.java @@ -73,6 +73,10 @@ public boolean isOpen() { return angleMotor.getSignal(TalonFXSignal.POSITION) > Conversions.degreesToRotations(70); } + public void resetPosition() { + angleMotor.setPosition(0); + } + public boolean atState(AlgaeManipulatorConstants.AlgaeManipulatorState state) { return targetState == state && atTargetAngle(); } diff --git a/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulatorCommands.java index 08ec973d..ef787b9a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulatorCommands.java @@ -1,10 +1,10 @@ package frc.trigon.robot.subsystems.algaemanipulator; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.StartEndCommand; +import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandfactories.GeneralCommands; +import frc.trigon.robot.constants.OperatorConstants; import org.trigon.commands.NetworkTablesCommand; import java.util.Set; @@ -45,7 +45,10 @@ public static Command getPressLimitSwitchCommand() { RobotContainer.ALGAE_MANIPULATOR::closeToLimit, RobotContainer.ALGAE_MANIPULATOR::stop, RobotContainer.ALGAE_MANIPULATOR - ).until(RobotContainer.ALGAE_MANIPULATOR::hasHitReverseLimit); + ).raceWith( + new WaitUntilCommand(RobotContainer.ALGAE_MANIPULATOR::hasHitReverseLimit), + new WaitUntilCommand(OperatorConstants.CONTINUE_TRIGGER).andThen(new InstantCommand(RobotContainer.ALGAE_MANIPULATOR::resetPosition)) + ); } public static Command getSetTargetStateCommand(AlgaeManipulatorConstants.AlgaeManipulatorState targetState) { diff --git a/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java b/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java index 75fa74e1..de17d223 100644 --- a/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java @@ -237,7 +237,7 @@ private static void configureBeamBreak() { public enum GripperState { REST(Rotation2d.fromDegrees(-56), 0, 1), EJECT(Rotation2d.fromDegrees(55), -3, 1), - EJECT_UPWARDS(Rotation2d.fromDegrees(107), -3, 1), + EJECT_UPWARDS(Rotation2d.fromDegrees(106), -3, 1), SCORE_L4_CLOSE(Rotation2d.fromDegrees(49), -6, 1), SCORE_L4_FAR(Rotation2d.fromDegrees(55), -6, 1), SCORE_L3_OR_L2(Rotation2d.fromDegrees(60), SCORE_L4_CLOSE.targetGripperVoltage, 1), @@ -253,7 +253,7 @@ public enum GripperState { SCORE_ALGAE_IN_PROCESSOR(Rotation2d.fromDegrees(-35), 11, 1), PREPARE_FOR_SCORING_ALGAE_IN_PROCESSOR(Rotation2d.fromDegrees(-35), -3, 0.3), COLLECT_CORAL_FROM_FEEDER(Rotation2d.fromDegrees(90), 8, 1), - OPEN_FOR_NOT_HITTING_REEF(Rotation2d.fromDegrees(107), 0.4, 1); + OPEN_FOR_NOT_HITTING_REEF(EJECT_UPWARDS.targetAngle, 0.4, 1); final Rotation2d targetAngle; final double targetGripperVoltage; From 4dad4a74d09aaf9a9c60d65360cb0974634c6d14 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Wed, 16 Apr 2025 18:58:54 -0500 Subject: [PATCH 19/65] cmt --- .../robot/commands/commandclasses/CoralAutoDriveCommand.java | 2 +- .../robot/commands/commandfactories/AutonomousCommands.java | 2 +- 2 files changed, 2 insertions(+), 2 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 f509abaf..8d47b2d1 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/CoralAutoDriveCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/CoralAutoDriveCommand.java @@ -72,7 +72,7 @@ public static Command getDriveToCoralCommand(Supplier distanceFro // ).until(() -> shouldMoveTowardsCoral(distanceFromTrackedCoral.get())), new InstantCommand(() -> X_PID_CONTROLLER.reset(distanceFromTrackedCoral.get().getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().vxMetersPerSecond)), SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> X_PID_CONTROLLER.calculate(distanceFromTrackedCoral.get().getX(), -0.7), + () -> X_PID_CONTROLLER.calculate(distanceFromTrackedCoral.get().getX(), -0.76), () -> Y_PID_CONTROLLER.calculate(distanceFromTrackedCoral.get().getY()), CoralAutoDriveCommand::calculateTargetAngle ) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index 85fe2e44..2b26ede8 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -77,7 +77,7 @@ public static Command getDriveToCoralCommand(boolean isRight) { public static Command getFindCoralCommand(boolean isRight) { return new SequentialCommandGroup( - SwerveCommands.getDriveToPoseCommand(() -> isRight ? FieldConstants.AUTO_FIND_CORAL_POSE_RIGHT : FieldConstants.AUTO_FIND_CORAL_POSE_LEFT, PathPlannerConstants.DRIVE_TO_REEF_CONSTRAINTS, 1.5), + SwerveCommands.getDriveToPoseCommand(() -> isRight ? FieldConstants.AUTO_FIND_CORAL_POSE_RIGHT : FieldConstants.AUTO_FIND_CORAL_POSE_LEFT, PathPlannerConstants.DRIVE_TO_REEF_CONSTRAINTS, 2.3), SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> 0, () -> 0, From 63e8fcdfac61b141d198ac808b7ba1cb09dcd6b8 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Thu, 17 Apr 2025 18:48:04 +0300 Subject: [PATCH 20/65] fix sim not always loading --- .../robot/subsystems/coralintake/CoralIntakeConstants.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeConstants.java index 72affd9d..e4b7be7e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeConstants.java @@ -172,6 +172,7 @@ public class CoralIntakeConstants { configureAngleMotor(); configureAngleEncoder(); configureBeamBreak(); + configureBackupBeamBreak(); configureDistanceSensor(); } @@ -278,6 +279,10 @@ private static void configureBeamBreak() { BEAM_BREAK.setSimulationSupplier(BEAM_BREAK_SIMULATION_VALUE_SUPPLIER); } + private static void configureBackupBeamBreak() { + BACKUP_BEAM_BREAK.setSimulationSupplier(BEAM_BREAK_SIMULATION_VALUE_SUPPLIER); + } + private static void configureDistanceSensor() { DISTANCE_SENSOR.setScalingConstants(DISTANCE_SENSOR_SCALING_SLOPE, DISTANCE_SENSOR_SCALING_INTERCEPT_POINT); DISTANCE_SENSOR.setSimulationSupplier(DISTANCE_SENSOR_SIMULATION_VALUE_SUPPLIER); From ea92f3674445321b26890c87bd607104e79396f3 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 5 May 2025 12:52:23 +0300 Subject: [PATCH 21/65] Controller Fixes from the lib (finally) --- build.gradle | 2 +- .../java/frc/trigon/robot/constants/OperatorConstants.java | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/build.gradle b/build.gradle index 0273d82c..2fe8b95a 100644 --- a/build.gradle +++ b/build.gradle @@ -107,7 +107,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() implementation 'com.google.code.gson:gson:2.10.1' - implementation 'com.github.Programming-TRIGON:TRIGONLib:v2025.3.0' + implementation 'com.github.Programming-TRIGON:TRIGONLib:c382fceed9' def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 4dcde4a2..b0b41aec 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -12,9 +12,11 @@ public class OperatorConstants { private static final int DRIVER_CONTROLLER_PORT = 0, REEF_CHOOSER_PORT = 1; - private static final int DRIVER_CONTROLLER_EXPONENT = 2; + private static final int + CONTROLLER_TRANSLATION_EXPONENT = 2, + CONTROLLER_ROTATION_EXPONENT = 1; public static final XboxController DRIVER_CONTROLLER = new XboxController( - DRIVER_CONTROLLER_PORT, DRIVER_CONTROLLER_EXPONENT, DRIVER_CONTROLLER_DEADBAND + DRIVER_CONTROLLER_PORT, CONTROLLER_ROTATION_EXPONENT, CONTROLLER_TRANSLATION_EXPONENT, DRIVER_CONTROLLER_DEADBAND ); public static final KeyboardController OPERATOR_CONTROLLER = new KeyboardController(); public static final ReefChooser REEF_CHOOSER = new ReefChooser(REEF_CHOOSER_PORT); From eb4ba4ceca2bc96cd552325332972fd899e1ed7e Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 29 May 2025 10:31:45 -0400 Subject: [PATCH 22/65] Made some better controls (not done) --- .../java/frc/trigon/robot/RobotContainer.java | 7 +- .../AlgaeManipulationCommands.java | 10 +-- .../CoralPlacingCommands.java | 83 +++++++++---------- .../robot/constants/OperatorConstants.java | 17 ++-- .../frc/trigon/robot/misc/ReefChooser.java | 2 - 5 files changed, 52 insertions(+), 67 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index bdfd5ae8..ca56a88b 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -95,8 +95,6 @@ private void bindDefaultCommands() { private void bindControllerCommands() { OperatorConstants.LED_AUTO_SETUP_TRIGGER.onTrue(new LEDAutoSetupCommand(() -> autoChooser.get().getName())); OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND); -// OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); -// OperatorConstants.TOGGLE_ROTATION_MODE_TRIGGER.onTrue(GeneralCommands.getToggleRotationModeCommand()); OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); OperatorConstants.DEBUGGING_TRIGGER.whileTrue(CommandConstants.WHEEL_RADIUS_CHARACTERIZATION_COMMAND); @@ -105,10 +103,11 @@ private void bindControllerCommands() { OperatorConstants.RESET_AMP_ALIGNER_TRIGGER.whileTrue(AlgaeManipulationCommands.getResetAmpAlignerCommand()); OperatorConstants.FLOOR_CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getFloorCoralCollectionCommand()); - OperatorConstants.FLOOR_CORAL_COLLECTION_TRIGGER.and(OperatorConstants.LEFT_MULTIFUNCTION_TRIGGER).whileTrue(new CoralAutoDriveCommand()); +// OperatorConstants.FLOOR_CORAL_COLLECTION_TRIGGER.and(OperatorConstants.LEFT_MULTIFUNCTION_TRIGGER).whileTrue(new CoralAutoDriveCommand()); OperatorConstants.FEEDER_CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getFeederCoralCollectionCommand()); OperatorConstants.FEEDER_CORAL_COLLECTION_WITH_GRIPPER.whileTrue(CoralCollectionCommands.getFeederCoralCollectionFromGripperCommand()); - OperatorConstants.SCORE_CORAL_IN_REEF_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand()); + OperatorConstants.RIGHT_SCORE_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(true)); + OperatorConstants.LEFT_SCORE_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(false)); OperatorConstants.EJECT_CORAL_TRIGGER.whileTrue(EjectionCommands.getEjectCoralCommand()); OperatorConstants.UNLOAD_CORAL_TRIGGER.whileTrue(CoralCollectionCommands.getUnloadCoralCommand()); diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java index d4d89c91..41604cd8 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -42,7 +42,7 @@ public static Command getCollectAlgaeFromReefCommand() { CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.GRIPPER::hasGamePiece).asProxy(), getCollectAlgaeFromReefManuallyCommand().asProxy() ) - .alongWith(getAlignToReefCommand().onlyIf(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY && !OperatorConstants.RIGHT_MULTIFUNCTION_TRIGGER.getAsBoolean()).asProxy()) + .alongWith(getAlignToReefCommand().onlyIf(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY).asProxy()) .raceWith(getScoreNetEndingConditionCommand()).andThen(AlgaeManipulationCommands::reloadAfterScore); } @@ -82,7 +82,7 @@ private static Command getCollectAlgaeFromReefManuallyCommand() { getGripAlgaeCommand(GripperConstants.GripperState.COLLECT_ALGAE_FROM_REEF), getOpenElevatorForAlgaeCommand() ).until(AlgaeManipulationCommands::isScoreAlgaeButtonPressed).andThen( - getScoreAlgaeCommand() + getScoreAlgaeCommand().asProxy() ); } @@ -99,7 +99,7 @@ private static Command getOpenElevatorForAlgaeCommand() { return GeneralCommands.getContinuousConditionalCommand( ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.COLLECT_ALGAE_FROM_L3), ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST_WITH_ALGAE), - OperatorConstants.LEFT_MULTIFUNCTION_TRIGGER + OperatorConstants.LEFT_SCORE_TRIGGER ); } @@ -137,12 +137,12 @@ private static Command getQuickScoreInNetCommand() { } private static Command getScoreInNetCommand() { - return new ParallelCommandGroup( + return new ParallelRaceGroup( ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_NET), AlgaeManipulatorCommands.getOpenCommand(), new SequentialCommandGroup( GripperCommands.getSetTargetStateWhileHoldingAlgaeCommand(GripperConstants.GripperState.PREPARE_FOR_SCORING_ALGAE_IN_NET).until(OperatorConstants.CONTINUE_TRIGGER), - GripperCommands.getSetTargetStateCommand(GripperConstants.GripperState.SCORE_ALGAE_IN_NET) + GripperCommands.getSetTargetStateCommand(GripperConstants.GripperState.SCORE_ALGAE_IN_NET).withTimeout(0.5) ), SwerveCommands.getClosedLoopFieldRelativeDriveCommand( () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java index 5bcc462c..6cd0b9f1 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -25,31 +25,23 @@ public class CoralPlacingCommands { public static boolean SHOULD_SCORE_AUTONOMOUSLY = true; private static final ReefChooser REEF_CHOOSER = OperatorConstants.REEF_CHOOSER; - public static Command getScoreInReefCommand() { + public static Command getScoreInReefCommand(boolean shouldScoreRight) { return new ConditionalCommand( - getCoralIntakeScoringSequenceCommand().asProxy(), - getScoreInReefFromGripperCommand().asProxy(), + getScoreInReefFromCoralIntakeCommand().asProxy(), + getScoreInReefFromGripperCommand(shouldScoreRight).asProxy(), () -> REEF_CHOOSER.getScoringLevel() == ScoringLevel.L1_CORAL_INTAKE - ).raceWith(getWaitUntilScoringTargetChangesCommand()).repeatedly(); - } - - private static Command getWaitUntilScoringTargetChangesCommand() { - return new ParallelRaceGroup( - new WaitUntilChangeCommand<>(REEF_CHOOSER::getClockPosition), - new WaitUntilChangeCommand<>(REEF_CHOOSER::getReefSide), - new WaitUntilChangeCommand<>(OperatorConstants.RIGHT_MULTIFUNCTION_TRIGGER::getAsBoolean) ); } - private static Command getScoreInReefFromGripperCommand() { + private static Command getScoreInReefFromGripperCommand(boolean shouldScoreRight) { return GeneralCommands.getContinuousConditionalCommand( - getAutonomouslyScoreInReefFromGripperCommand().asProxy(), + getAutonomouslyScoreInReefFromGripperCommand(shouldScoreRight).asProxy(), getManuallyScoreInReefFromGripperCommand().asProxy(), - () -> SHOULD_SCORE_AUTONOMOUSLY && !OperatorConstants.LEFT_MULTIFUNCTION_TRIGGER.getAsBoolean() && REEF_CHOOSER.getScoringLevel() != ScoringLevel.L1_GRIPPER + () -> SHOULD_SCORE_AUTONOMOUSLY && REEF_CHOOSER.getScoringLevel() != ScoringLevel.L1_GRIPPER ).until(() -> REEF_CHOOSER.getScoringLevel() == ScoringLevel.L1_CORAL_INTAKE); } - private static Command getCoralIntakeScoringSequenceCommand() { + private static Command getScoreInReefFromCoralIntakeCommand() { return new SequentialCommandGroup( CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.GRIPPER::hasGamePiece), CoralIntakeCommands.getPrepareForStateCommand(CoralIntakeConstants.CoralIntakeState.SCORE_L1).until(CoralPlacingCommands::canContinueScoringFromCoralIntake), @@ -67,16 +59,16 @@ private static Command getManuallyScoreInReefFromGripperCommand() { ); } - private static Command getAutonomouslyScoreInReefFromGripperCommand() { - return new ParallelCommandGroup( + private static Command getAutonomouslyScoreInReefFromGripperCommand(boolean shouldScoreRight) { + return new ParallelRaceGroup( CoralCollectionCommands.getLoadCoralCommand().asProxy().andThen( new ParallelCommandGroup( - getOpenElevatorWhenCloseToReefCommand().raceWith(new WaitUntilChangeCommand<>(REEF_CHOOSER::getElevatorState)).repeatedly(), - getAutoGripperScoringSequenceCommand() + getOpenElevatorWhenCloseToReefCommand(shouldScoreRight).raceWith(new WaitUntilChangeCommand<>(REEF_CHOOSER::getElevatorState)).repeatedly(), + getAutoGripperScoringSequenceCommand(shouldScoreRight) ).asProxy() ), - getAutonomousDriveToReefThenManualDriveCommand() - ); + getAutonomousDriveToReefThenManualDriveCommand(shouldScoreRight) + ).andThen(); } private static Command getGripperScoringSequenceCommand() { @@ -88,70 +80,70 @@ private static Command getGripperScoringSequenceCommand() { ); } - private static Command getAutoGripperScoringSequenceCommand() { + private static Command getAutoGripperScoringSequenceCommand(boolean shouldScoreRight) { return new SequentialCommandGroup( GripperCommands.getSetTargetStateCommand(GripperConstants.GripperState.OPEN_FOR_NOT_HITTING_REEF) .unless(() -> RobotContainer.ELEVATOR.atState(REEF_CHOOSER.getElevatorState()) || REEF_CHOOSER.getScoringLevel() == ScoringLevel.L2 || REEF_CHOOSER.getScoringLevel() == ScoringLevel.L1_GRIPPER) .until(RobotContainer.ELEVATOR::isCloseEnoughToOpenGripper), new ConditionalCommand( - scoreFromGripperInL4Command(), - scoreFromGripperReefChooserCommand(), + scoreFromGripperInL4Command(shouldScoreRight), + scoreFromGripperReefChooserCommand(shouldScoreRight), () -> REEF_CHOOSER.getScoringLevel() == ScoringLevel.L4 ) ); } + private static Command scoreFromGripperReefChooserCommand(boolean shouldScoreRight) { + return new SequentialCommandGroup( + GripperCommands.getPrepareForStateCommand(REEF_CHOOSER::getGripperState).until(() -> canContinueScoringFromGripper(shouldScoreRight)), + GripperCommands.getSetTargetStateCommand(REEF_CHOOSER::getGripperState).finallyDo(OperatorConstants.REEF_CHOOSER::switchReefSide) + ); + } + private static Command scoreFromGripperReefChooserCommand() { return new SequentialCommandGroup( - GripperCommands.getPrepareForStateCommand(REEF_CHOOSER::getGripperState).until(CoralPlacingCommands::canContinueScoringFromGripper), + GripperCommands.getPrepareForStateCommand(REEF_CHOOSER::getGripperState).until(OperatorConstants.CONTINUE_TRIGGER), GripperCommands.getSetTargetStateCommand(REEF_CHOOSER::getGripperState).finallyDo(OperatorConstants.REEF_CHOOSER::switchReefSide) ); } - private static Command scoreFromGripperInL4Command() { + private static Command scoreFromGripperInL4Command(boolean shouldScoreRight) { return new SequentialCommandGroup( - GripperCommands.getPrepareForScoringInL4Command(CoralPlacingCommands::calculateTargetScoringPose).until(CoralPlacingCommands::canContinueScoringFromGripper), - GripperCommands.getScoreInL4Command(CoralPlacingCommands::calculateTargetScoringPose).finallyDo(OperatorConstants.REEF_CHOOSER::switchReefSide) + GripperCommands.getPrepareForScoringInL4Command(REEF_CHOOSER::calculateTargetScoringPose).until(() -> canContinueScoringFromGripper(shouldScoreRight)), + GripperCommands.getScoreInL4Command(REEF_CHOOSER::calculateTargetScoringPose).finallyDo(OperatorConstants.REEF_CHOOSER::switchReefSide) ); } - private static Command getOpenElevatorWhenCloseToReefCommand() { + private static Command getOpenElevatorWhenCloseToReefCommand(boolean shouldScoreRight) { return GeneralCommands.runWhen( ElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getElevatorState), - () -> calculateDistanceToTargetScoringPose() < PathPlannerConstants.MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR + () -> calculateDistanceToTargetScoringPose(shouldScoreRight) < PathPlannerConstants.MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR ); } - private static Command getAutonomousDriveToReefThenManualDriveCommand() { + private static Command getAutonomousDriveToReefThenManualDriveCommand(boolean shouldScoreRight) { return new SequentialCommandGroup( SwerveCommands.getDriveToPoseCommand( - CoralPlacingCommands::calculateTargetScoringPose, + () -> CoralPlacingCommands.calculateClosestScoringPose(shouldScoreRight), PathPlannerConstants.DRIVE_TO_REEF_CONSTRAINTS ), GeneralCommands.getFieldRelativeDriveCommand() ); } - public static FlippablePose2d calculateTargetScoringPose() { - if (OperatorConstants.RIGHT_MULTIFUNCTION_TRIGGER.getAsBoolean()) - return REEF_CHOOSER.calculateTargetScoringPose(); - return calculateClosestScoringPose(); - } - - private static double calculateDistanceToTargetScoringPose() { + private static double calculateDistanceToTargetScoringPose(boolean shouldScoreRight) { final Translation2d currentTranslation = RobotContainer.POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); - final Translation2d targetTranslation = calculateTargetScoringPose().get().getTranslation(); + final Translation2d targetTranslation = calculateClosestScoringPose(shouldScoreRight).get().getTranslation(); return currentTranslation.getDistance(targetTranslation); } - public static FlippablePose2d calculateClosestScoringPose() { + public static FlippablePose2d calculateClosestScoringPose(boolean shouldScoreRight) { final Translation2d robotPositionOnField = RobotContainer.POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); final Translation2d reefCenterPosition = new FlippableTranslation2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, true).get(); final Rotation2d[] reefClockAngles = FieldConstants.REEF_CLOCK_ANGLES; final Transform2d reefCenterToScoringPose = new Transform2d(FieldConstants.REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS, 0, new Rotation2d()), scoringPoseToRightBranch = new Transform2d(0, FieldConstants.REEF_CENTER_TO_TARGET_SCORING_POSITION_Y_TRANSFORM_METERS, new Rotation2d()); - final boolean shouldScoreOnRightBranch = REEF_CHOOSER.getReefSide().doesFlipYTransformWhenFacingDriverStation; double distanceFromClosestScoringPoseMeters = Double.POSITIVE_INFINITY; Pose2d closestScoringPose = new Pose2d(); @@ -166,7 +158,7 @@ public static FlippablePose2d calculateClosestScoringPose() { } } - return new FlippablePose2d(closestScoringPose.transformBy(shouldScoreOnRightBranch ? scoringPoseToRightBranch : scoringPoseToRightBranch.inverse()), false); + return new FlippablePose2d(closestScoringPose.transformBy(shouldScoreRight ? scoringPoseToRightBranch : scoringPoseToRightBranch.inverse()), false); } private static boolean canContinueScoringFromCoralIntake() { @@ -174,11 +166,10 @@ private static boolean canContinueScoringFromCoralIntake() { OperatorConstants.CONTINUE_TRIGGER.getAsBoolean(); } - private static boolean canContinueScoringFromGripper() { + private static boolean canContinueScoringFromGripper(boolean shouldScoreRight) { return RobotContainer.ELEVATOR.atTargetState() && RobotContainer.GRIPPER.atTargetAngle() && - OperatorConstants.CONTINUE_TRIGGER.getAsBoolean(); -// RobotContainer.SWERVE.atPose(calculateTargetScoringPose()); + (OperatorConstants.CONTINUE_TRIGGER.getAsBoolean() || RobotContainer.SWERVE.atPose(calculateClosestScoringPose(shouldScoreRight))); } /** diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index b0b41aec..4c9449d6 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -31,8 +31,6 @@ public class OperatorConstants { DRIVE_FROM_DPAD_TRIGGER = new Trigger(() -> DRIVER_CONTROLLER.getPov() != -1), TOGGLE_BRAKE_TRIGGER = OPERATOR_CONTROLLER.g().or(RobotController::getUserButton), DEBUGGING_TRIGGER = OPERATOR_CONTROLLER.f2(), - LEFT_MULTIFUNCTION_TRIGGER = DRIVER_CONTROLLER.leftStick().or(OPERATOR_CONTROLLER.b()), - RIGHT_MULTIFUNCTION_TRIGGER = DRIVER_CONTROLLER.rightStick().or(OPERATOR_CONTROLLER.m()), FORWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.right(), BACKWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.left(), FORWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.up(), @@ -43,7 +41,8 @@ public class OperatorConstants { FLOOR_CORAL_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftTrigger().or(OPERATOR_CONTROLLER.c()), FEEDER_CORAL_COLLECTION_TRIGGER = OPERATOR_CONTROLLER.f().or(DRIVER_CONTROLLER.start()), FEEDER_CORAL_COLLECTION_WITH_GRIPPER = OPERATOR_CONTROLLER.d(), - SCORE_CORAL_IN_REEF_TRIGGER = DRIVER_CONTROLLER.rightBumper().and(() -> !RobotContainer.ALGAE_MANIPULATOR.isOpen()).or(OPERATOR_CONTROLLER.q()), + LEFT_SCORE_TRIGGER = DRIVER_CONTROLLER.leftStick().or(OPERATOR_CONTROLLER.b()).and(() -> !RobotContainer.ALGAE_MANIPULATOR.isOpen()), + RIGHT_SCORE_TRIGGER = DRIVER_CONTROLLER.rightStick().or(OPERATOR_CONTROLLER.m()).and(() -> !RobotContainer.ALGAE_MANIPULATOR.isOpen()), EJECT_CORAL_TRIGGER = OPERATOR_CONTROLLER.e().or(() -> DRIVER_CONTROLLER.x().getAsBoolean() && !RobotContainer.ALGAE_MANIPULATOR.isOpen()), UNLOAD_CORAL_TRIGGER = OPERATOR_CONTROLLER.z().or(DRIVER_CONTROLLER.back()), COLLECT_ALGAE_FROM_REEF_TRIGGER = OPERATOR_CONTROLLER.a().or(DRIVER_CONTROLLER.a()), @@ -60,10 +59,10 @@ public class OperatorConstants { DISABLE_AUTONOMOUS_REEF_SCORING_TRIGGER = OPERATOR_CONTROLLER.i(); public static final Trigger SET_TARGET_SCORING_REEF_LEVEL_L1_FROM_GRIPPER_TRIGGER = OPERATOR_CONTROLLER.numpadDecimal(), - SET_TARGET_SCORING_REEF_LEVEL_L1_FROM_CORAL_INTAKE_TRIGGER = OPERATOR_CONTROLLER.numpad0(), - SET_TARGET_SCORING_REEF_LEVEL_L2_TRIGGER = OPERATOR_CONTROLLER.numpad1(), - SET_TARGET_SCORING_REEF_LEVEL_L3_TRIGGER = OPERATOR_CONTROLLER.numpad2(), - SET_TARGET_SCORING_REEF_LEVEL_L4_TRIGGER = OPERATOR_CONTROLLER.numpad3(), + SET_TARGET_SCORING_REEF_LEVEL_L1_FROM_CORAL_INTAKE_TRIGGER = OPERATOR_CONTROLLER.numpad0().or(DRIVER_CONTROLLER.povLeft()), + SET_TARGET_SCORING_REEF_LEVEL_L2_TRIGGER = OPERATOR_CONTROLLER.numpad1().or(DRIVER_CONTROLLER.povDown()), + SET_TARGET_SCORING_REEF_LEVEL_L3_TRIGGER = OPERATOR_CONTROLLER.numpad2().or(DRIVER_CONTROLLER.povRight()), + SET_TARGET_SCORING_REEF_LEVEL_L4_TRIGGER = OPERATOR_CONTROLLER.numpad3().or(DRIVER_CONTROLLER.povUp()), 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(), @@ -71,7 +70,5 @@ public class OperatorConstants { SET_TARGET_SCORING_REEF_CLOCK_POSITION_10_OCLOCK_TRIGGER = OPERATOR_CONTROLLER.numpad7(), SET_TARGET_SCORING_REEF_CLOCK_POSITION_12_OCLOCK_TRIGGER = OPERATOR_CONTROLLER.numpad8(), SET_TARGET_REEF_SCORING_SIDE_LEFT_TRIGGER = OPERATOR_CONTROLLER.left().or(DRIVER_CONTROLLER.povLeft()), - SET_TARGET_REEF_SCORING_SIDE_RIGHT_TRIGGER = OPERATOR_CONTROLLER.right().or(DRIVER_CONTROLLER.povRight()), - INCREMENT_TARGET_SCORING_LEVEL_TRIGGER = DRIVER_CONTROLLER.povUp(), - DECREMENT_TARGET_SCORING_LEVEL_TRIGGER = DRIVER_CONTROLLER.povDown(); + SET_TARGET_REEF_SCORING_SIDE_RIGHT_TRIGGER = OPERATOR_CONTROLLER.right().or(DRIVER_CONTROLLER.povRight()); } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/misc/ReefChooser.java b/src/main/java/frc/trigon/robot/misc/ReefChooser.java index cd7c5f14..fa5626f5 100644 --- a/src/main/java/frc/trigon/robot/misc/ReefChooser.java +++ b/src/main/java/frc/trigon/robot/misc/ReefChooser.java @@ -65,8 +65,6 @@ private void configureScoringLevelBindings() { OperatorConstants.SET_TARGET_SCORING_REEF_LEVEL_L2_TRIGGER.onTrue(getSetTargetLevelCommand(() -> CoralPlacingCommands.ScoringLevel.L2)); OperatorConstants.SET_TARGET_SCORING_REEF_LEVEL_L3_TRIGGER.onTrue(getSetTargetLevelCommand(() -> CoralPlacingCommands.ScoringLevel.L3)); OperatorConstants.SET_TARGET_SCORING_REEF_LEVEL_L4_TRIGGER.onTrue(getSetTargetLevelCommand(() -> CoralPlacingCommands.ScoringLevel.L4)); - OperatorConstants.INCREMENT_TARGET_SCORING_LEVEL_TRIGGER.onTrue(getSetTargetLevelCommand(this::getIncrementedScoringLevel)); - OperatorConstants.DECREMENT_TARGET_SCORING_LEVEL_TRIGGER.onTrue(getSetTargetLevelCommand(this::getDecrementedScoringLevel)); } private Command getSetTargetLevelCommand(Supplier scoringLevel) { From e203335c138886871a5a1f09a695ebdee2ced533 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sat, 31 May 2025 20:43:19 -0400 Subject: [PATCH 23/65] Huge button changes --- .../java/frc/trigon/robot/RobotContainer.java | 1 - .../AlgaeManipulationCommands.java | 38 ++++++++----------- .../CoralPlacingCommands.java | 5 +-- .../robot/constants/OperatorConstants.java | 19 ++++++---- .../robot/subsystems/gripper/Gripper.java | 4 +- 5 files changed, 32 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index ca56a88b..ca2965c5 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -14,7 +14,6 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.commands.commandclasses.CoralAlignmentCommand; -import frc.trigon.robot.commands.commandclasses.CoralAutoDriveCommand; import frc.trigon.robot.commands.commandclasses.LEDAutoSetupCommand; import frc.trigon.robot.commands.commandfactories.*; import frc.trigon.robot.constants.CameraConstants; diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java index 41604cd8..937f5347 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.CommandConstants; +import frc.trigon.robot.commands.commandclasses.WaitUntilChangeCommand; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.constants.PathPlannerConstants; @@ -106,34 +107,28 @@ private static Command getOpenElevatorForAlgaeCommand() { private static Command getScoreAlgaeCommand() { return new SelectCommand<>( Map.of( - 1, getQuickScoreInNetCommand(), - 2, getScoreInNetCommand(), - 3, getScoreInProcessorCommand() + 0, getHoldAlgaeCommand(), + 1, getScoreInNetCommand(), + 2, getScoreInProcessorCommand() ), AlgaeManipulationCommands::getAlgaeScoreMethodSelector - ); + ).raceWith(new WaitUntilChangeCommand<>(AlgaeManipulationCommands::isScoreAlgaeButtonPressed)).onlyIf(RobotContainer.GRIPPER::isMovingSlowly).repeatedly(); } private static int getAlgaeScoreMethodSelector() { - if (OperatorConstants.QUICK_SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean()) + if (OperatorConstants.SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean()) return 1; - else if (OperatorConstants.SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean()) - return 2; else if (OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean()) - return 3; + return 2; return 0; } - private static Command getQuickScoreInNetCommand() { + private static Command getHoldAlgaeCommand() { return new ParallelCommandGroup( - new WaitCommand(0.3).andThen(ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.QUICK_SCORE_NET)), - AlgaeManipulatorCommands.getOpenCommand(), - new SequentialCommandGroup( - GripperCommands.getSetTargetStateCommand(GripperConstants.GripperState.PREPARE_FOR_SCORING_ALGAE_IN_NET) - .until(RobotContainer.ELEVATOR::isOverQuickNetScoreReleaseHeight), - GripperCommands.getSetTargetStateCommand(GripperConstants.GripperState.QUICK_SCORE_ALGAE_IN_NET) - ) - ).until(() -> RobotContainer.GRIPPER.atState(GripperConstants.GripperState.QUICK_SCORE_ALGAE_IN_NET)); + GripperCommands.getSetTargetStateWhileHoldingAlgaeCommand(GripperConstants.GripperState.HOLD_ALGAE), + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST_WITH_ALGAE), + AlgaeManipulatorCommands.getOpenCommand() + ); } private static Command getScoreInNetCommand() { @@ -154,7 +149,7 @@ private static Command getScoreInNetCommand() { private static Command getScoreInProcessorCommand() { return new ParallelCommandGroup( - AlgaeManipulatorCommands.getOpenCommand().until(OperatorConstants.CONTINUE_TRIGGER), + AlgaeManipulatorCommands.getOpenCommand(), new SequentialCommandGroup( GripperCommands.getSetTargetStateWhileHoldingAlgaeCommand(GripperConstants.GripperState.PREPARE_FOR_SCORING_ALGAE_IN_PROCESSOR).until(OperatorConstants.CONTINUE_TRIGGER), GripperCommands.getSetTargetStateCommand(GripperConstants.GripperState.SCORE_ALGAE_IN_PROCESSOR) @@ -164,7 +159,7 @@ private static Command getScoreInProcessorCommand() { () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), () -> new FlippableRotation2d(Rotation2d.fromDegrees(90), true) ).asProxy() - ).until(() -> RobotContainer.GRIPPER.atState(GripperConstants.GripperState.SCORE_ALGAE_IN_PROCESSOR) && OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean()); + ).until(() -> RobotContainer.GRIPPER.atState(GripperConstants.GripperState.SCORE_ALGAE_IN_PROCESSOR) && !OperatorConstants.CONTINUE_TRIGGER.getAsBoolean()); } private static Command getAlignToReefCommand() { @@ -178,13 +173,12 @@ private static Command getAlignToReefCommand() { () -> 0, () -> calculateClosestAlgaeCollectionPose().getRotation() ) - ).until(OperatorConstants.CONTINUE_TRIGGER); + ).until(RobotContainer.GRIPPER::isMovingSlowly); } private static boolean isScoreAlgaeButtonPressed() { return OperatorConstants.SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean() || - OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean() || - OperatorConstants.QUICK_SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean(); + OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean(); } private static FlippablePose2d calculateClosestAlgaeCollectionPose() { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java index 6cd0b9f1..f0e6f48f 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -45,7 +45,7 @@ private static Command getScoreInReefFromCoralIntakeCommand() { return new SequentialCommandGroup( CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.GRIPPER::hasGamePiece), CoralIntakeCommands.getPrepareForStateCommand(CoralIntakeConstants.CoralIntakeState.SCORE_L1).until(CoralPlacingCommands::canContinueScoringFromCoralIntake), - CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.SCORE_L1_BOOST).withTimeout(0.07), + CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.SCORE_L1_BOOST).withTimeout(0.05), CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.SCORE_L1) ).until(() -> REEF_CHOOSER.getScoringLevel() != ScoringLevel.L1_CORAL_INTAKE); } @@ -162,8 +162,7 @@ public static FlippablePose2d calculateClosestScoringPose(boolean shouldScoreRig } private static boolean canContinueScoringFromCoralIntake() { - return RobotContainer.CORAL_INTAKE.atTargetAngle() && - OperatorConstants.CONTINUE_TRIGGER.getAsBoolean(); + return OperatorConstants.CONTINUE_TRIGGER.getAsBoolean(); } private static boolean canContinueScoringFromGripper(boolean shouldScoreRight) { diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 4c9449d6..3c53727c 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -1,6 +1,7 @@ package frc.trigon.robot.constants; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.misc.ReefChooser; @@ -20,6 +21,9 @@ public class OperatorConstants { ); public static final KeyboardController OPERATOR_CONTROLLER = new KeyboardController(); public static final ReefChooser REEF_CHOOSER = new ReefChooser(REEF_CHOOSER_PORT); + private static boolean + IS_LEFT_SCORE_BUTTON_PRESSED = false, + IS_RIGHT_SCORE_BUTTON_PRESSED = false; public static final double POV_DIVIDER = 2, @@ -35,23 +39,22 @@ public class OperatorConstants { BACKWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.left(), FORWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.up(), BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.down(), - CONTINUE_TRIGGER = OPERATOR_CONTROLLER.k().or(DRIVER_CONTROLLER.leftBumper()); + CONTINUE_TRIGGER = OPERATOR_CONTROLLER.k().or(DRIVER_CONTROLLER.rightStick().and(DRIVER_CONTROLLER.leftStick())); public static final Trigger RESET_AMP_ALIGNER_TRIGGER = OPERATOR_CONTROLLER.v(), FLOOR_CORAL_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftTrigger().or(OPERATOR_CONTROLLER.c()), FEEDER_CORAL_COLLECTION_TRIGGER = OPERATOR_CONTROLLER.f().or(DRIVER_CONTROLLER.start()), FEEDER_CORAL_COLLECTION_WITH_GRIPPER = OPERATOR_CONTROLLER.d(), - LEFT_SCORE_TRIGGER = DRIVER_CONTROLLER.leftStick().or(OPERATOR_CONTROLLER.b()).and(() -> !RobotContainer.ALGAE_MANIPULATOR.isOpen()), - RIGHT_SCORE_TRIGGER = DRIVER_CONTROLLER.rightStick().or(OPERATOR_CONTROLLER.m()).and(() -> !RobotContainer.ALGAE_MANIPULATOR.isOpen()), + LEFT_SCORE_TRIGGER = DRIVER_CONTROLLER.leftStick().or(OPERATOR_CONTROLLER.b()).and(() -> !RobotContainer.ALGAE_MANIPULATOR.isOpen()).and(() -> !IS_RIGHT_SCORE_BUTTON_PRESSED).onTrue(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = true)).onFalse(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = false)), + RIGHT_SCORE_TRIGGER = DRIVER_CONTROLLER.rightStick().or(OPERATOR_CONTROLLER.m()).and(() -> !RobotContainer.ALGAE_MANIPULATOR.isOpen()).and(() -> !IS_LEFT_SCORE_BUTTON_PRESSED).onTrue(new InstantCommand(() -> IS_RIGHT_SCORE_BUTTON_PRESSED = true)).onFalse(new InstantCommand(() -> IS_RIGHT_SCORE_BUTTON_PRESSED = false)), EJECT_CORAL_TRIGGER = OPERATOR_CONTROLLER.e().or(() -> DRIVER_CONTROLLER.x().getAsBoolean() && !RobotContainer.ALGAE_MANIPULATOR.isOpen()), UNLOAD_CORAL_TRIGGER = OPERATOR_CONTROLLER.z().or(DRIVER_CONTROLLER.back()), COLLECT_ALGAE_FROM_REEF_TRIGGER = OPERATOR_CONTROLLER.a().or(DRIVER_CONTROLLER.a()), COLLECT_ALGAE_FROM_LOLLIPOP_TRIGGER = OPERATOR_CONTROLLER.l().or(DRIVER_CONTROLLER.y()), COLLECT_ALGAE_FROM_FLOOR_TRIGGER = OPERATOR_CONTROLLER.s(), ROLL_ALGAE_ON_FLOOR_TRIGGER = OPERATOR_CONTROLLER.w(), - QUICK_SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.x().or(DRIVER_CONTROLLER.rightBumper().and(RobotContainer.ALGAE_MANIPULATOR::isOpen)), - SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.n().or(DRIVER_CONTROLLER.b()), - SCORE_ALGAE_IN_PROCESSOR_TRIGGER = OPERATOR_CONTROLLER.j().or(() -> DRIVER_CONTROLLER.x().getAsBoolean() && RobotContainer.ALGAE_MANIPULATOR.isOpen()); + SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.n().or(DRIVER_CONTROLLER.rightStick()).and(() -> !IS_LEFT_SCORE_BUTTON_PRESSED).onTrue(new InstantCommand(() -> IS_RIGHT_SCORE_BUTTON_PRESSED = true)).onFalse(new InstantCommand(() -> IS_RIGHT_SCORE_BUTTON_PRESSED = false)), + SCORE_ALGAE_IN_PROCESSOR_TRIGGER = OPERATOR_CONTROLLER.j().or(() -> DRIVER_CONTROLLER.leftStick().getAsBoolean() && RobotContainer.ALGAE_MANIPULATOR.isOpen()).and(() -> !IS_RIGHT_SCORE_BUTTON_PRESSED).onTrue(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = true)).onFalse(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = false)); public static final Trigger ENABLE_IGNORE_LOLLIPOP_CORAL_TRIGGER = OPERATOR_CONTROLLER.o(), DISABLE_IGNORE_LOLLIPOP_CORAL_TRIGGER = OPERATOR_CONTROLLER.p(), @@ -69,6 +72,6 @@ public class OperatorConstants { SET_TARGET_SCORING_REEF_CLOCK_POSITION_8_OCLOCK_TRIGGER = OPERATOR_CONTROLLER.numpad4(), SET_TARGET_SCORING_REEF_CLOCK_POSITION_10_OCLOCK_TRIGGER = OPERATOR_CONTROLLER.numpad7(), SET_TARGET_SCORING_REEF_CLOCK_POSITION_12_OCLOCK_TRIGGER = OPERATOR_CONTROLLER.numpad8(), - SET_TARGET_REEF_SCORING_SIDE_LEFT_TRIGGER = OPERATOR_CONTROLLER.left().or(DRIVER_CONTROLLER.povLeft()), - SET_TARGET_REEF_SCORING_SIDE_RIGHT_TRIGGER = OPERATOR_CONTROLLER.right().or(DRIVER_CONTROLLER.povRight()); + SET_TARGET_REEF_SCORING_SIDE_LEFT_TRIGGER = OPERATOR_CONTROLLER.left(), + SET_TARGET_REEF_SCORING_SIDE_RIGHT_TRIGGER = OPERATOR_CONTROLLER.right(); } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/gripper/Gripper.java b/src/main/java/frc/trigon/robot/subsystems/gripper/Gripper.java index e403198d..c2a8ba65 100644 --- a/src/main/java/frc/trigon/robot/subsystems/gripper/Gripper.java +++ b/src/main/java/frc/trigon/robot/subsystems/gripper/Gripper.java @@ -8,9 +8,11 @@ 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 org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.misc.simplesensor.SimpleSensor; import org.trigon.hardware.phoenix6.cancoder.CANcoderEncoder; import org.trigon.hardware.phoenix6.cancoder.CANcoderSignal; @@ -151,7 +153,7 @@ public Translation3d getRobotRelativeExitVelocity() { @AutoLogOutput public boolean isMovingSlowly() { - return Math.abs(grippingMotor.getSignal(TalonFXSignal.VELOCITY)) < 4; + return Math.abs(grippingMotor.getSignal(TalonFXSignal.VELOCITY)) < 4 || (RobotHardwareStats.isSimulation() && SimulationFieldHandler.isHoldingAlgae()); } public Pose3d calculateAlgaeCollectionPose() { From 5eff4d469acd43647e2d7e6753bd6af2a82e9c6e Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sat, 31 May 2025 21:31:30 -0400 Subject: [PATCH 24/65] Fix to stuff but sim is bad (just bad) --- .../AlgaeManipulationCommands.java | 19 +++++++++++++------ .../CoralCollectionCommands.java | 2 +- .../robot/constants/OperatorConstants.java | 1 + 3 files changed, 15 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java index 937f5347..1e2a159f 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -35,7 +35,7 @@ public static Command getCollectAlgaeFromLollipopCommand() { getGripAlgaeCommand(GripperConstants.GripperState.COLLECT_ALGAE_FROM_LOLLIPOP).asProxy() .until(AlgaeManipulationCommands::isScoreAlgaeButtonPressed), getScoreAlgaeCommand().asProxy() - ).raceWith(getScoreNetEndingConditionCommand()).andThen(AlgaeManipulationCommands::reloadAfterScore); + ).raceWith(getScoreNetEndingConditionCommand(), getScoreProcessorEndingConditionCommand()).andThen(AlgaeManipulationCommands::reloadAfterScore); } public static Command getCollectAlgaeFromReefCommand() { @@ -44,7 +44,7 @@ public static Command getCollectAlgaeFromReefCommand() { getCollectAlgaeFromReefManuallyCommand().asProxy() ) .alongWith(getAlignToReefCommand().onlyIf(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY).asProxy()) - .raceWith(getScoreNetEndingConditionCommand()).andThen(AlgaeManipulationCommands::reloadAfterScore); + .raceWith(getScoreNetEndingConditionCommand(), getScoreProcessorEndingConditionCommand()).andThen(AlgaeManipulationCommands::reloadAfterScore); } public static Command getCollectAlgaeFromFloorCommand() { @@ -78,11 +78,15 @@ private static Command getScoreNetEndingConditionCommand() { return new WaitUntilCommand(() -> RobotContainer.GRIPPER.atState(GripperConstants.GripperState.SCORE_ALGAE_IN_NET) || RobotContainer.GRIPPER.atState(GripperConstants.GripperState.QUICK_SCORE_ALGAE_IN_NET)).andThen(new WaitCommand(0.1)); } + private static Command getScoreProcessorEndingConditionCommand() { + return new WaitUntilCommand(() -> RobotContainer.GRIPPER.atState(GripperConstants.GripperState.SCORE_ALGAE_IN_PROCESSOR) && !OperatorConstants.CONTINUE_TRIGGER.getAsBoolean()); + } + private static Command getCollectAlgaeFromReefManuallyCommand() { return new ParallelCommandGroup( getGripAlgaeCommand(GripperConstants.GripperState.COLLECT_ALGAE_FROM_REEF), getOpenElevatorForAlgaeCommand() - ).until(AlgaeManipulationCommands::isScoreAlgaeButtonPressed).andThen( + ).until(() -> isScoreAlgaeButtonPressed() && RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.REST_WITH_ALGAE) && RobotContainer.GRIPPER.atState(GripperConstants.GripperState.HOLD_ALGAE)).andThen( getScoreAlgaeCommand().asProxy() ); } @@ -100,7 +104,7 @@ private static Command getOpenElevatorForAlgaeCommand() { return GeneralCommands.getContinuousConditionalCommand( ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.COLLECT_ALGAE_FROM_L3), ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST_WITH_ALGAE), - OperatorConstants.LEFT_SCORE_TRIGGER + OperatorConstants.COLLECT_ALGAE_FROM_L3_OVERRIDE_TRIGGER ); } @@ -112,7 +116,7 @@ private static Command getScoreAlgaeCommand() { 2, getScoreInProcessorCommand() ), AlgaeManipulationCommands::getAlgaeScoreMethodSelector - ).raceWith(new WaitUntilChangeCommand<>(AlgaeManipulationCommands::isScoreAlgaeButtonPressed)).onlyIf(RobotContainer.GRIPPER::isMovingSlowly).repeatedly(); + ).raceWith(new WaitUntilChangeCommand<>(AlgaeManipulationCommands::isScoreAlgaeButtonPressed)).repeatedly(); } private static int getAlgaeScoreMethodSelector() { @@ -173,7 +177,10 @@ private static Command getAlignToReefCommand() { () -> 0, () -> calculateClosestAlgaeCollectionPose().getRotation() ) - ).until(RobotContainer.GRIPPER::isMovingSlowly); + ).raceWith( + new WaitCommand(1).andThen(new WaitUntilCommand(RobotContainer.GRIPPER::isMovingSlowly)), + new WaitUntilCommand(OperatorConstants.CONTINUE_TRIGGER) + ); } private static boolean isScoreAlgaeButtonPressed() { 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 44836eb6..58ecac3b 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -86,7 +86,7 @@ public static Command getUnloadCoralCommand() { } private static boolean shouldStopLoadingCoral() { - return RobotContainer.GRIPPER.hasGamePiece() || OperatorConstants.CONTINUE_TRIGGER.getAsBoolean(); + return RobotContainer.GRIPPER.hasGamePiece(); } private static Command getCoralIntakeLoadingSequenceCommand() { diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 3c53727c..40c876dc 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -50,6 +50,7 @@ public class OperatorConstants { EJECT_CORAL_TRIGGER = OPERATOR_CONTROLLER.e().or(() -> DRIVER_CONTROLLER.x().getAsBoolean() && !RobotContainer.ALGAE_MANIPULATOR.isOpen()), UNLOAD_CORAL_TRIGGER = OPERATOR_CONTROLLER.z().or(DRIVER_CONTROLLER.back()), COLLECT_ALGAE_FROM_REEF_TRIGGER = OPERATOR_CONTROLLER.a().or(DRIVER_CONTROLLER.a()), + COLLECT_ALGAE_FROM_L3_OVERRIDE_TRIGGER = DRIVER_CONTROLLER.leftStick().or(DRIVER_CONTROLLER.rightStick()), COLLECT_ALGAE_FROM_LOLLIPOP_TRIGGER = OPERATOR_CONTROLLER.l().or(DRIVER_CONTROLLER.y()), COLLECT_ALGAE_FROM_FLOOR_TRIGGER = OPERATOR_CONTROLLER.s(), ROLL_ALGAE_ON_FLOOR_TRIGGER = OPERATOR_CONTROLLER.w(), From a19f28f51aedfc4f934c5197304b240c168c9f2b Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 1 Jun 2025 07:55:50 -0400 Subject: [PATCH 25/65] Auto feeder intake selection --- .../java/frc/trigon/robot/RobotContainer.java | 1 - .../CoralCollectionCommands.java | 24 ++++++++++++++++++- .../robot/constants/FieldConstants.java | 2 ++ .../robot/constants/OperatorConstants.java | 1 - 4 files changed, 25 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index ca2965c5..b1ebbe65 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -104,7 +104,6 @@ private void bindControllerCommands() { OperatorConstants.FLOOR_CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getFloorCoralCollectionCommand()); // OperatorConstants.FLOOR_CORAL_COLLECTION_TRIGGER.and(OperatorConstants.LEFT_MULTIFUNCTION_TRIGGER).whileTrue(new CoralAutoDriveCommand()); OperatorConstants.FEEDER_CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getFeederCoralCollectionCommand()); - OperatorConstants.FEEDER_CORAL_COLLECTION_WITH_GRIPPER.whileTrue(CoralCollectionCommands.getFeederCoralCollectionFromGripperCommand()); OperatorConstants.RIGHT_SCORE_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(true)); OperatorConstants.LEFT_SCORE_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(false)); OperatorConstants.EJECT_CORAL_TRIGGER.whileTrue(EjectionCommands.getEjectCoralCommand()); 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 58ecac3b..ecbbbc4f 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -1,7 +1,10 @@ package frc.trigon.robot.commands.commandfactories; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.coralintake.CoralIntakeCommands; import frc.trigon.robot.subsystems.coralintake.CoralIntakeConstants; @@ -9,6 +12,8 @@ import frc.trigon.robot.subsystems.elevator.ElevatorConstants; import frc.trigon.robot.subsystems.gripper.GripperCommands; import frc.trigon.robot.subsystems.gripper.GripperConstants; +import org.trigon.utilities.flippable.FlippablePose2d; +import org.trigon.utilities.flippable.FlippableRotation2d; public class CoralCollectionCommands { @@ -27,7 +32,24 @@ public static Command getFloorCoralCollectionCommand() { } public static Command getFeederCoralCollectionCommand() { - return getInitiateFeederCoralCollectionCommand().unless(RobotContainer.GRIPPER::hasGamePiece); + return new ConditionalCommand( + getInitiateFeederCoralCollectionCommand().unless(RobotContainer.GRIPPER::hasGamePiece), + getFeederCoralCollectionFromGripperCommand(), + CoralCollectionCommands::isIntakeFacingFeeder + ).alongWith(new InstantCommand(() -> { + System.out.println(isIntakeFacingFeeder()); + }).repeatedly()); + } + + private static boolean isIntakeFacingFeeder() { + final Pose2d robotPose = new FlippablePose2d(RobotContainer.POSE_ESTIMATOR.getEstimatedRobotPose(), true).get(); + final Rotation2d + robotHeading = robotPose.getRotation(), + leftFeederAngle = FieldConstants.LEFT_FEEDER_ANGLE; + + if (robotPose.getY() > FieldConstants.FIELD_WIDTH_METERS / 2) + return robotHeading.plus(leftFeederAngle).plus(Rotation2d.kCW_90deg).getDegrees() > 0; + return robotHeading.minus(leftFeederAngle).plus(Rotation2d.kCCW_90deg).getDegrees() < 0; } private static Command getInitiateFeederCoralCollectionCommand() { diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 3e911a4d..fcdaaf5b 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -29,6 +29,8 @@ public class FieldConstants { public static final FlippablePose2d AUTO_FIND_CORAL_POSE_LEFT = new FlippablePose2d(3.3, 5.5, Rotation2d.fromDegrees(130), true); public static final FlippablePose2d AUTO_FIND_CORAL_POSE_RIGHT = new FlippablePose2d(3.3, FieldConstants.FIELD_WIDTH_METERS - 5.5, Rotation2d.fromDegrees(-130), true); + public static final Rotation2d LEFT_FEEDER_ANGLE = Rotation2d.fromDegrees(54); + 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(); diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 40c876dc..105f154f 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -44,7 +44,6 @@ public class OperatorConstants { RESET_AMP_ALIGNER_TRIGGER = OPERATOR_CONTROLLER.v(), FLOOR_CORAL_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftTrigger().or(OPERATOR_CONTROLLER.c()), FEEDER_CORAL_COLLECTION_TRIGGER = OPERATOR_CONTROLLER.f().or(DRIVER_CONTROLLER.start()), - FEEDER_CORAL_COLLECTION_WITH_GRIPPER = OPERATOR_CONTROLLER.d(), LEFT_SCORE_TRIGGER = DRIVER_CONTROLLER.leftStick().or(OPERATOR_CONTROLLER.b()).and(() -> !RobotContainer.ALGAE_MANIPULATOR.isOpen()).and(() -> !IS_RIGHT_SCORE_BUTTON_PRESSED).onTrue(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = true)).onFalse(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = false)), RIGHT_SCORE_TRIGGER = DRIVER_CONTROLLER.rightStick().or(OPERATOR_CONTROLLER.m()).and(() -> !RobotContainer.ALGAE_MANIPULATOR.isOpen()).and(() -> !IS_LEFT_SCORE_BUTTON_PRESSED).onTrue(new InstantCommand(() -> IS_RIGHT_SCORE_BUTTON_PRESSED = true)).onFalse(new InstantCommand(() -> IS_RIGHT_SCORE_BUTTON_PRESSED = false)), EJECT_CORAL_TRIGGER = OPERATOR_CONTROLLER.e().or(() -> DRIVER_CONTROLLER.x().getAsBoolean() && !RobotContainer.ALGAE_MANIPULATOR.isOpen()), From 1ce5502487a70b2c9f1b4a469390321e427d74b2 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 1 Jun 2025 07:56:56 -0400 Subject: [PATCH 26/65] Unnecessary --- src/main/java/frc/trigon/robot/constants/OperatorConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 105f154f..a89fc5b0 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -46,7 +46,7 @@ public class OperatorConstants { FEEDER_CORAL_COLLECTION_TRIGGER = OPERATOR_CONTROLLER.f().or(DRIVER_CONTROLLER.start()), LEFT_SCORE_TRIGGER = DRIVER_CONTROLLER.leftStick().or(OPERATOR_CONTROLLER.b()).and(() -> !RobotContainer.ALGAE_MANIPULATOR.isOpen()).and(() -> !IS_RIGHT_SCORE_BUTTON_PRESSED).onTrue(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = true)).onFalse(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = false)), RIGHT_SCORE_TRIGGER = DRIVER_CONTROLLER.rightStick().or(OPERATOR_CONTROLLER.m()).and(() -> !RobotContainer.ALGAE_MANIPULATOR.isOpen()).and(() -> !IS_LEFT_SCORE_BUTTON_PRESSED).onTrue(new InstantCommand(() -> IS_RIGHT_SCORE_BUTTON_PRESSED = true)).onFalse(new InstantCommand(() -> IS_RIGHT_SCORE_BUTTON_PRESSED = false)), - EJECT_CORAL_TRIGGER = OPERATOR_CONTROLLER.e().or(() -> DRIVER_CONTROLLER.x().getAsBoolean() && !RobotContainer.ALGAE_MANIPULATOR.isOpen()), + EJECT_CORAL_TRIGGER = OPERATOR_CONTROLLER.e().or(DRIVER_CONTROLLER.x()), UNLOAD_CORAL_TRIGGER = OPERATOR_CONTROLLER.z().or(DRIVER_CONTROLLER.back()), COLLECT_ALGAE_FROM_REEF_TRIGGER = OPERATOR_CONTROLLER.a().or(DRIVER_CONTROLLER.a()), COLLECT_ALGAE_FROM_L3_OVERRIDE_TRIGGER = DRIVER_CONTROLLER.leftStick().or(DRIVER_CONTROLLER.rightStick()), From 1c7d5e76aee15ac25773357992369a43a0dbc203 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 1 Jun 2025 07:59:10 -0400 Subject: [PATCH 27/65] Oops --- .../commands/commandfactories/CoralCollectionCommands.java | 4 +--- 1 file changed, 1 insertion(+), 3 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 ecbbbc4f..79838d1b 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -36,9 +36,7 @@ public static Command getFeederCoralCollectionCommand() { getInitiateFeederCoralCollectionCommand().unless(RobotContainer.GRIPPER::hasGamePiece), getFeederCoralCollectionFromGripperCommand(), CoralCollectionCommands::isIntakeFacingFeeder - ).alongWith(new InstantCommand(() -> { - System.out.println(isIntakeFacingFeeder()); - }).repeatedly()); + ); } private static boolean isIntakeFacingFeeder() { From ff386fe2419d46994f02b715736e1e0fdff99500 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 2 Jun 2025 14:03:38 -0400 Subject: [PATCH 28/65] Even better --- .../java/frc/trigon/robot/constants/OperatorConstants.java | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index a89fc5b0..ab361b37 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -31,8 +31,7 @@ public class OperatorConstants { public static final Trigger LED_AUTO_SETUP_TRIGGER = OPERATOR_CONTROLLER.backtick(), - RESET_HEADING_TRIGGER = OPERATOR_CONTROLLER.r(), - DRIVE_FROM_DPAD_TRIGGER = new Trigger(() -> DRIVER_CONTROLLER.getPov() != -1), + RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.leftBumper().and(DRIVER_CONTROLLER.rightBumper()), TOGGLE_BRAKE_TRIGGER = OPERATOR_CONTROLLER.g().or(RobotController::getUserButton), DEBUGGING_TRIGGER = OPERATOR_CONTROLLER.f2(), FORWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.right(), @@ -48,9 +47,9 @@ public class OperatorConstants { RIGHT_SCORE_TRIGGER = DRIVER_CONTROLLER.rightStick().or(OPERATOR_CONTROLLER.m()).and(() -> !RobotContainer.ALGAE_MANIPULATOR.isOpen()).and(() -> !IS_LEFT_SCORE_BUTTON_PRESSED).onTrue(new InstantCommand(() -> IS_RIGHT_SCORE_BUTTON_PRESSED = true)).onFalse(new InstantCommand(() -> IS_RIGHT_SCORE_BUTTON_PRESSED = false)), EJECT_CORAL_TRIGGER = OPERATOR_CONTROLLER.e().or(DRIVER_CONTROLLER.x()), UNLOAD_CORAL_TRIGGER = OPERATOR_CONTROLLER.z().or(DRIVER_CONTROLLER.back()), - COLLECT_ALGAE_FROM_REEF_TRIGGER = OPERATOR_CONTROLLER.a().or(DRIVER_CONTROLLER.a()), + COLLECT_ALGAE_FROM_REEF_TRIGGER = OPERATOR_CONTROLLER.a().or(DRIVER_CONTROLLER.rightBumper()), COLLECT_ALGAE_FROM_L3_OVERRIDE_TRIGGER = DRIVER_CONTROLLER.leftStick().or(DRIVER_CONTROLLER.rightStick()), - COLLECT_ALGAE_FROM_LOLLIPOP_TRIGGER = OPERATOR_CONTROLLER.l().or(DRIVER_CONTROLLER.y()), + COLLECT_ALGAE_FROM_LOLLIPOP_TRIGGER = OPERATOR_CONTROLLER.l().or(DRIVER_CONTROLLER.leftBumper()), COLLECT_ALGAE_FROM_FLOOR_TRIGGER = OPERATOR_CONTROLLER.s(), ROLL_ALGAE_ON_FLOOR_TRIGGER = OPERATOR_CONTROLLER.w(), SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.n().or(DRIVER_CONTROLLER.rightStick()).and(() -> !IS_LEFT_SCORE_BUTTON_PRESSED).onTrue(new InstantCommand(() -> IS_RIGHT_SCORE_BUTTON_PRESSED = true)).onFalse(new InstantCommand(() -> IS_RIGHT_SCORE_BUTTON_PRESSED = false)), From c82d2dc58488710a285fb5924f574a1bab34917f Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 3 Jun 2025 09:45:01 -0400 Subject: [PATCH 29/65] e --- src/main/java/frc/trigon/robot/constants/OperatorConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index ab361b37..e9be3c27 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -31,7 +31,7 @@ public class OperatorConstants { public static final Trigger LED_AUTO_SETUP_TRIGGER = OPERATOR_CONTROLLER.backtick(), - RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.leftBumper().and(DRIVER_CONTROLLER.rightBumper()), + RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.y(), TOGGLE_BRAKE_TRIGGER = OPERATOR_CONTROLLER.g().or(RobotController::getUserButton), DEBUGGING_TRIGGER = OPERATOR_CONTROLLER.f2(), FORWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.right(), From 9b9c5a3d9b74d07e8b59a7c950174c33616ae33c Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 4 Jun 2025 05:07:33 -0400 Subject: [PATCH 30/65] Better --- .../commands/commandfactories/AlgaeManipulationCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java index 1e2a159f..d34b2d0d 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -86,7 +86,7 @@ private static Command getCollectAlgaeFromReefManuallyCommand() { return new ParallelCommandGroup( getGripAlgaeCommand(GripperConstants.GripperState.COLLECT_ALGAE_FROM_REEF), getOpenElevatorForAlgaeCommand() - ).until(() -> isScoreAlgaeButtonPressed() && RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.REST_WITH_ALGAE) && RobotContainer.GRIPPER.atState(GripperConstants.GripperState.HOLD_ALGAE)).andThen( + ).until(() -> isScoreAlgaeButtonPressed() && RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.REST_WITH_ALGAE) && RobotContainer.GRIPPER.atState(GripperConstants.GripperState.HOLD_ALGAE) && RobotContainer.GRIPPER.isMovingSlowly()).andThen( getScoreAlgaeCommand().asProxy() ); } From d833620d680e03b1908be5f400038f9feadf32d3 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 22 Jun 2025 20:12:08 +0300 Subject: [PATCH 31/65] Only when it sees a tag --- .../CoralPlacingCommands.java | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java index f0e6f48f..0cf60287 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -63,9 +63,9 @@ private static Command getAutonomouslyScoreInReefFromGripperCommand(boolean shou return new ParallelRaceGroup( CoralCollectionCommands.getLoadCoralCommand().asProxy().andThen( new ParallelCommandGroup( - getOpenElevatorWhenCloseToReefCommand(shouldScoreRight).raceWith(new WaitUntilChangeCommand<>(REEF_CHOOSER::getElevatorState)).repeatedly(), + getOpenElevatorWhenCloseToReefCommand(shouldScoreRight), getAutoGripperScoringSequenceCommand(shouldScoreRight) - ).asProxy() + ).asProxy().raceWith(new WaitUntilChangeCommand<>(REEF_CHOOSER::getElevatorState)).repeatedly() ), getAutonomousDriveToReefThenManualDriveCommand(shouldScoreRight) ).andThen(); @@ -95,7 +95,12 @@ private static Command getAutoGripperScoringSequenceCommand(boolean shouldScoreR private static Command scoreFromGripperReefChooserCommand(boolean shouldScoreRight) { return new SequentialCommandGroup( - GripperCommands.getPrepareForStateCommand(REEF_CHOOSER::getGripperState).until(() -> canContinueScoringFromGripper(shouldScoreRight)), + GripperCommands.getPrepareForStateCommand(REEF_CHOOSER::getGripperState).raceWith( + new SequentialCommandGroup( + new WaitUntilCommand(() -> canAutonomouslyReleaseFromGripper(shouldScoreRight)), + new WaitUntilChangeCommand<>(RobotContainer.POSE_ESTIMATOR::getEstimatedRobotPose) + ) + ).until(OperatorConstants.CONTINUE_TRIGGER), GripperCommands.getSetTargetStateCommand(REEF_CHOOSER::getGripperState).finallyDo(OperatorConstants.REEF_CHOOSER::switchReefSide) ); } @@ -109,7 +114,12 @@ private static Command scoreFromGripperReefChooserCommand() { private static Command scoreFromGripperInL4Command(boolean shouldScoreRight) { return new SequentialCommandGroup( - GripperCommands.getPrepareForScoringInL4Command(REEF_CHOOSER::calculateTargetScoringPose).until(() -> canContinueScoringFromGripper(shouldScoreRight)), + GripperCommands.getPrepareForScoringInL4Command(REEF_CHOOSER::calculateTargetScoringPose).raceWith( + new SequentialCommandGroup( + new WaitUntilCommand(() -> canAutonomouslyReleaseFromGripper(shouldScoreRight)), + new WaitUntilChangeCommand<>(RobotContainer.POSE_ESTIMATOR::getEstimatedRobotPose) + ) + ).until(OperatorConstants.CONTINUE_TRIGGER), GripperCommands.getScoreInL4Command(REEF_CHOOSER::calculateTargetScoringPose).finallyDo(OperatorConstants.REEF_CHOOSER::switchReefSide) ); } @@ -165,10 +175,10 @@ private static boolean canContinueScoringFromCoralIntake() { return OperatorConstants.CONTINUE_TRIGGER.getAsBoolean(); } - private static boolean canContinueScoringFromGripper(boolean shouldScoreRight) { + private static boolean canAutonomouslyReleaseFromGripper(boolean shouldScoreRight) { return RobotContainer.ELEVATOR.atTargetState() && RobotContainer.GRIPPER.atTargetAngle() && - (OperatorConstants.CONTINUE_TRIGGER.getAsBoolean() || RobotContainer.SWERVE.atPose(calculateClosestScoringPose(shouldScoreRight))); + RobotContainer.SWERVE.atPose(calculateClosestScoringPose(shouldScoreRight)); } /** From 2b6bb8fc52e9b9c728ae52d4cb4859783bd06add Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 22 Jun 2025 20:43:31 +0300 Subject: [PATCH 32/65] EXTRA NEWLINE!??!?!??!?!?!?! --- .../apriltagcamera/io/AprilTagSimulationCameraIO.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java index c68ef0e0..4bc24b30 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java @@ -5,7 +5,6 @@ import org.photonvision.simulation.PhotonCameraSim; public class AprilTagSimulationCameraIO extends AprilTagPhotonCameraIO { - public AprilTagSimulationCameraIO(String cameraName, Transform3d robotToCamera) { super(cameraName, robotToCamera); From 8a39610005de2e4c089425168372c4ba01e5c5a5 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 24 Jun 2025 22:53:01 +0300 Subject: [PATCH 33/65] Quick cleaning? --- src/main/java/frc/trigon/robot/Robot.java | 2 +- .../java/frc/trigon/robot/RobotContainer.java | 3 +- .../robot/commands/CommandConstants.java | 13 ++-- .../robot/constants/CameraConstants.java | 12 +--- .../robot/constants/FieldConstants.java | 9 ++- .../robot/constants/OperatorConstants.java | 20 ++++-- .../robot/constants/PathPlannerConstants.java | 6 -- .../io/PhotonObjectDetectionCameraIO.java | 67 ------------------- .../apriltagcamera/AprilTagCamera.java | 4 +- .../AprilTagCameraConstants.java | 4 +- .../io/AprilTagPhotonCameraIO.java | 24 +++---- .../poseestimator/PoseEstimator.java | 2 +- .../robot/subsystems/MotorSubsystem.java | 4 +- .../elevator/ElevatorConstants.java | 1 - .../robot/subsystems/swerve/Swerve.java | 2 +- 15 files changed, 49 insertions(+), 124 deletions(-) diff --git a/src/main/java/frc/trigon/robot/Robot.java b/src/main/java/frc/trigon/robot/Robot.java index 7d3dc65c..0cd91d2a 100644 --- a/src/main/java/frc/trigon/robot/Robot.java +++ b/src/main/java/frc/trigon/robot/Robot.java @@ -65,7 +65,7 @@ public void testInit() { @Override public void simulationPeriodic() { - if (RobotHardwareStats.isSimulation()) + if (!RobotHardwareStats.isReplay()) AprilTagCameraConstants.VISION_SIMULATION.update(RobotContainer.POSE_ESTIMATOR.getEstimatedOdometryPose()); SimulationFieldHandler.update(); } diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index b1ebbe65..fe28a0d3 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -101,8 +101,7 @@ private void bindControllerCommands() { OperatorConstants.RESET_AMP_ALIGNER_TRIGGER.whileTrue(AlgaeManipulationCommands.getResetAmpAlignerCommand()); - OperatorConstants.FLOOR_CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getFloorCoralCollectionCommand()); -// OperatorConstants.FLOOR_CORAL_COLLECTION_TRIGGER.and(OperatorConstants.LEFT_MULTIFUNCTION_TRIGGER).whileTrue(new CoralAutoDriveCommand()); + OperatorConstants.FLOOR_CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getFloorCoralCollectionCommand());//TODO: Add collection assist OperatorConstants.FEEDER_CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getFeederCoralCollectionCommand()); OperatorConstants.RIGHT_SCORE_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(true)); OperatorConstants.LEFT_SCORE_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(false)); diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index be7898b8..8c9bfc8c 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -42,12 +42,11 @@ public class CommandConstants { () -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX()) ), RESET_HEADING_COMMAND = new InstantCommand(RobotContainer.POSE_ESTIMATOR::resetHeading), - // RESET_HEADING_COMMAND = new InstantCommand(() -> RobotContainer.POSE_ESTIMATOR.resetPose(RobotContainer.POSE_ESTIMATOR.getEstimatedRobotPose())), - SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND = SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> getXPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), - () -> getYPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), - () -> 0 - ), + SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND = SwerveCommands.getClosedLoopSelfRelativeDriveCommand( + () -> getXPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), + () -> getYPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), + () -> 0 + ), WHEEL_RADIUS_CHARACTERIZATION_COMMAND = new WheelRadiusCharacterizationCommand( PathPlannerConstants.ROBOT_CONFIG.moduleLocations, RobotContainer.SWERVE::getDriveWheelPositionsRadians, @@ -57,7 +56,7 @@ public class CommandConstants { ), CALCULATE_CAMERA_POSITION_COMMAND = new CameraPositionCalculationCommand( RobotContainer.POSE_ESTIMATOR::getEstimatedRobotPose, - Rotation2d.fromDegrees(200), + Rotation2d.fromDegrees(0), (omegaRadiansPerSecond) -> RobotContainer.SWERVE.selfRelativeDriveWithoutSetpointGeneration(new ChassisSpeeds(0, 0, omegaRadiansPerSecond), null), RobotContainer.SWERVE ); diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index a75046b3..cddadafe 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -14,11 +14,7 @@ public class CameraConstants { REEF_TAG_CAMERA_STANDARD_DEVIATIONS = new StandardDeviations( 0.015, 0.01 - ), - FEEDER_TAG_CAMERA_STANDARD_DEVIATIONS = new StandardDeviations( - 0.02, - 0.1 - ); + ); private static final Transform3d ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA = new Transform3d( @@ -55,10 +51,4 @@ public class CameraConstants { ROBOT_CENTER_TO_RIGHT_REEF_TAG_CAMERA, REEF_TAG_CAMERA_STANDARD_DEVIATIONS ); -// FEEDER_TAG_CAMERA = new AprilTagCamera( -// AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, -// "CoralStationTagCamera", -// ROBOT_CENTER_TO_FEEDER_TAG_CAMERA, -// FEEDER_TAG_CAMERA_STANDARD_DEVIATIONS -// ); } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index fcdaaf5b..26ef4ae0 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -26,8 +26,13 @@ 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 FlippablePose2d AUTO_FIND_CORAL_POSE_LEFT = new FlippablePose2d(3.3, 5.5, Rotation2d.fromDegrees(130), true); - public static final FlippablePose2d AUTO_FIND_CORAL_POSE_RIGHT = new FlippablePose2d(3.3, FieldConstants.FIELD_WIDTH_METERS - 5.5, Rotation2d.fromDegrees(-130), true); + private static final double + AUTO_FIND_CORAL_POSE_X = 3.3, + AUTO_FIND_CORAL_POSE_LEFT_Y = 5.5; + private static final Rotation2d AUTO_FIND_CORAL_POSE_LEFT_ROTATION = Rotation2d.fromDegrees(130); + public static final FlippablePose2d + AUTO_FIND_CORAL_POSE_LEFT = new FlippablePose2d(AUTO_FIND_CORAL_POSE_X, AUTO_FIND_CORAL_POSE_LEFT_Y, AUTO_FIND_CORAL_POSE_LEFT_ROTATION, true), + AUTO_FIND_CORAL_POSE_RIGHT = new FlippablePose2d(AUTO_FIND_CORAL_POSE_X, FieldConstants.FIELD_WIDTH_METERS - AUTO_FIND_CORAL_POSE_LEFT_Y, AUTO_FIND_CORAL_POSE_LEFT_ROTATION.unaryMinus(), true); public static final Rotation2d LEFT_FEEDER_ANGLE = Rotation2d.fromDegrees(54); diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index e9be3c27..cc05c3ed 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -43,8 +43,8 @@ public class OperatorConstants { RESET_AMP_ALIGNER_TRIGGER = OPERATOR_CONTROLLER.v(), FLOOR_CORAL_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftTrigger().or(OPERATOR_CONTROLLER.c()), FEEDER_CORAL_COLLECTION_TRIGGER = OPERATOR_CONTROLLER.f().or(DRIVER_CONTROLLER.start()), - LEFT_SCORE_TRIGGER = DRIVER_CONTROLLER.leftStick().or(OPERATOR_CONTROLLER.b()).and(() -> !RobotContainer.ALGAE_MANIPULATOR.isOpen()).and(() -> !IS_RIGHT_SCORE_BUTTON_PRESSED).onTrue(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = true)).onFalse(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = false)), - RIGHT_SCORE_TRIGGER = DRIVER_CONTROLLER.rightStick().or(OPERATOR_CONTROLLER.m()).and(() -> !RobotContainer.ALGAE_MANIPULATOR.isOpen()).and(() -> !IS_LEFT_SCORE_BUTTON_PRESSED).onTrue(new InstantCommand(() -> IS_RIGHT_SCORE_BUTTON_PRESSED = true)).onFalse(new InstantCommand(() -> IS_RIGHT_SCORE_BUTTON_PRESSED = false)), + RIGHT_SCORE_TRIGGER = OPERATOR_CONTROLLER.m().or(createScoreTrigger(DRIVER_CONTROLLER.rightStick().and(() -> !RobotContainer.ALGAE_MANIPULATOR.isOpen()), true)), + LEFT_SCORE_TRIGGER = OPERATOR_CONTROLLER.b().or(createScoreTrigger(DRIVER_CONTROLLER.leftStick().and(() -> !RobotContainer.ALGAE_MANIPULATOR.isOpen()), false)), EJECT_CORAL_TRIGGER = OPERATOR_CONTROLLER.e().or(DRIVER_CONTROLLER.x()), UNLOAD_CORAL_TRIGGER = OPERATOR_CONTROLLER.z().or(DRIVER_CONTROLLER.back()), COLLECT_ALGAE_FROM_REEF_TRIGGER = OPERATOR_CONTROLLER.a().or(DRIVER_CONTROLLER.rightBumper()), @@ -52,8 +52,8 @@ public class OperatorConstants { COLLECT_ALGAE_FROM_LOLLIPOP_TRIGGER = OPERATOR_CONTROLLER.l().or(DRIVER_CONTROLLER.leftBumper()), COLLECT_ALGAE_FROM_FLOOR_TRIGGER = OPERATOR_CONTROLLER.s(), ROLL_ALGAE_ON_FLOOR_TRIGGER = OPERATOR_CONTROLLER.w(), - SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.n().or(DRIVER_CONTROLLER.rightStick()).and(() -> !IS_LEFT_SCORE_BUTTON_PRESSED).onTrue(new InstantCommand(() -> IS_RIGHT_SCORE_BUTTON_PRESSED = true)).onFalse(new InstantCommand(() -> IS_RIGHT_SCORE_BUTTON_PRESSED = false)), - SCORE_ALGAE_IN_PROCESSOR_TRIGGER = OPERATOR_CONTROLLER.j().or(() -> DRIVER_CONTROLLER.leftStick().getAsBoolean() && RobotContainer.ALGAE_MANIPULATOR.isOpen()).and(() -> !IS_RIGHT_SCORE_BUTTON_PRESSED).onTrue(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = true)).onFalse(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = false)); + SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.n().or(createScoreTrigger(DRIVER_CONTROLLER.rightStick().and(RobotContainer.ALGAE_MANIPULATOR::isOpen), true)), + SCORE_ALGAE_IN_PROCESSOR_TRIGGER = OPERATOR_CONTROLLER.j().or(createScoreTrigger(DRIVER_CONTROLLER.leftStick().and(RobotContainer.ALGAE_MANIPULATOR::isOpen), false)); public static final Trigger ENABLE_IGNORE_LOLLIPOP_CORAL_TRIGGER = OPERATOR_CONTROLLER.o(), DISABLE_IGNORE_LOLLIPOP_CORAL_TRIGGER = OPERATOR_CONTROLLER.p(), @@ -73,4 +73,16 @@ public class OperatorConstants { SET_TARGET_SCORING_REEF_CLOCK_POSITION_12_OCLOCK_TRIGGER = OPERATOR_CONTROLLER.numpad8(), SET_TARGET_REEF_SCORING_SIDE_LEFT_TRIGGER = OPERATOR_CONTROLLER.left(), SET_TARGET_REEF_SCORING_SIDE_RIGHT_TRIGGER = OPERATOR_CONTROLLER.right(); + + private static Trigger createScoreTrigger(Trigger button, boolean isRight) { + if (isRight) + return button + .and(() -> !IS_LEFT_SCORE_BUTTON_PRESSED) + .onTrue(new InstantCommand(() -> IS_RIGHT_SCORE_BUTTON_PRESSED = true)) + .onFalse(new InstantCommand(() -> IS_RIGHT_SCORE_BUTTON_PRESSED = false)); + return button + .and(() -> !IS_RIGHT_SCORE_BUTTON_PRESSED) + .onTrue(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = true)) + .onFalse(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = false)); + } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/PathPlannerConstants.java b/src/main/java/frc/trigon/robot/constants/PathPlannerConstants.java index eb455676..9e198f45 100644 --- a/src/main/java/frc/trigon/robot/constants/PathPlannerConstants.java +++ b/src/main/java/frc/trigon/robot/constants/PathPlannerConstants.java @@ -43,12 +43,6 @@ public class PathPlannerConstants { AUTO_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(0, 0, 0) : new PIDConstants(0, 0, 0); - // AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? -// new PIDConstants(6, 0, 0) : -// new PIDConstants(6, 0, 0.1), -// AUTO_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? -// new PIDConstants(5, 0, 0) : -// new PIDConstants(8, 0, 0); private static final PPHolonomicDriveController AUTO_PATH_FOLLOWING_CONTROLLER = new PPHolonomicDriveController( AUTO_TRANSLATION_PID_CONSTANTS, AUTO_ROTATION_PID_CONSTANTS diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java index ad554e2f..88236d8f 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java @@ -1,19 +1,13 @@ package frc.trigon.robot.misc.objectdetectioncamera.io; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraConstants; import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraIO; import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraInputsAutoLogged; -import org.littletonrobotics.junction.Logger; -import org.opencv.core.Point; import org.photonvision.PhotonCamera; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; -import org.photonvision.targeting.TargetCorner; import java.util.ArrayList; import java.util.Arrays; @@ -82,71 +76,10 @@ private Rotation3d[] toArray(List list) { } private Rotation3d extractRotation3d(PhotonTrackedTarget target) { -// final List detectedCorners = target.getMinAreaRectCorners(); -// -// if (detectedCorners.size() != 4) - return calculateMiddleRotation(target); - -// final Point objectPointOnCamera = findAverageOfLowestTwo(detectedCorners); -// final Rotation3d objectRotationOnCamera = getCorrectedPixelRot(objectPointOnCamera); -// -// if (objectRotationOnCamera == null) -// calculateMiddleRotation(target); -// -// return objectRotationOnCamera; - } - - private Rotation3d calculateMiddleRotation(PhotonTrackedTarget target) { return new Rotation3d( 0, Units.degreesToRadians(target.getPitch()), Units.degreesToRadians(-target.getYaw()) ); } - - private Point findAverageOfLowestTwo(List corners) { - TargetCorner lowest = corners.get(0); - TargetCorner secondLowest = corners.get(1); - - if (secondLowest.y > lowest.y) { - TargetCorner temp = lowest; - lowest = secondLowest; - secondLowest = temp; - } - - for (int i = 2; i < 4; i++) { - final TargetCorner current = corners.get(i); - - if (current.y > lowest.y) { - secondLowest = lowest; - lowest = current; - } else if (current.y > secondLowest.y) { - secondLowest = current; - } - } - - final double averageX = (lowest.x + secondLowest.x) / 2.0; - final double averageY = (lowest.y + secondLowest.y) / 2.0; - - return new Point(averageX, averageY); - } - - private Rotation3d getCorrectedPixelRot(Point point) { - final Matrix cameraIntrinsics = photonCamera.getCameraMatrix().orElse(null); - if (cameraIntrinsics == null) - return null; - - final double fx = cameraIntrinsics.get(0, 0); - final double cx = cameraIntrinsics.get(0, 2); - final double xOffset = cx - point.x; - final double fy = cameraIntrinsics.get(1, 1); - final double cy = cameraIntrinsics.get(1, 2); - final double yOffset = cy - point.y; - - final Rotation2d yaw = new Rotation2d(fx, xOffset); - final Rotation2d pitch = new Rotation2d(fy / Math.cos(Math.atan(xOffset / fx)), -yOffset); - Logger.recordOutput("Yaw", yaw.getDegrees()); - Logger.recordOutput("Pitch", -pitch.getDegrees()); - return new Rotation3d(0.0, -pitch.getRadians(), yaw.getRadians()); - } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 06d66127..1ebddc80 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -63,7 +63,9 @@ public double getLatestResultTimestampSeconds() { } public boolean hasValidResult() { - return inputs.hasResult && inputs.poseAmbiguity < AprilTagCameraConstants.MAXIMUM_AMBIGUITY && inputs.distancesFromTags[0] < 5; + return inputs.hasResult && + inputs.poseAmbiguity < AprilTagCameraConstants.MAXIMUM_AMBIGUITY && + inputs.distancesFromTags[0] < AprilTagCameraConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_VALID_RESULT_METERS; } /** diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index 29ff6837..1e87d2c5 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -15,7 +15,9 @@ import java.util.function.BiFunction; public class AprilTagCameraConstants { - static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_ACCURATE_SOLVE_PNP_RESULT_METERS = 2; + static final double + MAXIMUM_DISTANCE_FROM_TAG_FOR_ACCURATE_SOLVE_PNP_RESULT_METERS = 2, + MAXIMUM_DISTANCE_FROM_TAG_FOR_VALID_RESULT_METERS = 5; static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; static final double MAXIMUM_AMBIGUITY = 0.4; public static final PhotonPoseEstimator.ConstrainedSolvepnpParams CONSTRAINED_SOLVE_PNP_PARAMS = new PhotonPoseEstimator.ConstrainedSolvepnpParams(false, 0.1); diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index a7367d8c..5a26aef7 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -79,15 +79,6 @@ private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, Photon inputs.visibleTagIDs = getVisibleTagIDs(visibleNotHatefulTags); inputs.poseAmbiguity = latestResult.getMultiTagResult().isPresent() ? 0 : bestTag.getPoseAmbiguity(); inputs.distancesFromTags = getDistancesFromTags(visibleNotHatefulTags); - inputs.hasConstrainedResult = false; - } - - private PhotonTrackedTarget getBestTag(PhotonPipelineResult result) { - for (PhotonTrackedTarget tag : result.getTargets()) { - if (FieldConstants.TAG_ID_TO_POSE.containsKey(tag.getFiducialId())) - return tag; - } - return null; } private void updateSolvePNPPoses(AprilTagCameraInputsAutoLogged inputs, PhotonPipelineResult latestResult, PhotonTrackedTarget bestTag) { @@ -111,6 +102,7 @@ private void updateSolvePNPPoses(AprilTagCameraInputsAutoLogged inputs, PhotonPi inputs.alternateCameraSolvePNPPose = tagPose.transformBy(alternateTargetToCamera); // updateConstrainedSolvePNPPose(inputs, latestResult); + inputs.hasConstrainedResult = false; } private void updateConstrainedSolvePNPPose(AprilTagCameraInputsAutoLogged inputs, PhotonPipelineResult latestResult) { @@ -172,20 +164,20 @@ private PhotonTrackedTarget[] getVisibleNotHatefulTags(PhotonPipelineResult resu return Arrays.copyOf(visibleTagIDs, index); } - private int[] getVisibleTagIDs(PhotonTrackedTarget[] targets) { - final int[] visibleTagIDs = new int[targets.length]; + private int[] getVisibleTagIDs(PhotonTrackedTarget[] tags) { + final int[] visibleTagIDs = new int[tags.length]; for (int i = 0; i < visibleTagIDs.length; i++) - visibleTagIDs[i] = targets[i].getFiducialId(); + visibleTagIDs[i] = tags[i].getFiducialId(); return visibleTagIDs; } - private double[] getDistancesFromTags(PhotonTrackedTarget[] targets) { - final double[] distances = new double[targets.length]; + private double[] getDistancesFromTags(PhotonTrackedTarget[] tags) { + final double[] distances = new double[tags.length]; - for (int i = 0; i < targets.length; i++) - distances[i] = getDistanceFromTarget(targets[i]); + for (int i = 0; i < tags.length; i++) + distances[i] = getDistanceFromTarget(tags[i]); return distances; } diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index acf42c4a..45070984 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -93,7 +93,7 @@ public void resetHeading() { public void resetPose(Pose2d newPose) { RobotContainer.SWERVE.setHeading(newPose.getRotation()); - swerveDrivePoseEstimator.resetPose(newPose); // TODO: Might not work as intended + swerveDrivePoseEstimator.resetPose(newPose); swerveDriveOdometry.resetPose(newPose); if (shouldUseRelativeRobotPoseSource) relativeRobotPoseSource.resetOffset(newPose); diff --git a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java index 228cdd97..e57911dc 100644 --- a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java +++ b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java @@ -27,8 +27,7 @@ public abstract class MotorSubsystem extends edu.wpi.first.wpilibj2.command.Subs private static final List REGISTERED_SUBSYSTEMS = new ArrayList<>(); private static final Trigger DISABLED_TRIGGER = new Trigger(DriverStation::isDisabled); private static final Executor BRAKE_MODE_EXECUTOR = Executors.newFixedThreadPool(8); - // private static final LoggedNetworkBoolean ENABLE_EXTENSIVE_LOGGING = new LoggedNetworkBoolean("/SmartDashboard/EnableExtensiveLogging", !DriverStation.isFMSAttached()); - private static final LoggedNetworkBoolean ENABLE_EXTENSIVE_LOGGING = new LoggedNetworkBoolean("/SmartDashboard/EnableExtensiveLogging", false); + private static final LoggedNetworkBoolean ENABLE_EXTENSIVE_LOGGING = new LoggedNetworkBoolean("/SmartDashboard/EnableExtensiveLogging", RobotHardwareStats.isSimulation()); static { DISABLED_TRIGGER.onTrue(new InstantCommand(() -> forEach(MotorSubsystem::stop)).ignoringDisable(true)); @@ -61,7 +60,6 @@ public static void forEach(Consumer toRun) { * @param brake whether the motors should brake or coast */ public static void setAllSubsystemsBrakeAsync(boolean brake) { -// CompletableFuture.runAsync(() -> forEach((subsystem) -> subsystem.setBrake(brake))); BRAKE_MODE_EXECUTOR.execute(() -> forEach((subsystem) -> subsystem.setBrake(brake))); } 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 e613897b..84afd241 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,6 @@ public enum ElevatorState { SCORE_L4(1.045, 1), COLLECT_ALGAE_FROM_L3(0.35, 1), REST_WITH_ALGAE(0, 0.3), - QUICK_SCORE_NET(1, 0.3), SCORE_NET(1.045, 0.3); public final double targetPositionMeters; 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 a6d0a833..9daa464a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -71,7 +71,7 @@ public void updatePeriodically() { updatePoseEstimatorStates(); RobotContainer.POSE_ESTIMATOR.periodic(); -// updateNetworkTables(); + updateNetworkTables(); } @Override From 4ab01c48774c8b6fe7e0d5484fcf596f24fe0698 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 30 Jun 2025 12:53:05 +0300 Subject: [PATCH 34/65] Clean? idk --- .../CoralCollectionCommands.java | 64 +++++++++---------- 1 file changed, 30 insertions(+), 34 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 79838d1b..a09a337b 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -13,22 +13,21 @@ import frc.trigon.robot.subsystems.gripper.GripperCommands; import frc.trigon.robot.subsystems.gripper.GripperConstants; import org.trigon.utilities.flippable.FlippablePose2d; -import org.trigon.utilities.flippable.FlippableRotation2d; public class CoralCollectionCommands { public static boolean SHOULD_IGNORE_LOLLIPOP_CORAL = true; - public static Command getFeederCoralCollectionFromGripperCommand() { + public static Command getFloorCoralCollectionCommand() { return new ParallelCommandGroup( - GripperCommands.getSetTargetStateCommand(GripperConstants.GripperState.COLLECT_CORAL_FROM_FEEDER), - ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST), + CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.COLLECT_FROM_FLOOR), getScheduleCoralLoadingWhenCollectedCommand() - ).until(RobotContainer.GRIPPER::hasGamePiece); - } - - public static Command getFloorCoralCollectionCommand() { - return getInitiateFloorCoralCollectionCommand(); +// SwerveCommands.getClosedLoopFieldRelativeDriveCommand( +// () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), +// () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), +// CommandConstants::calculateTargetHeadingFromJoystickAngle +// ).asProxy() + ); } public static Command getFeederCoralCollectionCommand() { @@ -39,17 +38,6 @@ public static Command getFeederCoralCollectionCommand() { ); } - private static boolean isIntakeFacingFeeder() { - final Pose2d robotPose = new FlippablePose2d(RobotContainer.POSE_ESTIMATOR.getEstimatedRobotPose(), true).get(); - final Rotation2d - robotHeading = robotPose.getRotation(), - leftFeederAngle = FieldConstants.LEFT_FEEDER_ANGLE; - - if (robotPose.getY() > FieldConstants.FIELD_WIDTH_METERS / 2) - return robotHeading.plus(leftFeederAngle).plus(Rotation2d.kCW_90deg).getDegrees() > 0; - return robotHeading.minus(leftFeederAngle).plus(Rotation2d.kCCW_90deg).getDegrees() < 0; - } - private static Command getInitiateFeederCoralCollectionCommand() { return new ParallelCommandGroup( CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.COLLECT_FROM_FEEDER), @@ -57,20 +45,33 @@ private static Command getInitiateFeederCoralCollectionCommand() { ); } - private static Command getInitiateFloorCoralCollectionCommand() { + public static Command getFeederCoralCollectionFromGripperCommand() { return new ParallelCommandGroup( - CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.COLLECT_FROM_FLOOR), + GripperCommands.getSetTargetStateCommand(GripperConstants.GripperState.COLLECT_CORAL_FROM_FEEDER), + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST), getScheduleCoralLoadingWhenCollectedCommand() -// SwerveCommands.getClosedLoopFieldRelativeDriveCommand( -// () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), -// () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), -// CommandConstants::calculateTargetHeadingFromJoystickAngle -// ).asProxy() - ); + ).until(RobotContainer.GRIPPER::hasGamePiece); + } + + private static boolean isIntakeFacingFeeder() { + final Pose2d robotPose = new FlippablePose2d(RobotContainer.POSE_ESTIMATOR.getEstimatedRobotPose(), true).get(); + final Rotation2d + robotHeading = robotPose.getRotation(), + leftFeederAngle = FieldConstants.LEFT_FEEDER_ANGLE; + + if (robotPose.getY() > FieldConstants.FIELD_WIDTH_METERS / 2) + return robotHeading.plus(leftFeederAngle).plus(Rotation2d.kCW_90deg).getDegrees() > 0; + return robotHeading.minus(leftFeederAngle).plus(Rotation2d.kCCW_90deg).getDegrees() < 0; } private static Command getScheduleCoralLoadingWhenCollectedCommand() { - return GeneralCommands.runWhen(getCollectionConfirmationCommand().alongWith(getScheduleCoralLoadingCommand()), CoralCollectionCommands::didCollectCoral); + return GeneralCommands.runWhen( + new ParallelCommandGroup( + getCollectionConfirmationCommand(), + getScheduleCoralLoadingCommand() + ), + CoralCollectionCommands::didCollectCoral + ); } private static Command getScheduleCoralLoadingCommand() { @@ -118,11 +119,6 @@ private static Command getCoralIntakeLoadingSequenceCommand() { ).repeatedly(); } - private static boolean isMovingCoralToGripper() { - return RobotContainer.CORAL_INTAKE.getTargetState() == CoralIntakeConstants.CoralIntakeState.LOAD_CORAL_TO_GRIPPER_SEEING_GAME_PIECE_WITH_BEAM_BREAK || - RobotContainer.CORAL_INTAKE.getTargetState() == CoralIntakeConstants.CoralIntakeState.LOAD_CORAL_TO_GRIPPER_NOT_SEEING_GAME_PIECE_WITH_BEAM_BREAK; - } - private static Command getCollectionConfirmationCommand() { return new ParallelCommandGroup( new InstantCommand(() -> OperatorConstants.DRIVER_CONTROLLER.rumble(CoralIntakeConstants.COLLECTION_RUMBLE_DURATION_SECONDS, CoralIntakeConstants.COLLECTION_RUMBLE_POWER)) From c427a6e9e3cc919273f3397e345fc06651b27851 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 30 Jun 2025 13:08:55 +0300 Subject: [PATCH 35/65] Ok? --- .../commands/commandfactories/AlgaeManipulationCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java index d34b2d0d..4ae969f3 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -180,7 +180,7 @@ private static Command getAlignToReefCommand() { ).raceWith( new WaitCommand(1).andThen(new WaitUntilCommand(RobotContainer.GRIPPER::isMovingSlowly)), new WaitUntilCommand(OperatorConstants.CONTINUE_TRIGGER) - ); + ); } private static boolean isScoreAlgaeButtonPressed() { From 56ae69b1bd207a66266df77722eb56c09750887d Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 30 Jun 2025 13:32:08 +0300 Subject: [PATCH 36/65] Keep open --- src/main/java/frc/trigon/robot/RobotContainer.java | 4 ++-- .../frc/trigon/robot/commands/CommandConstants.java | 3 ++- .../commandfactories/CoralCollectionCommands.java | 8 +++++--- .../frc/trigon/robot/constants/OperatorConstants.java | 3 ++- .../robot/subsystems/coralintake/CoralIntake.java | 6 +----- .../subsystems/coralintake/CoralIntakeCommands.java | 10 ++++++++++ 6 files changed, 22 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index fe28a0d3..43f35d36 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -26,7 +26,6 @@ import frc.trigon.robot.subsystems.algaemanipulator.AlgaeManipulatorCommands; import frc.trigon.robot.subsystems.coralintake.CoralIntake; import frc.trigon.robot.subsystems.coralintake.CoralIntakeCommands; -import frc.trigon.robot.subsystems.coralintake.CoralIntakeConstants; import frc.trigon.robot.subsystems.elevator.Elevator; import frc.trigon.robot.subsystems.elevator.ElevatorCommands; import frc.trigon.robot.subsystems.elevator.ElevatorConstants; @@ -86,7 +85,7 @@ private void configureBindings() { private void bindDefaultCommands() { SWERVE.setDefaultCommand(GeneralCommands.getFieldRelativeDriveCommand()); ALGAE_MANIPULATOR.setDefaultCommand(AlgaeManipulatorCommands.getDefaultCommand()); - CORAL_INTAKE.setDefaultCommand(CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.REST)); + CORAL_INTAKE.setDefaultCommand(CoralIntakeCommands.getCoralIntakeDefaultCommand()); ELEVATOR.setDefaultCommand(ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST)); GRIPPER.setDefaultCommand(GripperCommands.getGripperDefaultCommand()); } @@ -119,6 +118,7 @@ private void bindSetters() { OperatorConstants.DISABLE_IGNORE_LOLLIPOP_CORAL_TRIGGER.onTrue(CommandConstants.DISABLE_IGNORE_LOLLIPOP_CORAL_COMMAND); OperatorConstants.ENABLE_AUTONOMOUS_REEF_SCORING_TRIGGER.onTrue(CommandConstants.ENABLE_AUTONOMOUS_REEF_SCORING_COMMAND); OperatorConstants.DISABLE_AUTONOMOUS_REEF_SCORING_TRIGGER.onTrue(CommandConstants.DISABLE_AUTONOMOUS_REEF_SCORING_COMMAND); + OperatorConstants.TOGGLE_SHOULD_KEEP_INTAKE_OPEN_TRIGGER.onTrue(CommandConstants.TOGGLE_SHOULD_KEEP_INTAKE_OPEN_COMMAND); } private void configureSysIdBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index 8c9bfc8c..ea6ee46c 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -65,7 +65,8 @@ public class CommandConstants { ENABLE_AUTONOMOUS_REEF_SCORING_COMMAND = new InstantCommand(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY = true).ignoringDisable(true), DISABLE_AUTONOMOUS_REEF_SCORING_COMMAND = new InstantCommand(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY = false).ignoringDisable(true), ENABLE_IGNORE_LOLLIPOP_CORAL_COMMAND = new InstantCommand(() -> CoralCollectionCommands.SHOULD_IGNORE_LOLLIPOP_CORAL = true).ignoringDisable(true), - DISABLE_IGNORE_LOLLIPOP_CORAL_COMMAND = new InstantCommand(() -> CoralCollectionCommands.SHOULD_IGNORE_LOLLIPOP_CORAL = false).ignoringDisable(true); + DISABLE_IGNORE_LOLLIPOP_CORAL_COMMAND = new InstantCommand(() -> CoralCollectionCommands.SHOULD_IGNORE_LOLLIPOP_CORAL = false).ignoringDisable(true), + TOGGLE_SHOULD_KEEP_INTAKE_OPEN_COMMAND = new InstantCommand(() -> CoralCollectionCommands.SHOULD_KEEP_INTAKE_OPEN = !CoralCollectionCommands.SHOULD_KEEP_INTAKE_OPEN); /** * Calculates the target drive power from an axis value by dividing it by the shift mode value. 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 a09a337b..e5aec9b8 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -16,7 +16,9 @@ public class CoralCollectionCommands { - public static boolean SHOULD_IGNORE_LOLLIPOP_CORAL = true; + public static boolean + SHOULD_IGNORE_LOLLIPOP_CORAL = true, + SHOULD_KEEP_INTAKE_OPEN = true; public static Command getFloorCoralCollectionCommand() { return new ParallelCommandGroup( @@ -33,8 +35,8 @@ public static Command getFloorCoralCollectionCommand() { public static Command getFeederCoralCollectionCommand() { return new ConditionalCommand( getInitiateFeederCoralCollectionCommand().unless(RobotContainer.GRIPPER::hasGamePiece), - getFeederCoralCollectionFromGripperCommand(), - CoralCollectionCommands::isIntakeFacingFeeder + getFeederCoralCollectionFromGripperCommand().asProxy(), + () -> CoralCollectionCommands.isIntakeFacingFeeder() || RobotContainer.ALGAE_MANIPULATOR.isOpen() ); } diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index cc05c3ed..eb4aa4dd 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -58,7 +58,8 @@ public class OperatorConstants { ENABLE_IGNORE_LOLLIPOP_CORAL_TRIGGER = OPERATOR_CONTROLLER.o(), DISABLE_IGNORE_LOLLIPOP_CORAL_TRIGGER = OPERATOR_CONTROLLER.p(), ENABLE_AUTONOMOUS_REEF_SCORING_TRIGGER = OPERATOR_CONTROLLER.u(), - DISABLE_AUTONOMOUS_REEF_SCORING_TRIGGER = OPERATOR_CONTROLLER.i(); + DISABLE_AUTONOMOUS_REEF_SCORING_TRIGGER = OPERATOR_CONTROLLER.i(), + TOGGLE_SHOULD_KEEP_INTAKE_OPEN_TRIGGER = DRIVER_CONTROLLER.b(); public static final Trigger SET_TARGET_SCORING_REEF_LEVEL_L1_FROM_GRIPPER_TRIGGER = OPERATOR_CONTROLLER.numpadDecimal(), SET_TARGET_SCORING_REEF_LEVEL_L1_FROM_CORAL_INTAKE_TRIGGER = OPERATOR_CONTROLLER.numpad0().or(DRIVER_CONTROLLER.povLeft()), diff --git a/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntake.java b/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntake.java index 8a1b99d8..56f67364 100644 --- a/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntake.java +++ b/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntake.java @@ -6,7 +6,6 @@ import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.commands.commandfactories.CoralCollectionCommands; @@ -235,16 +234,13 @@ private Rotation2d getCurrentEncoderAngle() { return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION) + CoralIntakeConstants.ANGLE_ENCODER_POSITION_OFFSET_VALUE); } - /** - * Logs the current match time and target reef placement for the dashboard. - * We use {@link SmartDashboard} instead of {@link Logger} because the {@link Logger} inputs don't show up in QDashboard for some reason. - */ private void logToDashboard() { Logger.recordOutput("GameTime", DriverStation.getMatchTime()); Logger.recordOutput("TargetCoralPlacementStates/IsTargetSideLeft", !OperatorConstants.REEF_CHOOSER.getReefSide().doesFlipYTransformWhenFacingDriverStation); Logger.recordOutput("TargetCoralPlacementStates/TargetLevel", OperatorConstants.REEF_CHOOSER.getScoringLevel().level); Logger.recordOutput("TargetCoralPlacementStates/TargetClockPositionForElastic", getClockPositionForElastic()); Logger.recordOutput("ShouldIgnoreLollipopCoral", CoralCollectionCommands.SHOULD_IGNORE_LOLLIPOP_CORAL); + Logger.recordOutput("ShouldKeepIntakeOpen", CoralCollectionCommands.SHOULD_KEEP_INTAKE_OPEN); Logger.recordOutput("ShouldScoreCoralAutonomously", CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY); } diff --git a/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeCommands.java b/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeCommands.java index 74ac0ae0..9ef743eb 100644 --- a/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeCommands.java @@ -5,6 +5,8 @@ import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.commandfactories.CoralCollectionCommands; +import frc.trigon.robot.commands.commandfactories.GeneralCommands; import org.trigon.commands.GearRatioCalculationCommand; import org.trigon.commands.NetworkTablesCommand; @@ -35,6 +37,14 @@ public static Command getGearRatioCalculationCommand() { ); } + public static Command getCoralIntakeDefaultCommand() { + return GeneralCommands.getContinuousConditionalCommand( + getPrepareForStateCommand(CoralIntakeConstants.CoralIntakeState.COLLECT_FROM_FLOOR), + getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.REST), + () -> CoralCollectionCommands.SHOULD_KEEP_INTAKE_OPEN + ); + } + public static Command getCenterCoralWithPulsingCommand() { return new FunctionalCommand( RobotContainer.CORAL_INTAKE::initializePulsing, From 46f1da36acfdfdcfb47e15492328fbed5d64c2a3 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 1 Jul 2025 04:02:40 +0300 Subject: [PATCH 37/65] Added intake assist + cleaning --- .../java/frc/trigon/robot/RobotContainer.java | 12 +- .../robot/commands/CommandConstants.java | 8 +- .../commandclasses/IntakeAssistCommand.java | 147 ++++++++++++++++++ .../AlgaeManipulationCommands.java | 4 +- .../commandfactories/AutonomousCommands.java | 10 +- .../CoralCollectionCommands.java | 10 +- .../CoralPlacingCommands.java | 6 +- ...onstants.java => AutonomousConstants.java} | 11 +- .../subsystems/coralintake/CoralIntake.java | 1 + .../robot/subsystems/swerve/Swerve.java | 10 +- .../subsystems/swerve/SwerveConstants.java | 8 +- .../swervemodule/SwerveModuleConstants.java | 4 +- 12 files changed, 191 insertions(+), 40 deletions(-) create mode 100644 src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java rename src/main/java/frc/trigon/robot/constants/{PathPlannerConstants.java => AutonomousConstants.java} (92%) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 43f35d36..896b7319 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -16,10 +16,10 @@ import frc.trigon.robot.commands.commandclasses.CoralAlignmentCommand; import frc.trigon.robot.commands.commandclasses.LEDAutoSetupCommand; import frc.trigon.robot.commands.commandfactories.*; +import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.constants.OperatorConstants; -import frc.trigon.robot.constants.PathPlannerConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.algaemanipulator.AlgaeManipulator; @@ -73,7 +73,7 @@ public Command getAutonomousCommand() { */ private void initializeGeneralSystems() { Flippable.init(); - PathPlannerConstants.init(); + AutonomousConstants.init(); } private void configureBindings() { @@ -114,8 +114,8 @@ private void bindControllerCommands() { } private void bindSetters() { - OperatorConstants.ENABLE_IGNORE_LOLLIPOP_CORAL_TRIGGER.onTrue(CommandConstants.ENABLE_IGNORE_LOLLIPOP_CORAL_COMMAND); - OperatorConstants.DISABLE_IGNORE_LOLLIPOP_CORAL_TRIGGER.onTrue(CommandConstants.DISABLE_IGNORE_LOLLIPOP_CORAL_COMMAND); + OperatorConstants.ENABLE_IGNORE_LOLLIPOP_CORAL_TRIGGER.onTrue(CommandConstants.ENABLE_INTAKE_ASSIST_COMMAND); + OperatorConstants.DISABLE_IGNORE_LOLLIPOP_CORAL_TRIGGER.onTrue(CommandConstants.DISABLE_INTAKE_ASSIST_COMMAND); OperatorConstants.ENABLE_AUTONOMOUS_REEF_SCORING_TRIGGER.onTrue(CommandConstants.ENABLE_AUTONOMOUS_REEF_SCORING_COMMAND); OperatorConstants.DISABLE_AUTONOMOUS_REEF_SCORING_TRIGGER.onTrue(CommandConstants.DISABLE_AUTONOMOUS_REEF_SCORING_COMMAND); OperatorConstants.TOGGLE_SHOULD_KEEP_INTAKE_OPEN_TRIGGER.onTrue(CommandConstants.TOGGLE_SHOULD_KEEP_INTAKE_OPEN_COMMAND); @@ -140,11 +140,11 @@ private void buildAutoChooser() { final PathPlannerAuto autoNonMirrored = new PathPlannerAuto(autoName); final PathPlannerAuto autoMirrored = new PathPlannerAuto(autoName, true); - if (!PathPlannerConstants.DEFAULT_AUTO_NAME.isEmpty() && PathPlannerConstants.DEFAULT_AUTO_NAME.equals(autoName)) { + if (!AutonomousConstants.DEFAULT_AUTO_NAME.isEmpty() && AutonomousConstants.DEFAULT_AUTO_NAME.equals(autoName)) { hasDefault = true; autoChooser.addDefaultOption(autoNonMirrored.getName(), autoNonMirrored); autoChooser.addOption(autoMirrored.getName() + "Mirrored", autoMirrored); - } else if (!PathPlannerConstants.DEFAULT_AUTO_NAME.isEmpty() && PathPlannerConstants.DEFAULT_AUTO_NAME.equals(autoName + "Mirrored")) { + } else if (!AutonomousConstants.DEFAULT_AUTO_NAME.isEmpty() && AutonomousConstants.DEFAULT_AUTO_NAME.equals(autoName + "Mirrored")) { hasDefault = true; autoChooser.addDefaultOption(autoMirrored.getName() + "Mirrored", autoMirrored); autoChooser.addOption(autoNonMirrored.getName(), autoNonMirrored); diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index ea6ee46c..4f921ad7 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -9,9 +9,9 @@ import frc.trigon.robot.commands.commandfactories.CoralCollectionCommands; import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands; import frc.trigon.robot.commands.commandfactories.GeneralCommands; +import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.constants.OperatorConstants; -import frc.trigon.robot.constants.PathPlannerConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; import org.trigon.commands.CameraPositionCalculationCommand; import org.trigon.commands.WheelRadiusCharacterizationCommand; @@ -48,7 +48,7 @@ public class CommandConstants { () -> 0 ), WHEEL_RADIUS_CHARACTERIZATION_COMMAND = new WheelRadiusCharacterizationCommand( - PathPlannerConstants.ROBOT_CONFIG.moduleLocations, + AutonomousConstants.ROBOT_CONFIG.moduleLocations, RobotContainer.SWERVE::getDriveWheelPositionsRadians, () -> RobotContainer.SWERVE.getHeading().getRadians(), (omegaRadiansPerSecond) -> RobotContainer.SWERVE.selfRelativeDriveWithoutSetpointGeneration(new ChassisSpeeds(0, 0, omegaRadiansPerSecond), null), @@ -64,8 +64,8 @@ public class CommandConstants { public static final Command ENABLE_AUTONOMOUS_REEF_SCORING_COMMAND = new InstantCommand(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY = true).ignoringDisable(true), DISABLE_AUTONOMOUS_REEF_SCORING_COMMAND = new InstantCommand(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY = false).ignoringDisable(true), - ENABLE_IGNORE_LOLLIPOP_CORAL_COMMAND = new InstantCommand(() -> CoralCollectionCommands.SHOULD_IGNORE_LOLLIPOP_CORAL = true).ignoringDisable(true), - DISABLE_IGNORE_LOLLIPOP_CORAL_COMMAND = new InstantCommand(() -> CoralCollectionCommands.SHOULD_IGNORE_LOLLIPOP_CORAL = false).ignoringDisable(true), + ENABLE_INTAKE_ASSIST_COMMAND = new InstantCommand(() -> CoralCollectionCommands.SHOULD_ASSIST_INTAKE = true).ignoringDisable(true), + DISABLE_INTAKE_ASSIST_COMMAND = new InstantCommand(() -> CoralCollectionCommands.SHOULD_ASSIST_INTAKE = false).ignoringDisable(true), TOGGLE_SHOULD_KEEP_INTAKE_OPEN_COMMAND = new InstantCommand(() -> CoralCollectionCommands.SHOULD_KEEP_INTAKE_OPEN = !CoralCollectionCommands.SHOULD_KEEP_INTAKE_OPEN); /** diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java new file mode 100644 index 00000000..acbde52a --- /dev/null +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -0,0 +1,147 @@ +package frc.trigon.robot.commands.commandclasses; + +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.RunCommand; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.CommandConstants; +import frc.trigon.robot.commands.commandfactories.GeneralCommands; +import frc.trigon.robot.constants.AutonomousConstants; +import frc.trigon.robot.constants.CameraConstants; +import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; +import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; +import frc.trigon.robot.subsystems.swerve.SwerveCommands; +import org.littletonrobotics.junction.Logger; +import org.trigon.hardware.RobotHardwareStats; + +import java.util.function.Supplier; + +public class IntakeAssistCommand extends ParallelCommandGroup { + private static final ObjectDetectionCamera CAMERA = CameraConstants.OBJECT_DETECTION_CAMERA; + private static final ProfiledPIDController + X_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? + new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : + new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)), + Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? + new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : + new ProfiledPIDController(0.3, 0, 0.03, new TrapezoidProfile.Constraints(2.65, 5.5)), + THETA_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? + new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : + new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)); + private Translation2d distanceFromTrackedCoral; + + public IntakeAssistCommand() { + addCommands( + new InstantCommand(CAMERA::initializeTracking), + getTrackCoralCommand(), + GeneralCommands.getContinuousConditionalCommand( + GeneralCommands.getFieldRelativeDriveCommand(), + getAssistIntakeCommand(() -> distanceFromTrackedCoral), + () -> distanceFromTrackedCoral == null + ) + ); + } + + private Command getTrackCoralCommand() { + return new RunCommand(() -> { + CAMERA.trackObject(SimulatedGamePieceConstants.GamePieceType.CORAL); + distanceFromTrackedCoral = calculateDistanceFromTrackedCoral(); + }); + } + + public static Translation2d calculateDistanceFromTrackedCoral() { + final Pose2d robotPose = RobotContainer.POSE_ESTIMATOR.getEstimatedRobotPose(); + final Translation2d trackedObjectPositionOnField = CAMERA.getTrackedObjectFieldRelativePosition(); + if (trackedObjectPositionOnField == null) + return null; + + final Translation2d difference = robotPose.getTranslation().minus(trackedObjectPositionOnField); + Translation2d robotToTrackedCoralDistance = difference.rotateBy(robotPose.getRotation().unaryMinus()); + Logger.recordOutput("IntakeAssist/TrackedCoralDistance", robotToTrackedCoralDistance); + return robotToTrackedCoralDistance; + } + + public static Command getAssistIntakeCommand(Supplier distanceFromTrackedCoral) { + return SwerveCommands.getClosedLoopSelfRelativeDriveCommand( + () -> AutonomousConstants.INTAKE_ASSIST_MODE.shouldAssistX ? getXAssistedPower(distanceFromTrackedCoral.get()) : getXJoystickPower(), + () -> AutonomousConstants.INTAKE_ASSIST_MODE.shouldAssistY ? getYAssistedPower(distanceFromTrackedCoral.get()) : getYJoystickPower(), + () -> AutonomousConstants.INTAKE_ASSIST_MODE.shouldAssistTheta ? getThetaAssistedPower(distanceFromTrackedCoral.get()) : OperatorConstants.DRIVER_CONTROLLER.getRightX() + ); + } + + private static double getXAssistedPower(Translation2d distanceFromTrackedCoral) { + return calculateTranslationAssistPower(distanceFromTrackedCoral.getX(), X_PID_CONTROLLER, getXJoystickPower()); + } + + private static double getYAssistedPower(Translation2d distanceFromTrackedCoral) { + return calculateTranslationAssistPower(distanceFromTrackedCoral.getY(), Y_PID_CONTROLLER, getYJoystickPower()); + } + + private static double getThetaAssistedPower(Translation2d distanceFromTrackedCoral) { + return calculateRotationAssistPower(distanceFromTrackedCoral.getAngle().plus(Rotation2d.k180deg)); + } + + private static double getXJoystickPower() { + final Rotation2d robotHeading = RobotContainer.SWERVE.getDriveRelativeAngle(); + + final double + joystickX = OperatorConstants.DRIVER_CONTROLLER.getLeftY(), + joystickY = OperatorConstants.DRIVER_CONTROLLER.getLeftX(); + final double + xValue = CommandConstants.calculateDriveStickAxisValue(joystickX), + yValue = CommandConstants.calculateDriveStickAxisValue(joystickY); + + return (xValue * robotHeading.getCos()) + (yValue * robotHeading.getSin()); + } + + private static double getYJoystickPower() { + final Rotation2d robotHeading = RobotContainer.SWERVE.getDriveRelativeAngle(); + + final double + joystickX = OperatorConstants.DRIVER_CONTROLLER.getLeftY(), + joystickY = OperatorConstants.DRIVER_CONTROLLER.getLeftX(); + final double + xValue = CommandConstants.calculateDriveStickAxisValue(joystickX), + yValue = CommandConstants.calculateDriveStickAxisValue(joystickY); + + return (yValue * robotHeading.getCos()) - (xValue * robotHeading.getSin()); + } + + private static double calculateTranslationAssistPower(double distance, ProfiledPIDController pidController, double joystickValue) { + final double + assistPower = pidController.calculate(distance) * AutonomousConstants.INTAKE_ASSIST_SCALAR, + stickPower = joystickValue * (1 - AutonomousConstants.INTAKE_ASSIST_SCALAR); + return assistPower + stickPower; + } + + private static double calculateRotationAssistPower(Rotation2d angleOffset) { + final double + assistPower = THETA_PID_CONTROLLER.calculate(-angleOffset.getRadians()) * AutonomousConstants.INTAKE_ASSIST_SCALAR, + stickPower = OperatorConstants.DRIVER_CONTROLLER.getRightX() * (1 - AutonomousConstants.INTAKE_ASSIST_SCALAR); + return assistPower + stickPower; + } + + public enum AssistMode { + FULL_ASSIST(true, true, true), + ALIGN_ASSIST(false, true, true), + ASSIST_ROTATION(false, false, true); + + final boolean + shouldAssistX, + shouldAssistY, + shouldAssistTheta; + + AssistMode(boolean shouldAssistX, boolean shouldAssistY, boolean shouldAssistTheta) { + this.shouldAssistX = shouldAssistX; + this.shouldAssistY = shouldAssistY; + this.shouldAssistTheta = shouldAssistTheta; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java index 4ae969f3..e0468bdf 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -8,9 +8,9 @@ import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.commands.commandclasses.WaitUntilChangeCommand; +import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.constants.OperatorConstants; -import frc.trigon.robot.constants.PathPlannerConstants; import frc.trigon.robot.misc.ReefChooser; import frc.trigon.robot.subsystems.algaemanipulator.AlgaeManipulatorCommands; import frc.trigon.robot.subsystems.coralintake.CoralIntakeCommands; @@ -170,7 +170,7 @@ private static Command getAlignToReefCommand() { return new SequentialCommandGroup( SwerveCommands.getDriveToPoseCommand( AlgaeManipulationCommands::calculateClosestAlgaeCollectionPose, - PathPlannerConstants.DRIVE_TO_REEF_CONSTRAINTS + AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS ), SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> fieldRelativePowersToSelfRelativeXPower(OperatorConstants.DRIVER_CONTROLLER.getLeftY(), OperatorConstants.DRIVER_CONTROLLER.getLeftX()), diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index 2b26ede8..2fe139c3 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -9,9 +9,9 @@ import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandclasses.CoralAutoDriveCommand; +import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.FieldConstants; -import frc.trigon.robot.constants.PathPlannerConstants; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.subsystems.coralintake.CoralIntakeCommands; import frc.trigon.robot.subsystems.coralintake.CoralIntakeConstants; @@ -77,7 +77,7 @@ public static Command getDriveToCoralCommand(boolean isRight) { public static Command getFindCoralCommand(boolean isRight) { return new SequentialCommandGroup( - SwerveCommands.getDriveToPoseCommand(() -> isRight ? FieldConstants.AUTO_FIND_CORAL_POSE_RIGHT : FieldConstants.AUTO_FIND_CORAL_POSE_LEFT, PathPlannerConstants.DRIVE_TO_REEF_CONSTRAINTS, 2.3), + SwerveCommands.getDriveToPoseCommand(() -> isRight ? FieldConstants.AUTO_FIND_CORAL_POSE_RIGHT : FieldConstants.AUTO_FIND_CORAL_POSE_LEFT, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS, 2.3), SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> 0, () -> 0, @@ -99,8 +99,8 @@ public static Command getDriveToReefWithoutHittingAlgaeCommand(FieldConstants.Re return new SequentialCommandGroup( new InstantCommand(() -> TARGET_SCORING_POSE = calculateClosestScoringPose(true, reefClockPositions, false)), new WaitUntilCommand(() -> TARGET_SCORING_POSE != null).raceWith(SwerveCommands.getClosedLoopSelfRelativeDriveCommand(() -> 0, () -> 0, () -> 0)), - SwerveCommands.getDriveToPoseCommand(() -> calculateClosestScoringPose(true, reefClockPositions, true), PathPlannerConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly().until(RobotContainer.ELEVATOR::isOverAlgaeHitRange), - SwerveCommands.getDriveToPoseCommand(() -> TARGET_SCORING_POSE, PathPlannerConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly() + SwerveCommands.getDriveToPoseCommand(() -> calculateClosestScoringPose(true, reefClockPositions, true), AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly().until(RobotContainer.ELEVATOR::isOverAlgaeHitRange), + SwerveCommands.getDriveToPoseCommand(() -> TARGET_SCORING_POSE, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly() ); } @@ -141,7 +141,7 @@ public static Command getPrepareForScoreCommand() { private static Command getOpenElevatorWhenCloseToReefCommand() { return GeneralCommands.runWhen( ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_L4), - () -> calculateDistanceToTargetScoringPose() < PathPlannerConstants.MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR + () -> calculateDistanceToTargetScoringPose() < AutonomousConstants.MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR ); } 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 e5aec9b8..f62d3dc8 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -4,6 +4,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.coralintake.CoralIntakeCommands; @@ -18,17 +19,14 @@ public class CoralCollectionCommands { public static boolean SHOULD_IGNORE_LOLLIPOP_CORAL = true, + SHOULD_ASSIST_INTAKE = true, SHOULD_KEEP_INTAKE_OPEN = true; public static Command getFloorCoralCollectionCommand() { return new ParallelCommandGroup( CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.COLLECT_FROM_FLOOR), - getScheduleCoralLoadingWhenCollectedCommand() -// SwerveCommands.getClosedLoopFieldRelativeDriveCommand( -// () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), -// () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), -// CommandConstants::calculateTargetHeadingFromJoystickAngle -// ).asProxy() + getScheduleCoralLoadingWhenCollectedCommand(), + new IntakeAssistCommand().asProxy().onlyWhile(() -> SHOULD_ASSIST_INTAKE) ); } diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java index 0cf60287..3c171048 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -7,9 +7,9 @@ import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandclasses.WaitUntilChangeCommand; +import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.constants.OperatorConstants; -import frc.trigon.robot.constants.PathPlannerConstants; import frc.trigon.robot.misc.ReefChooser; import frc.trigon.robot.subsystems.coralintake.CoralIntakeCommands; import frc.trigon.robot.subsystems.coralintake.CoralIntakeConstants; @@ -127,7 +127,7 @@ private static Command scoreFromGripperInL4Command(boolean shouldScoreRight) { private static Command getOpenElevatorWhenCloseToReefCommand(boolean shouldScoreRight) { return GeneralCommands.runWhen( ElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getElevatorState), - () -> calculateDistanceToTargetScoringPose(shouldScoreRight) < PathPlannerConstants.MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR + () -> calculateDistanceToTargetScoringPose(shouldScoreRight) < AutonomousConstants.MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR ); } @@ -135,7 +135,7 @@ private static Command getAutonomousDriveToReefThenManualDriveCommand(boolean sh return new SequentialCommandGroup( SwerveCommands.getDriveToPoseCommand( () -> CoralPlacingCommands.calculateClosestScoringPose(shouldScoreRight), - PathPlannerConstants.DRIVE_TO_REEF_CONSTRAINTS + AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS ), GeneralCommands.getFieldRelativeDriveCommand() ); diff --git a/src/main/java/frc/trigon/robot/constants/PathPlannerConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java similarity index 92% rename from src/main/java/frc/trigon/robot/constants/PathPlannerConstants.java rename to src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index 9e198f45..66dd2eb4 100644 --- a/src/main/java/frc/trigon/robot/constants/PathPlannerConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -10,6 +10,7 @@ import com.pathplanner.lib.pathfinding.Pathfinding; import edu.wpi.first.math.util.Units; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand; import frc.trigon.robot.commands.commandfactories.AutonomousCommands; import frc.trigon.robot.commands.commandfactories.CoralCollectionCommands; import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands; @@ -27,14 +28,16 @@ import java.io.IOException; /** - * A class that contains the constants and configurations for everything related to PathPlanner. + * A class that contains the constants and configurations for everything related to the 15-second autonomous period at the start of the match. */ -public class PathPlannerConstants { +public class AutonomousConstants { public static final PathConstraints DRIVE_TO_REEF_CONSTRAINTS = new PathConstraints(2.5, 4, Units.degreesToRadians(450), Units.degreesToRadians(900)); public static final double MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR = 2.2; public static final String DEFAULT_AUTO_NAME = "FloorAutonomousRight6Branches"; public static final RobotConfig ROBOT_CONFIG = getRobotConfig(); - public static final double FEEDFORWARD_SCALAR = 0.57; + public static final double + FEEDFORWARD_SCALAR = 0.57, + INTAKE_ASSIST_SCALAR = 0.5; private static final PIDConstants AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? @@ -48,6 +51,8 @@ public class PathPlannerConstants { AUTO_ROTATION_PID_CONSTANTS ); + public static final IntakeAssistCommand.AssistMode INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.FULL_ASSIST; + /** * Initializes PathPlanner. This needs to be called before any PathPlanner function can be used. */ diff --git a/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntake.java b/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntake.java index 56f67364..38b9694b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntake.java +++ b/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntake.java @@ -240,6 +240,7 @@ private void logToDashboard() { Logger.recordOutput("TargetCoralPlacementStates/TargetLevel", OperatorConstants.REEF_CHOOSER.getScoringLevel().level); Logger.recordOutput("TargetCoralPlacementStates/TargetClockPositionForElastic", getClockPositionForElastic()); Logger.recordOutput("ShouldIgnoreLollipopCoral", CoralCollectionCommands.SHOULD_IGNORE_LOLLIPOP_CORAL); + Logger.recordOutput("ShouldAssistIntake", CoralCollectionCommands.SHOULD_ASSIST_INTAKE); Logger.recordOutput("ShouldKeepIntakeOpen", CoralCollectionCommands.SHOULD_KEEP_INTAKE_OPEN); Logger.recordOutput("ShouldScoreCoralAutonomously", CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY); } 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 9daa464a..5624aaf3 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -13,7 +13,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.constants.PathPlannerConstants; +import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModule; @@ -32,10 +32,10 @@ public class Swerve extends MotorSubsystem { private final Pigeon2Gyro gyro = SwerveConstants.GYRO; private final SwerveModule[] swerveModules = SwerveConstants.SWERVE_MODULES; private final Phoenix6SignalThread phoenix6SignalThread = Phoenix6SignalThread.getInstance(); - private final SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator(PathPlannerConstants.ROBOT_CONFIG, SwerveModuleConstants.MAXIMUM_MODULE_ROTATIONAL_SPEED_RADIANS_PER_SECOND); + private final SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator(AutonomousConstants.ROBOT_CONFIG, SwerveModuleConstants.MAXIMUM_MODULE_ROTATIONAL_SPEED_RADIANS_PER_SECOND); public Pose2d targetPathPlannerPose = new Pose2d(); public boolean isPathPlannerDriving = false; - private SwerveSetpoint previousSetpoint = new SwerveSetpoint(getSelfRelativeVelocity(), getModuleStates(), DriveFeedforwards.zeros(PathPlannerConstants.ROBOT_CONFIG.numModules)); + private SwerveSetpoint previousSetpoint = new SwerveSetpoint(getSelfRelativeVelocity(), getModuleStates(), DriveFeedforwards.zeros(AutonomousConstants.ROBOT_CONFIG.numModules)); public Swerve() { setName("Swerve"); @@ -157,7 +157,7 @@ public void drivePathPlanner(ChassisSpeeds targetPathPlannerFeedforwardSpeeds, b if (isFromPathPlanner && DriverStation.isAutonomous() && !isPathPlannerDriving) return; final ChassisSpeeds pidSpeeds = calculateSelfRelativePIDToPoseSpeeds(new FlippablePose2d(targetPathPlannerPose, false)); - final ChassisSpeeds scaledSpeeds = targetPathPlannerFeedforwardSpeeds.times(PathPlannerConstants.FEEDFORWARD_SCALAR); + final ChassisSpeeds scaledSpeeds = targetPathPlannerFeedforwardSpeeds.times(AutonomousConstants.FEEDFORWARD_SCALAR); final ChassisSpeeds combinedSpeeds = pidSpeeds.plus(scaledSpeeds); selfRelativeDrive(combinedSpeeds); } @@ -191,7 +191,7 @@ public Rotation2d getDriveRelativeAngle() { } public void resetSetpoint() { - previousSetpoint = new SwerveSetpoint(getSelfRelativeVelocity(), getModuleStates(), DriveFeedforwards.zeros(PathPlannerConstants.ROBOT_CONFIG.numModules)); + previousSetpoint = new SwerveSetpoint(getSelfRelativeVelocity(), getModuleStates(), DriveFeedforwards.zeros(AutonomousConstants.ROBOT_CONFIG.numModules)); } public void initializeDrive(boolean shouldUseClosedLoop) { diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index 5bb1ac14..050fad3f 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -8,7 +8,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.constants.PathPlannerConstants; +import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.constants.RobotConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants; import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModule; @@ -44,7 +44,7 @@ public class SwerveConstants { new SwerveModule(REAR_RIGHT_ID, REAR_RIGHT_STEER_ENCODER_OFFSET, REAR_RIGHT_WHEEL_DIAMETER) }; - public static final SwerveDriveKinematics KINEMATICS = new SwerveDriveKinematics(PathPlannerConstants.ROBOT_CONFIG.moduleLocations); + public static final SwerveDriveKinematics KINEMATICS = new SwerveDriveKinematics(AutonomousConstants.ROBOT_CONFIG.moduleLocations); static final double TRANSLATION_TOLERANCE_METERS = 0.035, ROTATION_TOLERANCE_DEGREES = 1.5, @@ -55,8 +55,8 @@ public class SwerveConstants { ROTATION_NEUTRAL_DEADBAND = 0.2; public static final double - MAXIMUM_SPEED_METERS_PER_SECOND = PathPlannerConstants.ROBOT_CONFIG.moduleConfig.maxDriveVelocityMPS, - MAXIMUM_ROTATIONAL_SPEED_RADIANS_PER_SECOND = PathPlannerConstants.ROBOT_CONFIG.moduleConfig.maxDriveVelocityMPS / PathPlannerConstants.ROBOT_CONFIG.modulePivotDistance[0]; + MAXIMUM_SPEED_METERS_PER_SECOND = AutonomousConstants.ROBOT_CONFIG.moduleConfig.maxDriveVelocityMPS, + MAXIMUM_ROTATIONAL_SPEED_RADIANS_PER_SECOND = AutonomousConstants.ROBOT_CONFIG.moduleConfig.maxDriveVelocityMPS / AutonomousConstants.ROBOT_CONFIG.modulePivotDistance[0]; private static final PIDConstants TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java index 99367104..1d28efa0 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/swervemodule/SwerveModuleConstants.java @@ -9,7 +9,7 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.trigon.robot.constants.PathPlannerConstants; +import frc.trigon.robot.constants.AutonomousConstants; import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.simulation.SimpleMotorSimulation; @@ -68,7 +68,7 @@ static TalonFXConfiguration generateDriveMotorConfiguration() { config.MotorOutput.NeutralMode = NeutralModeValue.Brake; config.Feedback.SensorToMechanismRatio = DRIVE_MOTOR_GEAR_RATIO; - final double driveMotorSlipCurrent = PathPlannerConstants.ROBOT_CONFIG.moduleConfig.driveCurrentLimit; + final double driveMotorSlipCurrent = AutonomousConstants.ROBOT_CONFIG.moduleConfig.driveCurrentLimit; config.TorqueCurrent.PeakForwardTorqueCurrent = driveMotorSlipCurrent; config.TorqueCurrent.PeakReverseTorqueCurrent = -driveMotorSlipCurrent; config.CurrentLimits.StatorCurrentLimit = driveMotorSlipCurrent; From 6a8fe45b34ed1a8afcacd29b3a216097ac61f29b Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 1 Jul 2025 15:41:12 +0300 Subject: [PATCH 38/65] Good code --- .../robot/commands/commandclasses/IntakeAssistCommand.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 acbde52a..753798e1 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -44,7 +44,7 @@ public IntakeAssistCommand() { GeneralCommands.getContinuousConditionalCommand( GeneralCommands.getFieldRelativeDriveCommand(), getAssistIntakeCommand(() -> distanceFromTrackedCoral), - () -> distanceFromTrackedCoral == null + () -> CAMERA.getTrackedObjectFieldRelativePosition() == null ) ); } From 1be345b080e19ee295a8f9c45f5acce7df2d9e65 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 1 Jul 2025 16:03:42 +0300 Subject: [PATCH 39/65] Yay --- build.gradle | 2 +- .../commands/commandfactories/CoralCollectionCommands.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/build.gradle b/build.gradle index 2fe8b95a..c233d28c 100644 --- a/build.gradle +++ b/build.gradle @@ -107,7 +107,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() implementation 'com.google.code.gson:gson:2.10.1' - implementation 'com.github.Programming-TRIGON:TRIGONLib:c382fceed9' + implementation 'com.github.Programming-TRIGON:TRIGONLib:v2025.3.1' def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" 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 f62d3dc8..5bdd8cce 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -18,7 +18,7 @@ public class CoralCollectionCommands { public static boolean - SHOULD_IGNORE_LOLLIPOP_CORAL = true, + SHOULD_IGNORE_LOLLIPOP_CORAL = false, SHOULD_ASSIST_INTAKE = true, SHOULD_KEEP_INTAKE_OPEN = true; From 69aeb18dfbef43c1259e9b0992d8cebf6d3b94be Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 1 Jul 2025 18:45:29 +0300 Subject: [PATCH 40/65] tehehe --- .../java/frc/trigon/robot/subsystems/elevator/Elevator.java | 4 ---- 1 file changed, 4 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 deeb7f9f..76a74022 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -83,10 +83,6 @@ public boolean willCurrentMovementMoveThroughHitRange() { return willMovementMoveThroughHitRange(metersToRotations(targetState.targetPositionMeters)); } - public boolean isOverQuickNetScoreReleaseHeight() { - return getPositionRotations() > ElevatorConstants.QUICK_SCORE_NET_HEIGHT_LOWER_BOUND_POSITION_ROTATIONS; - } - public boolean isOverAlgaeHitRange() { return getPositionRotations() > ElevatorConstants.GRIPPER_HITTING_ALGAE_UPPER_BOUND_POSITION_ROTATIONS; } From 1f8deff4c73c9d7103084a838bd5b87c9f890c31 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 2 Jul 2025 12:30:58 +0300 Subject: [PATCH 41/65] Relocate --- .../commandclasses/IntakeAssistCommand.java | 15 +++++++-------- .../robot/constants/AutonomousConstants.java | 7 +------ .../trigon/robot/constants/OperatorConstants.java | 4 ++++ 3 files changed, 12 insertions(+), 14 deletions(-) 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 753798e1..b2688609 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -12,7 +12,6 @@ import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.commands.commandfactories.GeneralCommands; -import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; @@ -70,9 +69,9 @@ public static Translation2d calculateDistanceFromTrackedCoral() { public static Command getAssistIntakeCommand(Supplier distanceFromTrackedCoral) { return SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> AutonomousConstants.INTAKE_ASSIST_MODE.shouldAssistX ? getXAssistedPower(distanceFromTrackedCoral.get()) : getXJoystickPower(), - () -> AutonomousConstants.INTAKE_ASSIST_MODE.shouldAssistY ? getYAssistedPower(distanceFromTrackedCoral.get()) : getYJoystickPower(), - () -> AutonomousConstants.INTAKE_ASSIST_MODE.shouldAssistTheta ? getThetaAssistedPower(distanceFromTrackedCoral.get()) : OperatorConstants.DRIVER_CONTROLLER.getRightX() + () -> OperatorConstants.INTAKE_ASSIST_MODE.shouldAssistX ? getXAssistedPower(distanceFromTrackedCoral.get()) : getXJoystickPower(), + () -> OperatorConstants.INTAKE_ASSIST_MODE.shouldAssistY ? getYAssistedPower(distanceFromTrackedCoral.get()) : getYJoystickPower(), + () -> OperatorConstants.INTAKE_ASSIST_MODE.shouldAssistTheta ? getThetaAssistedPower(distanceFromTrackedCoral.get()) : OperatorConstants.DRIVER_CONTROLLER.getRightX() ); } @@ -116,15 +115,15 @@ private static double getYJoystickPower() { private static double calculateTranslationAssistPower(double distance, ProfiledPIDController pidController, double joystickValue) { final double - assistPower = pidController.calculate(distance) * AutonomousConstants.INTAKE_ASSIST_SCALAR, - stickPower = joystickValue * (1 - AutonomousConstants.INTAKE_ASSIST_SCALAR); + assistPower = pidController.calculate(distance) * OperatorConstants.INTAKE_ASSIST_SCALAR, + stickPower = joystickValue * (1 - OperatorConstants.INTAKE_ASSIST_SCALAR); return assistPower + stickPower; } private static double calculateRotationAssistPower(Rotation2d angleOffset) { final double - assistPower = THETA_PID_CONTROLLER.calculate(-angleOffset.getRadians()) * AutonomousConstants.INTAKE_ASSIST_SCALAR, - stickPower = OperatorConstants.DRIVER_CONTROLLER.getRightX() * (1 - AutonomousConstants.INTAKE_ASSIST_SCALAR); + assistPower = THETA_PID_CONTROLLER.calculate(-angleOffset.getRadians()) * OperatorConstants.INTAKE_ASSIST_SCALAR, + stickPower = OperatorConstants.DRIVER_CONTROLLER.getRightX() * (1 - OperatorConstants.INTAKE_ASSIST_SCALAR); return assistPower + stickPower; } diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index 66dd2eb4..fb3ea818 100644 --- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -10,7 +10,6 @@ import com.pathplanner.lib.pathfinding.Pathfinding; import edu.wpi.first.math.util.Units; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand; import frc.trigon.robot.commands.commandfactories.AutonomousCommands; import frc.trigon.robot.commands.commandfactories.CoralCollectionCommands; import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands; @@ -35,9 +34,7 @@ public class AutonomousConstants { public static final double MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR = 2.2; public static final String DEFAULT_AUTO_NAME = "FloorAutonomousRight6Branches"; public static final RobotConfig ROBOT_CONFIG = getRobotConfig(); - public static final double - FEEDFORWARD_SCALAR = 0.57, - INTAKE_ASSIST_SCALAR = 0.5; + public static final double FEEDFORWARD_SCALAR = 0.57; private static final PIDConstants AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? @@ -51,8 +48,6 @@ public class AutonomousConstants { AUTO_ROTATION_PID_CONSTANTS ); - public static final IntakeAssistCommand.AssistMode INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.FULL_ASSIST; - /** * Initializes PathPlanner. This needs to be called before any PathPlanner function can be used. */ diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index eb4aa4dd..0ddda9d1 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -4,6 +4,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand; import frc.trigon.robot.misc.ReefChooser; import org.trigon.hardware.misc.KeyboardController; import org.trigon.hardware.misc.XboxController; @@ -29,6 +30,9 @@ public class OperatorConstants { POV_DIVIDER = 2, ROTATION_STICK_SPEED_DIVIDER = 1; + public static final double INTAKE_ASSIST_SCALAR = 0.5; + public static final IntakeAssistCommand.AssistMode INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.FULL_ASSIST; + public static final Trigger LED_AUTO_SETUP_TRIGGER = OPERATOR_CONTROLLER.backtick(), RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.y(), From 60d13d85123e77828905e0e31160676f8f582b02 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 2 Jul 2025 12:36:35 +0300 Subject: [PATCH 42/65] Keep it like this for now --- .../java/frc/trigon/robot/constants/OperatorConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 0ddda9d1..f40da68d 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -30,8 +30,8 @@ public class OperatorConstants { POV_DIVIDER = 2, ROTATION_STICK_SPEED_DIVIDER = 1; - public static final double INTAKE_ASSIST_SCALAR = 0.5; - public static final IntakeAssistCommand.AssistMode INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.FULL_ASSIST; + public static final double INTAKE_ASSIST_SCALAR = 0.8; + public static final IntakeAssistCommand.AssistMode INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.ALIGN_ASSIST; public static final Trigger LED_AUTO_SETUP_TRIGGER = OPERATOR_CONTROLLER.backtick(), From 9d1d09d8d7df01b64ed1d43e841706d3246db253 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 3 Jul 2025 11:02:04 +0300 Subject: [PATCH 43/65] =?UTF-8?q?=F0=9F=91=BB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/trigon/robot/RobotContainer.java | 6 +++--- .../java/frc/trigon/robot/constants/OperatorConstants.java | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 896b7319..864bae00 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -100,7 +100,7 @@ private void bindControllerCommands() { OperatorConstants.RESET_AMP_ALIGNER_TRIGGER.whileTrue(AlgaeManipulationCommands.getResetAmpAlignerCommand()); - OperatorConstants.FLOOR_CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getFloorCoralCollectionCommand());//TODO: Add collection assist + OperatorConstants.FLOOR_CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getFloorCoralCollectionCommand()); OperatorConstants.FEEDER_CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getFeederCoralCollectionCommand()); OperatorConstants.RIGHT_SCORE_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(true)); OperatorConstants.LEFT_SCORE_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(false)); @@ -114,8 +114,8 @@ private void bindControllerCommands() { } private void bindSetters() { - OperatorConstants.ENABLE_IGNORE_LOLLIPOP_CORAL_TRIGGER.onTrue(CommandConstants.ENABLE_INTAKE_ASSIST_COMMAND); - OperatorConstants.DISABLE_IGNORE_LOLLIPOP_CORAL_TRIGGER.onTrue(CommandConstants.DISABLE_INTAKE_ASSIST_COMMAND); + OperatorConstants.ENABLE_INTAKE_ASSIST_TRIGGER.onTrue(CommandConstants.ENABLE_INTAKE_ASSIST_COMMAND); + OperatorConstants.DISABLE_INTAKE_ASSIST_TRIGGER.onTrue(CommandConstants.DISABLE_INTAKE_ASSIST_COMMAND); OperatorConstants.ENABLE_AUTONOMOUS_REEF_SCORING_TRIGGER.onTrue(CommandConstants.ENABLE_AUTONOMOUS_REEF_SCORING_COMMAND); OperatorConstants.DISABLE_AUTONOMOUS_REEF_SCORING_TRIGGER.onTrue(CommandConstants.DISABLE_AUTONOMOUS_REEF_SCORING_COMMAND); OperatorConstants.TOGGLE_SHOULD_KEEP_INTAKE_OPEN_TRIGGER.onTrue(CommandConstants.TOGGLE_SHOULD_KEEP_INTAKE_OPEN_COMMAND); diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index f40da68d..b51c7a02 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -59,8 +59,8 @@ public class OperatorConstants { SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.n().or(createScoreTrigger(DRIVER_CONTROLLER.rightStick().and(RobotContainer.ALGAE_MANIPULATOR::isOpen), true)), SCORE_ALGAE_IN_PROCESSOR_TRIGGER = OPERATOR_CONTROLLER.j().or(createScoreTrigger(DRIVER_CONTROLLER.leftStick().and(RobotContainer.ALGAE_MANIPULATOR::isOpen), false)); public static final Trigger - ENABLE_IGNORE_LOLLIPOP_CORAL_TRIGGER = OPERATOR_CONTROLLER.o(), - DISABLE_IGNORE_LOLLIPOP_CORAL_TRIGGER = OPERATOR_CONTROLLER.p(), + ENABLE_INTAKE_ASSIST_TRIGGER = OPERATOR_CONTROLLER.o(), + DISABLE_INTAKE_ASSIST_TRIGGER = OPERATOR_CONTROLLER.p(), ENABLE_AUTONOMOUS_REEF_SCORING_TRIGGER = OPERATOR_CONTROLLER.u(), DISABLE_AUTONOMOUS_REEF_SCORING_TRIGGER = OPERATOR_CONTROLLER.i(), TOGGLE_SHOULD_KEEP_INTAKE_OPEN_TRIGGER = DRIVER_CONTROLLER.b(); From d728e0c7399c3942593db4ba15ac13f94a7a99e8 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 23 Jul 2025 04:03:18 +0300 Subject: [PATCH 44/65] Still untested, but good idea(?) --- .../commandclasses/IntakeAssistCommand.java | 51 ++++++++++++------- .../CoralCollectionCommands.java | 2 +- .../robot/constants/OperatorConstants.java | 2 +- 3 files changed, 36 insertions(+), 19 deletions(-) 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 b2688609..c3dcffed 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -36,13 +36,13 @@ public class IntakeAssistCommand extends ParallelCommandGroup { new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)); private Translation2d distanceFromTrackedCoral; - public IntakeAssistCommand() { + public IntakeAssistCommand(AssistMode assistMode) { addCommands( new InstantCommand(CAMERA::initializeTracking), getTrackCoralCommand(), GeneralCommands.getContinuousConditionalCommand( GeneralCommands.getFieldRelativeDriveCommand(), - getAssistIntakeCommand(() -> distanceFromTrackedCoral), + getAssistIntakeCommand(assistMode, () -> distanceFromTrackedCoral), () -> CAMERA.getTrackedObjectFieldRelativePosition() == null ) ); @@ -67,24 +67,24 @@ public static Translation2d calculateDistanceFromTrackedCoral() { return robotToTrackedCoralDistance; } - public static Command getAssistIntakeCommand(Supplier distanceFromTrackedCoral) { + public static Command getAssistIntakeCommand(AssistMode assistMode, Supplier distanceFromTrackedCoral) { return SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> OperatorConstants.INTAKE_ASSIST_MODE.shouldAssistX ? getXAssistedPower(distanceFromTrackedCoral.get()) : getXJoystickPower(), - () -> OperatorConstants.INTAKE_ASSIST_MODE.shouldAssistY ? getYAssistedPower(distanceFromTrackedCoral.get()) : getYJoystickPower(), - () -> OperatorConstants.INTAKE_ASSIST_MODE.shouldAssistTheta ? getThetaAssistedPower(distanceFromTrackedCoral.get()) : OperatorConstants.DRIVER_CONTROLLER.getRightX() + () -> OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE.shouldAssistX ? getXAssistedPower(assistMode, distanceFromTrackedCoral.get()) : getXJoystickPower(), + () -> OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE.shouldAssistY ? getYAssistedPower(assistMode, distanceFromTrackedCoral.get()) : getYJoystickPower(), + () -> OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE.shouldAssistTheta ? getThetaAssistedPower(assistMode, distanceFromTrackedCoral.get()) : OperatorConstants.DRIVER_CONTROLLER.getRightX() ); } - private static double getXAssistedPower(Translation2d distanceFromTrackedCoral) { - return calculateTranslationAssistPower(distanceFromTrackedCoral.getX(), X_PID_CONTROLLER, getXJoystickPower()); + private static double getXAssistedPower(AssistMode assistMode, Translation2d distanceFromTrackedCoral) { + return calculateTranslationAssistPower(assistMode, distanceFromTrackedCoral.getX(), X_PID_CONTROLLER, getXJoystickPower()); } - private static double getYAssistedPower(Translation2d distanceFromTrackedCoral) { - return calculateTranslationAssistPower(distanceFromTrackedCoral.getY(), Y_PID_CONTROLLER, getYJoystickPower()); + private static double getYAssistedPower(AssistMode assistMode, Translation2d distanceFromTrackedCoral) { + return calculateTranslationAssistPower(assistMode, distanceFromTrackedCoral.getY(), Y_PID_CONTROLLER, getYJoystickPower()); } - private static double getThetaAssistedPower(Translation2d distanceFromTrackedCoral) { - return calculateRotationAssistPower(distanceFromTrackedCoral.getAngle().plus(Rotation2d.k180deg)); + private static double getThetaAssistedPower(AssistMode assistMode, Translation2d distanceFromTrackedCoral) { + return calculateRotationAssistPower(assistMode, distanceFromTrackedCoral.getAngle().plus(Rotation2d.k180deg)); } private static double getXJoystickPower() { @@ -113,21 +113,38 @@ private static double getYJoystickPower() { return (yValue * robotHeading.getCos()) - (xValue * robotHeading.getSin()); } - private static double calculateTranslationAssistPower(double distance, ProfiledPIDController pidController, double joystickValue) { + private static double calculateTranslationAssistPower(AssistMode assistMode, double distance, ProfiledPIDController pidController, double joystickValue) { + final double pidOutput = pidController.calculate(distance); + + if (assistMode.equals(AssistMode.ALTERNATE_ASSIST)) + return calculateAlternateAssistPower(pidOutput, joystickValue); + final double - assistPower = pidController.calculate(distance) * OperatorConstants.INTAKE_ASSIST_SCALAR, + assistPower = pidOutput * OperatorConstants.INTAKE_ASSIST_SCALAR, stickPower = joystickValue * (1 - OperatorConstants.INTAKE_ASSIST_SCALAR); return assistPower + stickPower; } - private static double calculateRotationAssistPower(Rotation2d angleOffset) { + private static double calculateRotationAssistPower(AssistMode assistMode, Rotation2d angleOffset) { + final double + pidOutput = THETA_PID_CONTROLLER.calculate(angleOffset.getRadians()), + joystickValue = OperatorConstants.DRIVER_CONTROLLER.getRightX(); + + if (assistMode.equals(AssistMode.ALTERNATE_ASSIST)) + return calculateAlternateAssistPower(pidOutput, joystickValue); + final double - assistPower = THETA_PID_CONTROLLER.calculate(-angleOffset.getRadians()) * OperatorConstants.INTAKE_ASSIST_SCALAR, - stickPower = OperatorConstants.DRIVER_CONTROLLER.getRightX() * (1 - OperatorConstants.INTAKE_ASSIST_SCALAR); + assistPower = pidOutput * OperatorConstants.INTAKE_ASSIST_SCALAR, + stickPower = joystickValue * (1 - OperatorConstants.INTAKE_ASSIST_SCALAR); return assistPower + stickPower; } + private static double calculateAlternateAssistPower(double pidOutput, double joystickValue) { + return pidOutput * (1 - Math.abs(joystickValue)) * Math.signum(joystickValue) + joystickValue; + } + public enum AssistMode { + ALTERNATE_ASSIST(true, true, true), FULL_ASSIST(true, true, true), ALIGN_ASSIST(false, true, true), ASSIST_ROTATION(false, false, 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 5bdd8cce..3cab36bd 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -26,7 +26,7 @@ public static Command getFloorCoralCollectionCommand() { return new ParallelCommandGroup( CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.COLLECT_FROM_FLOOR), getScheduleCoralLoadingWhenCollectedCommand(), - new IntakeAssistCommand().asProxy().onlyWhile(() -> SHOULD_ASSIST_INTAKE) + new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE).asProxy().onlyWhile(() -> SHOULD_ASSIST_INTAKE) ); } diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index b51c7a02..47ca6a4e 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -31,7 +31,7 @@ public class OperatorConstants { ROTATION_STICK_SPEED_DIVIDER = 1; public static final double INTAKE_ASSIST_SCALAR = 0.8; - public static final IntakeAssistCommand.AssistMode INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.ALIGN_ASSIST; + public static final IntakeAssistCommand.AssistMode DEFAULT_INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.ALIGN_ASSIST; public static final Trigger LED_AUTO_SETUP_TRIGGER = OPERATOR_CONTROLLER.backtick(), From a90161e20f9ceb4f4931836250f8edbe1cd7ea11 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 23 Jul 2025 14:57:39 +0300 Subject: [PATCH 45/65] =?UTF-8?q?=F0=9F=98=A7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/trigon/robot/commands/CommandConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index 4f921ad7..1ad13a71 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -27,7 +27,7 @@ public class CommandConstants { private static final XboxController DRIVER_CONTROLLER = OperatorConstants.DRIVER_CONTROLLER; private static final double MINIMUM_TRANSLATION_SHIFT_POWER = 0.30, - MINIMUM_ROTATION_SHIFT_POWER = 0.4; + MINIMUM_ROTATION_SHIFT_POWER = 0.4;//TODO: Calibrate with real robot private static final double JOYSTICK_ORIENTED_ROTATION_DEADBAND = 0.07; public static final Command From d77906ada860780bb30e6e80e432616e96a92a5a Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 23 Jul 2025 17:50:20 +0300 Subject: [PATCH 46/65] todo --- .../robot/commands/commandfactories/AutonomousCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index 2fe139c3..b7e8a2bf 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -97,7 +97,7 @@ public static Command getDriveToReefAndScoreCommand(FieldConstants.ReefClockPosi public static Command getDriveToReefWithoutHittingAlgaeCommand(FieldConstants.ReefClockPosition[] reefClockPositions) { return new SequentialCommandGroup( - new InstantCommand(() -> TARGET_SCORING_POSE = calculateClosestScoringPose(true, reefClockPositions, false)), + new InstantCommand(() -> TARGET_SCORING_POSE = calculateClosestScoringPose(true, reefClockPositions, false)),//TODO: Always stay behind bc of gripper aiming new WaitUntilCommand(() -> TARGET_SCORING_POSE != null).raceWith(SwerveCommands.getClosedLoopSelfRelativeDriveCommand(() -> 0, () -> 0, () -> 0)), SwerveCommands.getDriveToPoseCommand(() -> calculateClosestScoringPose(true, reefClockPositions, true), AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly().until(RobotContainer.ELEVATOR::isOverAlgaeHitRange), SwerveCommands.getDriveToPoseCommand(() -> TARGET_SCORING_POSE, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly() From d91a173b62e4842d260a51636795340d970d454d Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 31 Jul 2025 22:52:57 +0300 Subject: [PATCH 47/65] wowowiwa --- .../commandfactories/AlgaeManipulationCommands.java | 4 ++-- .../java/frc/trigon/robot/constants/OperatorConstants.java | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java index e0468bdf..fcd3fadc 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -86,7 +86,7 @@ private static Command getCollectAlgaeFromReefManuallyCommand() { return new ParallelCommandGroup( getGripAlgaeCommand(GripperConstants.GripperState.COLLECT_ALGAE_FROM_REEF), getOpenElevatorForAlgaeCommand() - ).until(() -> isScoreAlgaeButtonPressed() && RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.REST_WITH_ALGAE) && RobotContainer.GRIPPER.atState(GripperConstants.GripperState.HOLD_ALGAE) && RobotContainer.GRIPPER.isMovingSlowly()).andThen( + ).until(() -> isScoreAlgaeButtonPressed() && !RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.COLLECT_ALGAE_FROM_L3) && RobotContainer.GRIPPER.isMovingSlowly()).andThen( getScoreAlgaeCommand().asProxy() ); } @@ -147,7 +147,7 @@ private static Command getScoreInNetCommand() { () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), () -> new FlippableRotation2d(Rotation2d.k180deg, true) - ).asProxy() + ).asProxy().onlyWhile(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY) ); } diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 47ca6a4e..69b1ba21 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -66,9 +66,9 @@ public class OperatorConstants { TOGGLE_SHOULD_KEEP_INTAKE_OPEN_TRIGGER = DRIVER_CONTROLLER.b(); public static final Trigger SET_TARGET_SCORING_REEF_LEVEL_L1_FROM_GRIPPER_TRIGGER = OPERATOR_CONTROLLER.numpadDecimal(), - SET_TARGET_SCORING_REEF_LEVEL_L1_FROM_CORAL_INTAKE_TRIGGER = OPERATOR_CONTROLLER.numpad0().or(DRIVER_CONTROLLER.povLeft()), - SET_TARGET_SCORING_REEF_LEVEL_L2_TRIGGER = OPERATOR_CONTROLLER.numpad1().or(DRIVER_CONTROLLER.povDown()), - SET_TARGET_SCORING_REEF_LEVEL_L3_TRIGGER = OPERATOR_CONTROLLER.numpad2().or(DRIVER_CONTROLLER.povRight()), + SET_TARGET_SCORING_REEF_LEVEL_L1_FROM_CORAL_INTAKE_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_CLOCK_POSITION_2_OCLOCK_TRIGGER = OPERATOR_CONTROLLER.numpad9(), SET_TARGET_SCORING_REEF_CLOCK_POSITION_4_OCLOCK_TRIGGER = OPERATOR_CONTROLLER.numpad6(), From ae6c84f98d16344a2642dc665f2bcaa477c6598e Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 6 Aug 2025 00:52:50 +0300 Subject: [PATCH 48/65] Small stuff --- src/main/java/frc/trigon/robot/RobotContainer.java | 2 +- .../commands/commandfactories/AlgaeManipulationCommands.java | 2 +- .../commands/commandfactories/CoralCollectionCommands.java | 4 ++-- .../trigon/robot/subsystems/elevator/ElevatorConstants.java | 3 +-- 4 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 864bae00..fe826c9b 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -101,7 +101,7 @@ private void bindControllerCommands() { OperatorConstants.RESET_AMP_ALIGNER_TRIGGER.whileTrue(AlgaeManipulationCommands.getResetAmpAlignerCommand()); OperatorConstants.FLOOR_CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getFloorCoralCollectionCommand()); - OperatorConstants.FEEDER_CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getFeederCoralCollectionCommand()); + OperatorConstants.FEEDER_CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getFeederCoralCollectionFromIntakeCommand()); OperatorConstants.RIGHT_SCORE_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(true)); OperatorConstants.LEFT_SCORE_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(false)); OperatorConstants.EJECT_CORAL_TRIGGER.whileTrue(EjectionCommands.getEjectCoralCommand()); diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java index fcd3fadc..59860854 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -162,7 +162,7 @@ private static Command getScoreInProcessorCommand() { () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), () -> new FlippableRotation2d(Rotation2d.fromDegrees(90), true) - ).asProxy() + ).asProxy().onlyWhile(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY) ).until(() -> RobotContainer.GRIPPER.atState(GripperConstants.GripperState.SCORE_ALGAE_IN_PROCESSOR) && !OperatorConstants.CONTINUE_TRIGGER.getAsBoolean()); } 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 3cab36bd..57a5e99d 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -32,13 +32,13 @@ public static Command getFloorCoralCollectionCommand() { public static Command getFeederCoralCollectionCommand() { return new ConditionalCommand( - getInitiateFeederCoralCollectionCommand().unless(RobotContainer.GRIPPER::hasGamePiece), + getFeederCoralCollectionFromIntakeCommand().unless(RobotContainer.GRIPPER::hasGamePiece), getFeederCoralCollectionFromGripperCommand().asProxy(), () -> CoralCollectionCommands.isIntakeFacingFeeder() || RobotContainer.ALGAE_MANIPULATOR.isOpen() ); } - private static Command getInitiateFeederCoralCollectionCommand() { + public static Command getFeederCoralCollectionFromIntakeCommand() { return new ParallelCommandGroup( CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.COLLECT_FROM_FEEDER), getScheduleCoralLoadingWhenCollectedCommand() 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 84afd241..c4e9b707 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -79,8 +79,7 @@ public class ElevatorConstants { static final double GRIPPER_HITTING_ELEVATOR_BASE_LOWER_BOUND_POSITION_ROTATIONS = 0.29, GRIPPER_HITTING_ELEVATOR_BASE_UPPER_BOUND_POSITION_ROTATIONS = 0.7, - GRIPPER_HITTING_ALGAE_UPPER_BOUND_POSITION_ROTATIONS = 3, - QUICK_SCORE_NET_HEIGHT_LOWER_BOUND_POSITION_ROTATIONS = 3; + GRIPPER_HITTING_ALGAE_UPPER_BOUND_POSITION_ROTATIONS = 3; static { configureMasterMotor(); From 5f6e97e90ce6ffaea2cb355761157e5ff01b92de Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 6 Aug 2025 01:53:52 +0300 Subject: [PATCH 49/65] Probably better logic, totally better controls --- .../AlgaeManipulationCommands.java | 19 +++++++++++++------ .../robot/constants/OperatorConstants.java | 13 +++++++------ .../subsystems/gripper/GripperConstants.java | 1 - 3 files changed, 20 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java index 59860854..d971ce78 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -28,14 +28,19 @@ public class AlgaeManipulationCommands { private static final ReefChooser REEF_CHOOSER = OperatorConstants.REEF_CHOOSER; + public static boolean IS_HOLDING_ALGAE = false; public static Command getCollectAlgaeFromLollipopCommand() { return new SequentialCommandGroup( + new InstantCommand(() -> IS_HOLDING_ALGAE = true), CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.GRIPPER::hasGamePiece).asProxy(), getGripAlgaeCommand(GripperConstants.GripperState.COLLECT_ALGAE_FROM_LOLLIPOP).asProxy() .until(AlgaeManipulationCommands::isScoreAlgaeButtonPressed), getScoreAlgaeCommand().asProxy() - ).raceWith(getScoreNetEndingConditionCommand(), getScoreProcessorEndingConditionCommand()).andThen(AlgaeManipulationCommands::reloadAfterScore); + ) + .raceWith(getScoreNetEndingConditionCommand(), getScoreProcessorEndingConditionCommand()) + .andThen(AlgaeManipulationCommands::reloadAfterScore) + .finallyDo(() -> IS_HOLDING_ALGAE = false); } public static Command getCollectAlgaeFromReefCommand() { @@ -44,7 +49,9 @@ public static Command getCollectAlgaeFromReefCommand() { getCollectAlgaeFromReefManuallyCommand().asProxy() ) .alongWith(getAlignToReefCommand().onlyIf(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY).asProxy()) - .raceWith(getScoreNetEndingConditionCommand(), getScoreProcessorEndingConditionCommand()).andThen(AlgaeManipulationCommands::reloadAfterScore); + .raceWith(getScoreNetEndingConditionCommand(), getScoreProcessorEndingConditionCommand()) + .andThen(AlgaeManipulationCommands::reloadAfterScore) + .finallyDo(() -> IS_HOLDING_ALGAE = false); } public static Command getCollectAlgaeFromFloorCommand() { @@ -75,18 +82,18 @@ private static void reloadAfterScore() { } private static Command getScoreNetEndingConditionCommand() { - return new WaitUntilCommand(() -> RobotContainer.GRIPPER.atState(GripperConstants.GripperState.SCORE_ALGAE_IN_NET) || RobotContainer.GRIPPER.atState(GripperConstants.GripperState.QUICK_SCORE_ALGAE_IN_NET)).andThen(new WaitCommand(0.1)); + return new WaitUntilCommand(() -> RobotContainer.GRIPPER.atState(GripperConstants.GripperState.SCORE_ALGAE_IN_NET)).andThen(new WaitCommand(0.1)).andThen(new WaitUntilCommand(() -> !OperatorConstants.SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean())); } private static Command getScoreProcessorEndingConditionCommand() { - return new WaitUntilCommand(() -> RobotContainer.GRIPPER.atState(GripperConstants.GripperState.SCORE_ALGAE_IN_PROCESSOR) && !OperatorConstants.CONTINUE_TRIGGER.getAsBoolean()); + return new WaitUntilCommand(() -> RobotContainer.GRIPPER.atState(GripperConstants.GripperState.SCORE_ALGAE_IN_PROCESSOR)).andThen(new WaitCommand(0.1)).andThen(new WaitUntilCommand(() -> !OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean())); } private static Command getCollectAlgaeFromReefManuallyCommand() { return new ParallelCommandGroup( getGripAlgaeCommand(GripperConstants.GripperState.COLLECT_ALGAE_FROM_REEF), getOpenElevatorForAlgaeCommand() - ).until(() -> isScoreAlgaeButtonPressed() && !RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.COLLECT_ALGAE_FROM_L3) && RobotContainer.GRIPPER.isMovingSlowly()).andThen( + ).until(() -> isScoreAlgaeButtonPressed() && RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.REST_WITH_ALGAE) && RobotContainer.GRIPPER.atState(GripperConstants.GripperState.HOLD_ALGAE)).andThen( getScoreAlgaeCommand().asProxy() ); } @@ -179,7 +186,7 @@ private static Command getAlignToReefCommand() { ) ).raceWith( new WaitCommand(1).andThen(new WaitUntilCommand(RobotContainer.GRIPPER::isMovingSlowly)), - new WaitUntilCommand(OperatorConstants.CONTINUE_TRIGGER) + new WaitUntilCommand(OperatorConstants.STOP_ALGAE_AUTO_ALIGN_OVERRIDE_TRIGGER) ); } diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 69b1ba21..45607b6d 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -3,8 +3,8 @@ import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand; +import frc.trigon.robot.commands.commandfactories.AlgaeManipulationCommands; import frc.trigon.robot.misc.ReefChooser; import org.trigon.hardware.misc.KeyboardController; import org.trigon.hardware.misc.XboxController; @@ -47,17 +47,18 @@ public class OperatorConstants { RESET_AMP_ALIGNER_TRIGGER = OPERATOR_CONTROLLER.v(), FLOOR_CORAL_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftTrigger().or(OPERATOR_CONTROLLER.c()), FEEDER_CORAL_COLLECTION_TRIGGER = OPERATOR_CONTROLLER.f().or(DRIVER_CONTROLLER.start()), - RIGHT_SCORE_TRIGGER = OPERATOR_CONTROLLER.m().or(createScoreTrigger(DRIVER_CONTROLLER.rightStick().and(() -> !RobotContainer.ALGAE_MANIPULATOR.isOpen()), true)), - LEFT_SCORE_TRIGGER = OPERATOR_CONTROLLER.b().or(createScoreTrigger(DRIVER_CONTROLLER.leftStick().and(() -> !RobotContainer.ALGAE_MANIPULATOR.isOpen()), false)), - EJECT_CORAL_TRIGGER = OPERATOR_CONTROLLER.e().or(DRIVER_CONTROLLER.x()), + RIGHT_SCORE_TRIGGER = OPERATOR_CONTROLLER.m().or(createScoreTrigger(DRIVER_CONTROLLER.rightStick().and(() -> !AlgaeManipulationCommands.IS_HOLDING_ALGAE), true)), + LEFT_SCORE_TRIGGER = OPERATOR_CONTROLLER.b().or(createScoreTrigger(DRIVER_CONTROLLER.leftStick().and(() -> !AlgaeManipulationCommands.IS_HOLDING_ALGAE), false)), + EJECT_CORAL_TRIGGER = OPERATOR_CONTROLLER.e().or(DRIVER_CONTROLLER.x().and(() -> !AlgaeManipulationCommands.IS_HOLDING_ALGAE)), UNLOAD_CORAL_TRIGGER = OPERATOR_CONTROLLER.z().or(DRIVER_CONTROLLER.back()), COLLECT_ALGAE_FROM_REEF_TRIGGER = OPERATOR_CONTROLLER.a().or(DRIVER_CONTROLLER.rightBumper()), COLLECT_ALGAE_FROM_L3_OVERRIDE_TRIGGER = DRIVER_CONTROLLER.leftStick().or(DRIVER_CONTROLLER.rightStick()), COLLECT_ALGAE_FROM_LOLLIPOP_TRIGGER = OPERATOR_CONTROLLER.l().or(DRIVER_CONTROLLER.leftBumper()), COLLECT_ALGAE_FROM_FLOOR_TRIGGER = OPERATOR_CONTROLLER.s(), ROLL_ALGAE_ON_FLOOR_TRIGGER = OPERATOR_CONTROLLER.w(), - SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.n().or(createScoreTrigger(DRIVER_CONTROLLER.rightStick().and(RobotContainer.ALGAE_MANIPULATOR::isOpen), true)), - SCORE_ALGAE_IN_PROCESSOR_TRIGGER = OPERATOR_CONTROLLER.j().or(createScoreTrigger(DRIVER_CONTROLLER.leftStick().and(RobotContainer.ALGAE_MANIPULATOR::isOpen), false)); + SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.n().or(createScoreTrigger(DRIVER_CONTROLLER.rightStick().and(() -> AlgaeManipulationCommands.IS_HOLDING_ALGAE), true)), + SCORE_ALGAE_IN_PROCESSOR_TRIGGER = OPERATOR_CONTROLLER.j().or(createScoreTrigger(DRIVER_CONTROLLER.leftStick().and(() -> AlgaeManipulationCommands.IS_HOLDING_ALGAE), false)), + STOP_ALGAE_AUTO_ALIGN_OVERRIDE_TRIGGER = DRIVER_CONTROLLER.x().and(() -> AlgaeManipulationCommands.IS_HOLDING_ALGAE); public static final Trigger ENABLE_INTAKE_ASSIST_TRIGGER = OPERATOR_CONTROLLER.o(), DISABLE_INTAKE_ASSIST_TRIGGER = OPERATOR_CONTROLLER.p(), diff --git a/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java b/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java index de17d223..ca74f5cc 100644 --- a/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java @@ -247,7 +247,6 @@ public enum GripperState { COLLECT_ALGAE_FROM_REEF(Rotation2d.fromDegrees(30), -4, 1), COLLECT_ALGAE_FROM_LOLLIPOP(Rotation2d.fromDegrees(-35), COLLECT_ALGAE_FROM_REEF.targetGripperVoltage, 1), HOLD_ALGAE(Rotation2d.fromDegrees(40), -2, 0.3), - QUICK_SCORE_ALGAE_IN_NET(Rotation2d.fromDegrees(60), 11, 1), SCORE_ALGAE_IN_NET(Rotation2d.fromDegrees(60), 11, 1), PREPARE_FOR_SCORING_ALGAE_IN_NET(Rotation2d.fromDegrees(100), COLLECT_ALGAE_FROM_REEF.targetGripperVoltage, 0.3), SCORE_ALGAE_IN_PROCESSOR(Rotation2d.fromDegrees(-35), 11, 1), From 20b86a2ea496f804e5e9d5e0a78b9d5e5442da78 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 6 Aug 2025 21:04:41 +0300 Subject: [PATCH 50/65] Fix algae logic --- .../java/frc/trigon/robot/RobotContainer.java | 2 - .../AlgaeManipulationCommands.java | 40 ++++++++----------- .../robot/constants/OperatorConstants.java | 2 - 3 files changed, 16 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index fe826c9b..0e784ed8 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -109,8 +109,6 @@ private void bindControllerCommands() { OperatorConstants.COLLECT_ALGAE_FROM_REEF_TRIGGER.toggleOnTrue(AlgaeManipulationCommands.getCollectAlgaeFromReefCommand()); OperatorConstants.COLLECT_ALGAE_FROM_LOLLIPOP_TRIGGER.toggleOnTrue(AlgaeManipulationCommands.getCollectAlgaeFromLollipopCommand()); - OperatorConstants.COLLECT_ALGAE_FROM_FLOOR_TRIGGER.toggleOnTrue(AlgaeManipulationCommands.getCollectAlgaeFromFloorCommand()); - OperatorConstants.ROLL_ALGAE_ON_FLOOR_TRIGGER.toggleOnTrue(AlgaeManipulationCommands.getRollAlgaeOnFloorCommand()); } private void bindSetters() { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java index d971ce78..d58e835b 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -13,8 +13,6 @@ import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.misc.ReefChooser; import frc.trigon.robot.subsystems.algaemanipulator.AlgaeManipulatorCommands; -import frc.trigon.robot.subsystems.coralintake.CoralIntakeCommands; -import frc.trigon.robot.subsystems.coralintake.CoralIntakeConstants; import frc.trigon.robot.subsystems.elevator.ElevatorCommands; import frc.trigon.robot.subsystems.elevator.ElevatorConstants; import frc.trigon.robot.subsystems.gripper.GripperCommands; @@ -40,34 +38,20 @@ public static Command getCollectAlgaeFromLollipopCommand() { ) .raceWith(getScoreNetEndingConditionCommand(), getScoreProcessorEndingConditionCommand()) .andThen(AlgaeManipulationCommands::reloadAfterScore) - .finallyDo(() -> IS_HOLDING_ALGAE = false); + .finallyDo(AlgaeManipulationCommands::disableIsHoldingAlgae); } public static Command getCollectAlgaeFromReefCommand() { return new SequentialCommandGroup( + new InstantCommand(() -> IS_HOLDING_ALGAE = true), CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.GRIPPER::hasGamePiece).asProxy(), - getCollectAlgaeFromReefManuallyCommand().asProxy() + getCollectAlgaeFromReefManuallyCommand().asProxy(), + getScoreAlgaeCommand().asProxy() ) .alongWith(getAlignToReefCommand().onlyIf(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY).asProxy()) .raceWith(getScoreNetEndingConditionCommand(), getScoreProcessorEndingConditionCommand()) .andThen(AlgaeManipulationCommands::reloadAfterScore) - .finallyDo(() -> IS_HOLDING_ALGAE = false); - } - - public static Command getCollectAlgaeFromFloorCommand() { - return new SequentialCommandGroup( - CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.COLLECT_ALGAE_FROM_FLOOR) - .raceWith(new WaitCommand(1).andThen(new WaitUntilCommand(() -> RobotContainer.CORAL_INTAKE.isMovingSlowly() || OperatorConstants.CONTINUE_TRIGGER.getAsBoolean()))), - CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.PREPARE_SCORE_ALGAE_IN_PROCESSOR) - .raceWith(new WaitCommand(1).andThen(new WaitUntilCommand(OperatorConstants.CONTINUE_TRIGGER))), - CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.SCORE_ALGAE_IN_PROCESSOR) - .until(OperatorConstants.CONTINUE_TRIGGER.negate()) - ); - } - - public static Command getRollAlgaeOnFloorCommand() { - return CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.ROLL_ALGAE_ON_FLOOR) - .until(OperatorConstants.CONTINUE_TRIGGER); + .finallyDo(AlgaeManipulationCommands::disableIsHoldingAlgae); } public static Command getResetAmpAlignerCommand() { @@ -81,8 +65,12 @@ private static void reloadAfterScore() { new WaitUntilCommand(() -> RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.REST)).andThen(CoralCollectionCommands.getLoadCoralCommand().asProxy()).onlyIf(() -> RobotContainer.CORAL_INTAKE.hasGamePiece() && REEF_CHOOSER.getScoringLevel() != CoralPlacingCommands.ScoringLevel.L1_CORAL_INTAKE).schedule(); } + private static void disableIsHoldingAlgae() { + new WaitUntilCommand(() -> RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.REST)).andThen(() -> IS_HOLDING_ALGAE = false).schedule(); + } + private static Command getScoreNetEndingConditionCommand() { - return new WaitUntilCommand(() -> RobotContainer.GRIPPER.atState(GripperConstants.GripperState.SCORE_ALGAE_IN_NET)).andThen(new WaitCommand(0.1)).andThen(new WaitUntilCommand(() -> !OperatorConstants.SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean())); + return new WaitUntilCommand(() -> RobotContainer.GRIPPER.atState(GripperConstants.GripperState.SCORE_ALGAE_IN_NET)).andThen(new WaitCommand(0.1)); } private static Command getScoreProcessorEndingConditionCommand() { @@ -93,8 +81,12 @@ private static Command getCollectAlgaeFromReefManuallyCommand() { return new ParallelCommandGroup( getGripAlgaeCommand(GripperConstants.GripperState.COLLECT_ALGAE_FROM_REEF), getOpenElevatorForAlgaeCommand() - ).until(() -> isScoreAlgaeButtonPressed() && RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.REST_WITH_ALGAE) && RobotContainer.GRIPPER.atState(GripperConstants.GripperState.HOLD_ALGAE)).andThen( - getScoreAlgaeCommand().asProxy() + ).raceWith( + new SequentialCommandGroup( + new WaitCommand(1), + new WaitUntilCommand(RobotContainer.GRIPPER::isMovingSlowly), + new WaitUntilCommand(() -> !OperatorConstants.COLLECT_ALGAE_FROM_L3_OVERRIDE_TRIGGER.getAsBoolean()) + ) ); } diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 45607b6d..f3bc70b7 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -54,8 +54,6 @@ public class OperatorConstants { COLLECT_ALGAE_FROM_REEF_TRIGGER = OPERATOR_CONTROLLER.a().or(DRIVER_CONTROLLER.rightBumper()), COLLECT_ALGAE_FROM_L3_OVERRIDE_TRIGGER = DRIVER_CONTROLLER.leftStick().or(DRIVER_CONTROLLER.rightStick()), COLLECT_ALGAE_FROM_LOLLIPOP_TRIGGER = OPERATOR_CONTROLLER.l().or(DRIVER_CONTROLLER.leftBumper()), - COLLECT_ALGAE_FROM_FLOOR_TRIGGER = OPERATOR_CONTROLLER.s(), - ROLL_ALGAE_ON_FLOOR_TRIGGER = OPERATOR_CONTROLLER.w(), SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.n().or(createScoreTrigger(DRIVER_CONTROLLER.rightStick().and(() -> AlgaeManipulationCommands.IS_HOLDING_ALGAE), true)), SCORE_ALGAE_IN_PROCESSOR_TRIGGER = OPERATOR_CONTROLLER.j().or(createScoreTrigger(DRIVER_CONTROLLER.leftStick().and(() -> AlgaeManipulationCommands.IS_HOLDING_ALGAE), false)), STOP_ALGAE_AUTO_ALIGN_OVERRIDE_TRIGGER = DRIVER_CONTROLLER.x().and(() -> AlgaeManipulationCommands.IS_HOLDING_ALGAE); From 7a86ac387c99c4d91e13bafecef99d7cfd65a445 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 6 Aug 2025 21:08:24 +0300 Subject: [PATCH 51/65] =?UTF-8?q?Alternate=20assist=20=F0=9F=98=8E?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../commandclasses/IntakeAssistCommand.java | 34 ++++++++++--------- .../robot/constants/OperatorConstants.java | 2 +- 2 files changed, 19 insertions(+), 17 deletions(-) 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 c3dcffed..945157d7 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -1,5 +1,6 @@ package frc.trigon.robot.commands.commandclasses; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -69,25 +70,25 @@ public static Translation2d calculateDistanceFromTrackedCoral() { public static Command getAssistIntakeCommand(AssistMode assistMode, Supplier distanceFromTrackedCoral) { return SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE.shouldAssistX ? getXAssistedPower(assistMode, distanceFromTrackedCoral.get()) : getXJoystickPower(), - () -> OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE.shouldAssistY ? getYAssistedPower(assistMode, distanceFromTrackedCoral.get()) : getYJoystickPower(), + () -> OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE.shouldAssistX ? getXAssistedPower(assistMode, distanceFromTrackedCoral.get()) : getSelfRelativeXJoystickPower(), + () -> OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE.shouldAssistY ? getYAssistedPower(assistMode, distanceFromTrackedCoral.get()) : getSelfRelativeYJoystickPower(), () -> OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE.shouldAssistTheta ? getThetaAssistedPower(assistMode, distanceFromTrackedCoral.get()) : OperatorConstants.DRIVER_CONTROLLER.getRightX() ); } private static double getXAssistedPower(AssistMode assistMode, Translation2d distanceFromTrackedCoral) { - return calculateTranslationAssistPower(assistMode, distanceFromTrackedCoral.getX(), X_PID_CONTROLLER, getXJoystickPower()); + return calculateTranslationAssistPower(assistMode, distanceFromTrackedCoral.getX(), X_PID_CONTROLLER, getSelfRelativeXJoystickPower()); } private static double getYAssistedPower(AssistMode assistMode, Translation2d distanceFromTrackedCoral) { - return calculateTranslationAssistPower(assistMode, distanceFromTrackedCoral.getY(), Y_PID_CONTROLLER, getYJoystickPower()); + return calculateTranslationAssistPower(assistMode, distanceFromTrackedCoral.getY(), Y_PID_CONTROLLER, getSelfRelativeYJoystickPower()); } private static double getThetaAssistedPower(AssistMode assistMode, Translation2d distanceFromTrackedCoral) { return calculateRotationAssistPower(assistMode, distanceFromTrackedCoral.getAngle().plus(Rotation2d.k180deg)); } - private static double getXJoystickPower() { + private static double getSelfRelativeXJoystickPower() { final Rotation2d robotHeading = RobotContainer.SWERVE.getDriveRelativeAngle(); final double @@ -100,7 +101,7 @@ private static double getXJoystickPower() { return (xValue * robotHeading.getCos()) + (yValue * robotHeading.getSin()); } - private static double getYJoystickPower() { + private static double getSelfRelativeYJoystickPower() { final Rotation2d robotHeading = RobotContainer.SWERVE.getDriveRelativeAngle(); final double @@ -118,31 +119,32 @@ private static double calculateTranslationAssistPower(AssistMode assistMode, dou if (assistMode.equals(AssistMode.ALTERNATE_ASSIST)) return calculateAlternateAssistPower(pidOutput, joystickValue); - - final double - assistPower = pidOutput * OperatorConstants.INTAKE_ASSIST_SCALAR, - stickPower = joystickValue * (1 - OperatorConstants.INTAKE_ASSIST_SCALAR); - return assistPower + stickPower; + return calculateNormalAssistPower(pidOutput, joystickValue); } private static double calculateRotationAssistPower(AssistMode assistMode, Rotation2d angleOffset) { final double - pidOutput = THETA_PID_CONTROLLER.calculate(angleOffset.getRadians()), + pidOutput = MathUtil.clamp(THETA_PID_CONTROLLER.calculate(angleOffset.getRadians()), -1, 1), joystickValue = OperatorConstants.DRIVER_CONTROLLER.getRightX(); if (assistMode.equals(AssistMode.ALTERNATE_ASSIST)) return calculateAlternateAssistPower(pidOutput, joystickValue); + return calculateNormalAssistPower(pidOutput, joystickValue); + } + private static double calculateAlternateAssistPower(double pidOutput, double joystickValue) { + if (joystickValue == 0) + return pidOutput; + return pidOutput * (1 - Math.cbrt(joystickValue)) + joystickValue; + } + + private static double calculateNormalAssistPower(double pidOutput, double joystickValue) { final double assistPower = pidOutput * OperatorConstants.INTAKE_ASSIST_SCALAR, stickPower = joystickValue * (1 - OperatorConstants.INTAKE_ASSIST_SCALAR); return assistPower + stickPower; } - private static double calculateAlternateAssistPower(double pidOutput, double joystickValue) { - return pidOutput * (1 - Math.abs(joystickValue)) * Math.signum(joystickValue) + joystickValue; - } - public enum AssistMode { ALTERNATE_ASSIST(true, true, true), FULL_ASSIST(true, true, true), diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index f3bc70b7..ac99ab20 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -31,7 +31,7 @@ public class OperatorConstants { ROTATION_STICK_SPEED_DIVIDER = 1; public static final double INTAKE_ASSIST_SCALAR = 0.8; - public static final IntakeAssistCommand.AssistMode DEFAULT_INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.ALIGN_ASSIST; + public static final IntakeAssistCommand.AssistMode DEFAULT_INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.ALTERNATE_ASSIST; public static final Trigger LED_AUTO_SETUP_TRIGGER = OPERATOR_CONTROLLER.backtick(), From 189304b376d37d47285316112dbb9a88194c2f6c Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 7 Aug 2025 15:08:22 +0300 Subject: [PATCH 52/65] Fix errors --- .../commandclasses/IntakeAssistCommand.java | 19 +++++++------------ 1 file changed, 7 insertions(+), 12 deletions(-) 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 945157d7..159824db 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -7,7 +7,6 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import frc.trigon.robot.RobotContainer; @@ -16,7 +15,6 @@ import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; -import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; import org.littletonrobotics.junction.Logger; import org.trigon.hardware.RobotHardwareStats; @@ -39,31 +37,27 @@ public class IntakeAssistCommand extends ParallelCommandGroup { public IntakeAssistCommand(AssistMode assistMode) { addCommands( - new InstantCommand(CAMERA::initializeTracking), getTrackCoralCommand(), GeneralCommands.getContinuousConditionalCommand( GeneralCommands.getFieldRelativeDriveCommand(), getAssistIntakeCommand(assistMode, () -> distanceFromTrackedCoral), - () -> CAMERA.getTrackedObjectFieldRelativePosition() == null + () -> RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() == null ) ); } private Command getTrackCoralCommand() { - return new RunCommand(() -> { - CAMERA.trackObject(SimulatedGamePieceConstants.GamePieceType.CORAL); - distanceFromTrackedCoral = calculateDistanceFromTrackedCoral(); - }); + return new RunCommand(() -> distanceFromTrackedCoral = calculateDistanceFromTrackedCoral()); } public static Translation2d calculateDistanceFromTrackedCoral() { - final Pose2d robotPose = RobotContainer.POSE_ESTIMATOR.getEstimatedRobotPose(); - final Translation2d trackedObjectPositionOnField = CAMERA.getTrackedObjectFieldRelativePosition(); + final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + final Translation2d trackedObjectPositionOnField = RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot(); if (trackedObjectPositionOnField == null) return null; final Translation2d difference = robotPose.getTranslation().minus(trackedObjectPositionOnField); - Translation2d robotToTrackedCoralDistance = difference.rotateBy(robotPose.getRotation().unaryMinus()); + final Translation2d robotToTrackedCoralDistance = difference.rotateBy(robotPose.getRotation().unaryMinus()); Logger.recordOutput("IntakeAssist/TrackedCoralDistance", robotToTrackedCoralDistance); return robotToTrackedCoralDistance; } @@ -135,7 +129,8 @@ private static double calculateRotationAssistPower(AssistMode assistMode, Rotati private static double calculateAlternateAssistPower(double pidOutput, double joystickValue) { if (joystickValue == 0) return pidOutput; - return pidOutput * (1 - Math.cbrt(joystickValue)) + joystickValue; + final double joystickPower = Math.cbrt(joystickValue); + return pidOutput * (1 - joystickPower) + joystickPower; } private static double calculateNormalAssistPower(double pidOutput, double joystickValue) { From 9d7140c3a93b90890d1d9ef4df368f1262cac7c2 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 7 Aug 2025 20:42:43 +0300 Subject: [PATCH 53/65] Added jd and reworked alternate assist --- .../commandclasses/IntakeAssistCommand.java | 122 ++++++++++-------- 1 file changed, 69 insertions(+), 53 deletions(-) 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 159824db..0bc59297 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -10,19 +10,18 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.commands.commandfactories.GeneralCommands; -import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.OperatorConstants; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; import frc.trigon.robot.subsystems.swerve.SwerveCommands; import org.littletonrobotics.junction.Logger; import org.trigon.hardware.RobotHardwareStats; import java.util.function.Supplier; +/** + * A command class to assist in the intaking of a game piece. + */ public class IntakeAssistCommand extends ParallelCommandGroup { - private static final ObjectDetectionCamera CAMERA = CameraConstants.OBJECT_DETECTION_CAMERA; private static final ProfiledPIDController X_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : @@ -35,6 +34,11 @@ public class IntakeAssistCommand extends ParallelCommandGroup { new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)); private Translation2d distanceFromTrackedCoral; + /** + * Creates a new intake assist command of the given assist type. + * + * @param assistMode the type of assistance + */ public IntakeAssistCommand(AssistMode assistMode) { addCommands( getTrackCoralCommand(), @@ -46,11 +50,28 @@ public IntakeAssistCommand(AssistMode assistMode) { ); } + /** + * Returns a command that assists the intake of a game piece at the given location. + * + * @param assistMode the type of assistance + * @param distanceFromTrackedCoral the position of the game piece relative to the robot + * @return the command + */ + public static Command getAssistIntakeCommand(AssistMode assistMode, Supplier distanceFromTrackedCoral) { + final Translation2d translationPower = calculateTranslationPower(assistMode, distanceFromTrackedCoral.get()); + final double rotationPower = calculateRotationPower(assistMode, distanceFromTrackedCoral.get()); + return SwerveCommands.getClosedLoopSelfRelativeDriveCommand( + translationPower::getX, + translationPower::getY, + () -> rotationPower + ); + } + private Command getTrackCoralCommand() { return new RunCommand(() -> distanceFromTrackedCoral = calculateDistanceFromTrackedCoral()); } - public static Translation2d calculateDistanceFromTrackedCoral() { + private static Translation2d calculateDistanceFromTrackedCoral() { final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); final Translation2d trackedObjectPositionOnField = RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot(); if (trackedObjectPositionOnField == null) @@ -62,58 +83,43 @@ public static Translation2d calculateDistanceFromTrackedCoral() { return robotToTrackedCoralDistance; } - public static Command getAssistIntakeCommand(AssistMode assistMode, Supplier distanceFromTrackedCoral) { - return SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE.shouldAssistX ? getXAssistedPower(assistMode, distanceFromTrackedCoral.get()) : getSelfRelativeXJoystickPower(), - () -> OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE.shouldAssistY ? getYAssistedPower(assistMode, distanceFromTrackedCoral.get()) : getSelfRelativeYJoystickPower(), - () -> OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE.shouldAssistTheta ? getThetaAssistedPower(assistMode, distanceFromTrackedCoral.get()) : OperatorConstants.DRIVER_CONTROLLER.getRightX() - ); - } + private static Translation2d calculateTranslationPower(AssistMode assistMode, Translation2d distanceFromTrackedCoral) { + final Translation2d joystickPower = new Translation2d(OperatorConstants.DRIVER_CONTROLLER.getLeftY(), OperatorConstants.DRIVER_CONTROLLER.getLeftX()); + final Translation2d selfRelativeJoystickPower = joystickPower.rotateBy(RobotContainer.SWERVE.getDriveRelativeAngle()); - private static double getXAssistedPower(AssistMode assistMode, Translation2d distanceFromTrackedCoral) { - return calculateTranslationAssistPower(assistMode, distanceFromTrackedCoral.getX(), X_PID_CONTROLLER, getSelfRelativeXJoystickPower()); - } + final double xPIDOutput = X_PID_CONTROLLER.calculate(distanceFromTrackedCoral.getX()); + final double yPIDOutput = Y_PID_CONTROLLER.calculate(distanceFromTrackedCoral.getY()); - private static double getYAssistedPower(AssistMode assistMode, Translation2d distanceFromTrackedCoral) { - return calculateTranslationAssistPower(assistMode, distanceFromTrackedCoral.getY(), Y_PID_CONTROLLER, getSelfRelativeYJoystickPower()); + if (assistMode.equals(AssistMode.ALTERNATE_ASSIST)) + return calculateAlternateAssistTranslationPower(selfRelativeJoystickPower, xPIDOutput, yPIDOutput); + return calculateNormalAssistTranslationPower(assistMode, joystickPower, xPIDOutput, yPIDOutput); } - private static double getThetaAssistedPower(AssistMode assistMode, Translation2d distanceFromTrackedCoral) { + private static double calculateRotationPower(AssistMode assistMode, Translation2d distanceFromTrackedCoral) { return calculateRotationAssistPower(assistMode, distanceFromTrackedCoral.getAngle().plus(Rotation2d.k180deg)); } - private static double getSelfRelativeXJoystickPower() { - final Rotation2d robotHeading = RobotContainer.SWERVE.getDriveRelativeAngle(); - + private static Translation2d calculateAlternateAssistTranslationPower(Translation2d joystickValue, double xPIDOutput, double yPIDOutput) { + final double pidScalar = joystickValue.getNorm(); final double - joystickX = OperatorConstants.DRIVER_CONTROLLER.getLeftY(), - joystickY = OperatorConstants.DRIVER_CONTROLLER.getLeftX(); + xJoystickPower = Math.cbrt(joystickValue.getX()), + yJoystickPower = Math.cbrt(joystickValue.getY()); final double - xValue = CommandConstants.calculateDriveStickAxisValue(joystickX), - yValue = CommandConstants.calculateDriveStickAxisValue(joystickY); + xPower = calculateAlternateAssistPower(xPIDOutput, pidScalar, xJoystickPower), + yPower = calculateAlternateAssistPower(yPIDOutput, pidScalar, yJoystickPower); - return (xValue * robotHeading.getCos()) + (yValue * robotHeading.getSin()); + return new Translation2d(xPower, yPower); } - private static double getSelfRelativeYJoystickPower() { - final Rotation2d robotHeading = RobotContainer.SWERVE.getDriveRelativeAngle(); - + private static Translation2d calculateNormalAssistTranslationPower(AssistMode assistMode, Translation2d joystickValue, double xPIDOutput, double yPIDOutput) { final double - joystickX = OperatorConstants.DRIVER_CONTROLLER.getLeftY(), - joystickY = OperatorConstants.DRIVER_CONTROLLER.getLeftX(); + xJoystickPower = joystickValue.getX(), + yJoystickPower = joystickValue.getY(); final double - xValue = CommandConstants.calculateDriveStickAxisValue(joystickX), - yValue = CommandConstants.calculateDriveStickAxisValue(joystickY); + xPower = assistMode.shouldAssistX ? calculateNormalAssistPower(xPIDOutput, xJoystickPower) : xJoystickPower, + yPower = assistMode.shouldAssistY ? calculateNormalAssistPower(yPIDOutput, yJoystickPower) : yJoystickPower; - return (yValue * robotHeading.getCos()) - (xValue * robotHeading.getSin()); - } - - private static double calculateTranslationAssistPower(AssistMode assistMode, double distance, ProfiledPIDController pidController, double joystickValue) { - final double pidOutput = pidController.calculate(distance); - - if (assistMode.equals(AssistMode.ALTERNATE_ASSIST)) - return calculateAlternateAssistPower(pidOutput, joystickValue); - return calculateNormalAssistPower(pidOutput, joystickValue); + return new Translation2d(xPower, yPower); } private static double calculateRotationAssistPower(AssistMode assistMode, Rotation2d angleOffset) { @@ -122,28 +128,38 @@ private static double calculateRotationAssistPower(AssistMode assistMode, Rotati joystickValue = OperatorConstants.DRIVER_CONTROLLER.getRightX(); if (assistMode.equals(AssistMode.ALTERNATE_ASSIST)) - return calculateAlternateAssistPower(pidOutput, joystickValue); + return calculateAlternateAssistPower(pidOutput, joystickValue, joystickValue); return calculateNormalAssistPower(pidOutput, joystickValue); } - private static double calculateAlternateAssistPower(double pidOutput, double joystickValue) { - if (joystickValue == 0) - return pidOutput; - final double joystickPower = Math.cbrt(joystickValue); - return pidOutput * (1 - joystickPower) + joystickPower; + private static double calculateAlternateAssistPower(double pidOutput, double pidScalar, double joystickPower) { + return pidOutput * (1 - pidScalar) + joystickPower; } - private static double calculateNormalAssistPower(double pidOutput, double joystickValue) { - final double - assistPower = pidOutput * OperatorConstants.INTAKE_ASSIST_SCALAR, - stickPower = joystickValue * (1 - OperatorConstants.INTAKE_ASSIST_SCALAR); - return assistPower + stickPower; + private static double calculateNormalAssistPower(double pidOutput, double joystickPower) { + final double intakeAssistScalar = OperatorConstants.INTAKE_ASSIST_SCALAR; + return (pidOutput * intakeAssistScalar) + (joystickPower * (1 - intakeAssistScalar)); } + /** + * An enum containing different modes in which the command assists the intake of the game piece. + */ public enum AssistMode { + /** + * An alternate method for assisting the intake where the pid output is scaled down the more input the driver gives. + */ ALTERNATE_ASSIST(true, true, true), + /** + * Applies pid values to autonomously drive to the game piece, scaled by {@link OperatorConstants#INTAKE_ASSIST_SCALAR} in addition to the driver's inputs + */ FULL_ASSIST(true, true, true), + /** + * Applies pid values to align to the game piece, scaled by {@link OperatorConstants#INTAKE_ASSIST_SCALAR} in addition to the driver's inputs + */ ALIGN_ASSIST(false, true, true), + /** + * Applies pid values to face the game piece, scaled by {@link OperatorConstants#INTAKE_ASSIST_SCALAR} in addition to the driver's inputs + */ ASSIST_ROTATION(false, false, true); final boolean From 48efc238f68bae3c9bba256bd7f1ae6f6718474b Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 7 Aug 2025 20:47:19 +0300 Subject: [PATCH 54/65] Oops --- src/main/java/frc/trigon/robot/RobotContainer.java | 4 +--- .../commands/commandfactories/CoralCollectionCommands.java | 2 +- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 7ecff164..521402af 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -102,9 +102,7 @@ private void bindControllerCommands() { OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND); OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); OperatorConstants.DEBUGGING_TRIGGER.whileTrue(CommandConstants.WHEEL_RADIUS_CHARACTERIZATION_COMMAND); - - OperatorConstants.OPERATOR_CONTROLLER.f3().whileTrue(new CoralAlignmentCommand()); - + OperatorConstants.RESET_AMP_ALIGNER_TRIGGER.whileTrue(AlgaeManipulationCommands.getResetAmpAlignerCommand()); OperatorConstants.FLOOR_CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getFloorCoralCollectionCommand()); 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 57a5e99d..ea6276ff 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -54,7 +54,7 @@ public static Command getFeederCoralCollectionFromGripperCommand() { } private static boolean isIntakeFacingFeeder() { - final Pose2d robotPose = new FlippablePose2d(RobotContainer.POSE_ESTIMATOR.getEstimatedRobotPose(), true).get(); + final Pose2d robotPose = new FlippablePose2d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(), true).get(); final Rotation2d robotHeading = robotPose.getRotation(), leftFeederAngle = FieldConstants.LEFT_FEEDER_ANGLE; From 3ddb93857aaae824873e0ea79ac0f9fe4ef1aadb Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sat, 9 Aug 2025 23:25:38 +0300 Subject: [PATCH 55/65] Update JD, rotation -> theta, clamping for everything --- .../commandclasses/IntakeAssistCommand.java | 22 ++++++++++++------- 1 file changed, 14 insertions(+), 8 deletions(-) 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 0bc59297..3bb76402 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -52,6 +52,8 @@ public IntakeAssistCommand(AssistMode assistMode) { /** * Returns a command that assists the intake of a game piece at the given location. + * 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 distanceFromTrackedCoral the position of the game piece relative to the robot @@ -59,11 +61,11 @@ public IntakeAssistCommand(AssistMode assistMode) { */ public static Command getAssistIntakeCommand(AssistMode assistMode, Supplier distanceFromTrackedCoral) { final Translation2d translationPower = calculateTranslationPower(assistMode, distanceFromTrackedCoral.get()); - final double rotationPower = calculateRotationPower(assistMode, distanceFromTrackedCoral.get()); + final double thetaPower = calculateThetaPower(assistMode, distanceFromTrackedCoral.get()); return SwerveCommands.getClosedLoopSelfRelativeDriveCommand( translationPower::getX, translationPower::getY, - () -> rotationPower + () -> thetaPower ); } @@ -87,16 +89,16 @@ private static Translation2d calculateTranslationPower(AssistMode assistMode, Tr final Translation2d joystickPower = new Translation2d(OperatorConstants.DRIVER_CONTROLLER.getLeftY(), OperatorConstants.DRIVER_CONTROLLER.getLeftX()); final Translation2d selfRelativeJoystickPower = joystickPower.rotateBy(RobotContainer.SWERVE.getDriveRelativeAngle()); - final double xPIDOutput = X_PID_CONTROLLER.calculate(distanceFromTrackedCoral.getX()); - final double yPIDOutput = Y_PID_CONTROLLER.calculate(distanceFromTrackedCoral.getY()); + final double xPIDOutput = clampToOutputRange(X_PID_CONTROLLER.calculate(distanceFromTrackedCoral.getX())); + final double yPIDOutput = clampToOutputRange(Y_PID_CONTROLLER.calculate(distanceFromTrackedCoral.getY())); if (assistMode.equals(AssistMode.ALTERNATE_ASSIST)) return calculateAlternateAssistTranslationPower(selfRelativeJoystickPower, xPIDOutput, yPIDOutput); return calculateNormalAssistTranslationPower(assistMode, joystickPower, xPIDOutput, yPIDOutput); } - private static double calculateRotationPower(AssistMode assistMode, Translation2d distanceFromTrackedCoral) { - return calculateRotationAssistPower(assistMode, distanceFromTrackedCoral.getAngle().plus(Rotation2d.k180deg)); + private static double calculateThetaPower(AssistMode assistMode, Translation2d distanceFromTrackedCoral) { + return calculateThetaAssistPower(assistMode, distanceFromTrackedCoral.getAngle().plus(Rotation2d.k180deg)); } private static Translation2d calculateAlternateAssistTranslationPower(Translation2d joystickValue, double xPIDOutput, double yPIDOutput) { @@ -122,9 +124,9 @@ private static Translation2d calculateNormalAssistTranslationPower(AssistMode as return new Translation2d(xPower, yPower); } - private static double calculateRotationAssistPower(AssistMode assistMode, Rotation2d angleOffset) { + private static double calculateThetaAssistPower(AssistMode assistMode, Rotation2d angleOffset) { final double - pidOutput = MathUtil.clamp(THETA_PID_CONTROLLER.calculate(angleOffset.getRadians()), -1, 1), + pidOutput = clampToOutputRange(THETA_PID_CONTROLLER.calculate(angleOffset.getRadians())), joystickValue = OperatorConstants.DRIVER_CONTROLLER.getRightX(); if (assistMode.equals(AssistMode.ALTERNATE_ASSIST)) @@ -132,6 +134,10 @@ private static double calculateRotationAssistPower(AssistMode assistMode, Rotati return calculateNormalAssistPower(pidOutput, joystickValue); } + private static double clampToOutputRange(double value) { + return MathUtil.clamp(value, -1, 1); + } + private static double calculateAlternateAssistPower(double pidOutput, double pidScalar, double joystickPower) { return pidOutput * (1 - pidScalar) + joystickPower; } From d704fba0c1a27c977d1f0426abdd1d44bff6e4aa Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 10 Aug 2025 00:42:54 +0300 Subject: [PATCH 56/65] Logic fix, but it's still errors when the piece is no longer visible --- .../commandclasses/IntakeAssistCommand.java | 35 +++++++++++-------- .../robot/constants/OperatorConstants.java | 2 +- 2 files changed, 21 insertions(+), 16 deletions(-) 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 3bb76402..010cab45 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -6,9 +6,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandfactories.GeneralCommands; import frc.trigon.robot.constants.OperatorConstants; @@ -45,7 +43,7 @@ public IntakeAssistCommand(AssistMode assistMode) { GeneralCommands.getContinuousConditionalCommand( GeneralCommands.getFieldRelativeDriveCommand(), getAssistIntakeCommand(assistMode, () -> distanceFromTrackedCoral), - () -> RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() == null + () -> RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() == null || distanceFromTrackedCoral == null ) ); } @@ -60,13 +58,14 @@ public IntakeAssistCommand(AssistMode assistMode) { * @return the command */ public static Command getAssistIntakeCommand(AssistMode assistMode, Supplier distanceFromTrackedCoral) { - final Translation2d translationPower = calculateTranslationPower(assistMode, distanceFromTrackedCoral.get()); - final double thetaPower = calculateThetaPower(assistMode, distanceFromTrackedCoral.get()); - return SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - translationPower::getX, - translationPower::getY, - () -> thetaPower - ); + final Supplier translationAssistPowerSupplier = () -> calculateTranslationPower(assistMode, distanceFromTrackedCoral.get()); + return new SequentialCommandGroup( + new InstantCommand(() -> resetPIDControllers(distanceFromTrackedCoral.get())), + SwerveCommands.getClosedLoopSelfRelativeDriveCommand( + () -> translationAssistPowerSupplier.get().getX(), + () -> translationAssistPowerSupplier.get().getY(), + () -> calculateThetaPower(assistMode, distanceFromTrackedCoral.get()) + )); } private Command getTrackCoralCommand() { @@ -87,14 +86,14 @@ private static Translation2d calculateDistanceFromTrackedCoral() { private static Translation2d calculateTranslationPower(AssistMode assistMode, Translation2d distanceFromTrackedCoral) { final Translation2d joystickPower = new Translation2d(OperatorConstants.DRIVER_CONTROLLER.getLeftY(), OperatorConstants.DRIVER_CONTROLLER.getLeftX()); - final Translation2d selfRelativeJoystickPower = joystickPower.rotateBy(RobotContainer.SWERVE.getDriveRelativeAngle()); + final Translation2d selfRelativeJoystickPower = joystickPower.rotateBy(RobotContainer.SWERVE.getDriveRelativeAngle().unaryMinus()); final double xPIDOutput = clampToOutputRange(X_PID_CONTROLLER.calculate(distanceFromTrackedCoral.getX())); final double yPIDOutput = clampToOutputRange(Y_PID_CONTROLLER.calculate(distanceFromTrackedCoral.getY())); if (assistMode.equals(AssistMode.ALTERNATE_ASSIST)) return calculateAlternateAssistTranslationPower(selfRelativeJoystickPower, xPIDOutput, yPIDOutput); - return calculateNormalAssistTranslationPower(assistMode, joystickPower, xPIDOutput, yPIDOutput); + return calculateNormalAssistTranslationPower(assistMode, selfRelativeJoystickPower, xPIDOutput, yPIDOutput); } private static double calculateThetaPower(AssistMode assistMode, Translation2d distanceFromTrackedCoral) { @@ -102,7 +101,7 @@ private static double calculateThetaPower(AssistMode assistMode, Translation2d d } private static Translation2d calculateAlternateAssistTranslationPower(Translation2d joystickValue, double xPIDOutput, double yPIDOutput) { - final double pidScalar = joystickValue.getNorm(); + final double pidScalar = Math.cbrt(joystickValue.getNorm()); final double xJoystickPower = Math.cbrt(joystickValue.getX()), yJoystickPower = Math.cbrt(joystickValue.getY()); @@ -126,7 +125,7 @@ private static Translation2d calculateNormalAssistTranslationPower(AssistMode as private static double calculateThetaAssistPower(AssistMode assistMode, Rotation2d angleOffset) { final double - pidOutput = clampToOutputRange(THETA_PID_CONTROLLER.calculate(angleOffset.getRadians())), + pidOutput = clampToOutputRange(THETA_PID_CONTROLLER.calculate(-angleOffset.getRadians())), joystickValue = OperatorConstants.DRIVER_CONTROLLER.getRightX(); if (assistMode.equals(AssistMode.ALTERNATE_ASSIST)) @@ -147,6 +146,12 @@ private static double calculateNormalAssistPower(double pidOutput, double joysti return (pidOutput * intakeAssistScalar) + (joystickPower * (1 - intakeAssistScalar)); } + private static void resetPIDControllers(Translation2d distanceFromTrackedCoral) { + X_PID_CONTROLLER.reset(distanceFromTrackedCoral.getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().vxMetersPerSecond); + Y_PID_CONTROLLER.reset(distanceFromTrackedCoral.getY(), RobotContainer.SWERVE.getSelfRelativeVelocity().vyMetersPerSecond); + THETA_PID_CONTROLLER.reset(distanceFromTrackedCoral.getAngle().getRadians(), RobotContainer.SWERVE.getSelfRelativeVelocity().omegaRadiansPerSecond); + } + /** * An enum containing different modes in which the command assists the intake of the game piece. */ diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index ac99ab20..a1ffa7d8 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -30,7 +30,7 @@ public class OperatorConstants { POV_DIVIDER = 2, ROTATION_STICK_SPEED_DIVIDER = 1; - public static final double INTAKE_ASSIST_SCALAR = 0.8; + public static final double INTAKE_ASSIST_SCALAR = 0.0; public static final IntakeAssistCommand.AssistMode DEFAULT_INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.ALTERNATE_ASSIST; public static final Trigger From 252581b30fe9d82f7c7cd8afeffc8cbd23368ef6 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 10 Aug 2025 00:57:12 +0300 Subject: [PATCH 57/65] Logic stuff I think --- .../robot/commands/commandclasses/IntakeAssistCommand.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 010cab45..5115a604 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -138,7 +138,7 @@ private static double clampToOutputRange(double value) { } private static double calculateAlternateAssistPower(double pidOutput, double pidScalar, double joystickPower) { - return pidOutput * (1 - pidScalar) + joystickPower; + return pidOutput * (1 - Math.abs(pidScalar)) + joystickPower; } private static double calculateNormalAssistPower(double pidOutput, double joystickPower) { From 36a298c70f164e526796d665bfb3ea8e33e08861 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 10 Aug 2025 01:20:46 +0300 Subject: [PATCH 58/65] Formatting --- .../robot/commands/commandclasses/IntakeAssistCommand.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 5115a604..3d1b21a0 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -65,7 +65,8 @@ public static Command getAssistIntakeCommand(AssistMode assistMode, Supplier translationAssistPowerSupplier.get().getX(), () -> translationAssistPowerSupplier.get().getY(), () -> calculateThetaPower(assistMode, distanceFromTrackedCoral.get()) - )); + ) + ); } private Command getTrackCoralCommand() { From 9d8915ce8985a41f00b9431d711bf7987dff7b98 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 10 Aug 2025 18:16:31 +0300 Subject: [PATCH 59/65] At least it doesn't crash --- .../commandclasses/IntakeAssistCommand.java | 18 ++++++++++-------- .../commandfactories/CoralPlacingCommands.java | 5 +---- 2 files changed, 11 insertions(+), 12 deletions(-) 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 3d1b21a0..e96b2523 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -28,7 +28,7 @@ public class IntakeAssistCommand extends ParallelCommandGroup { new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : new ProfiledPIDController(0.3, 0, 0.03, new TrapezoidProfile.Constraints(2.65, 5.5)), THETA_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? - new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : + new ProfiledPIDController(0.4, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)); private Translation2d distanceFromTrackedCoral; @@ -58,19 +58,21 @@ public IntakeAssistCommand(AssistMode assistMode) { * @return the command */ public static Command getAssistIntakeCommand(AssistMode assistMode, Supplier distanceFromTrackedCoral) { - final Supplier translationAssistPowerSupplier = () -> calculateTranslationPower(assistMode, distanceFromTrackedCoral.get()); return new SequentialCommandGroup( new InstantCommand(() -> resetPIDControllers(distanceFromTrackedCoral.get())), SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> translationAssistPowerSupplier.get().getX(), - () -> translationAssistPowerSupplier.get().getY(), + () -> calculateTranslationPower(assistMode, distanceFromTrackedCoral.get()).getX(), + () -> calculateTranslationPower(assistMode, distanceFromTrackedCoral.get()).getY(), () -> calculateThetaPower(assistMode, distanceFromTrackedCoral.get()) ) ); } private Command getTrackCoralCommand() { - return new RunCommand(() -> distanceFromTrackedCoral = calculateDistanceFromTrackedCoral()); + return new RunCommand(() -> { + if (RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null) + distanceFromTrackedCoral = calculateDistanceFromTrackedCoral(); + }); } private static Translation2d calculateDistanceFromTrackedCoral() { @@ -98,7 +100,7 @@ private static Translation2d calculateTranslationPower(AssistMode assistMode, Tr } private static double calculateThetaPower(AssistMode assistMode, Translation2d distanceFromTrackedCoral) { - return calculateThetaAssistPower(assistMode, distanceFromTrackedCoral.getAngle().plus(Rotation2d.k180deg)); + return calculateThetaAssistPower(assistMode, distanceFromTrackedCoral.getAngle().plus(Rotation2d.k180deg).unaryMinus()); } private static Translation2d calculateAlternateAssistTranslationPower(Translation2d joystickValue, double xPIDOutput, double yPIDOutput) { @@ -124,9 +126,9 @@ private static Translation2d calculateNormalAssistTranslationPower(AssistMode as return new Translation2d(xPower, yPower); } - private static double calculateThetaAssistPower(AssistMode assistMode, Rotation2d angleOffset) { + private static double calculateThetaAssistPower(AssistMode assistMode, Rotation2d thetaOffset) { final double - pidOutput = clampToOutputRange(THETA_PID_CONTROLLER.calculate(-angleOffset.getRadians())), + pidOutput = clampToOutputRange(THETA_PID_CONTROLLER.calculate(thetaOffset.getRadians())), joystickValue = OperatorConstants.DRIVER_CONTROLLER.getRightX(); if (assistMode.equals(AssistMode.ALTERNATE_ASSIST)) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java index 1cc04e82..71ab7f12 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -115,10 +115,7 @@ private static Command scoreFromGripperReefChooserCommand() { private static Command scoreFromGripperInL4Command(boolean shouldScoreRight) { return new SequentialCommandGroup( GripperCommands.getPrepareForScoringInL4Command(REEF_CHOOSER::calculateTargetScoringPose).raceWith( - new SequentialCommandGroup( - new WaitUntilCommand(() -> canAutonomouslyReleaseFromGripper(shouldScoreRight)), - new WaitUntilChangeCommand<>(RobotContainer.ROBOT_POSE_ESTIMATOR::getEstimatedRobotPose) - ) + new WaitUntilCommand(() -> canAutonomouslyReleaseFromGripper(shouldScoreRight)) ).until(OperatorConstants.CONTINUE_TRIGGER), GripperCommands.getScoreInL4Command(REEF_CHOOSER::calculateTargetScoringPose).finallyDo(OperatorConstants.REEF_CHOOSER::switchReefSide) ); From 115a012e184c07e5d692da34d622ac16cc5203c2 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 10 Aug 2025 18:39:12 +0300 Subject: [PATCH 60/65] Log known object poses and some random stuff --- .../ObjectPoseEstimator.java | 34 ++++++++++++------- 1 file changed, 22 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java index b7532202..eacae1f3 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; +import org.littletonrobotics.junction.Logger; import java.util.ArrayList; import java.util.HashMap; @@ -41,6 +42,7 @@ public ObjectPoseEstimator(double deletionThresholdSeconds, DistanceCalculationM public void periodic() { updateObjectPositions(); removeOldObjects(); + Logger.recordOutput("ObjectPoseEstimator/knownObjectPositions", knownObjectPositions.keySet().toArray(Translation2d[]::new)); } /** @@ -57,7 +59,7 @@ public ArrayList getObjectsOnField() { */ public void removeClosestObjectToRobot() { final Translation2d closestObject = getClosestObjectToRobot(); - knownObjectPositions.remove(closestObject); + removeObject(closestObject); } /** @@ -67,7 +69,7 @@ public void removeClosestObjectToRobot() { */ public void removeClosestObjectToIntake(Transform2d intakeTransform) { final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); - knownObjectPositions.remove(getClosestObjectToPose(robotPose.transformBy(intakeTransform))); + removeObject(getClosestObjectToPosition(robotPose.transformBy(intakeTransform).getTranslation())); } /** @@ -76,8 +78,13 @@ public void removeClosestObjectToIntake(Transform2d intakeTransform) { * @param fieldRelativePose the pose to which the removed object is closest */ public void removeClosestObjectToPose(Pose2d fieldRelativePose) { - final Translation2d closestObject = getClosestObjectToPose(fieldRelativePose); - knownObjectPositions.remove(closestObject); + final Translation2d closestObject = getClosestObjectToPosition(fieldRelativePose.getTranslation()); + removeObject(closestObject); + } + + public void removeClosestObjectToPosition(Translation2d position) { + final Translation2d closestObject = getClosestObjectToPosition(position); + removeObject(closestObject); } /** @@ -98,27 +105,30 @@ public void removeObject(Translation2d objectPosition) { * @return the best object's 2D position on the field (z is assumed to be 0) */ public Translation2d getClosestObjectToRobot() { - return getClosestObjectToPose(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose()); + return getClosestObjectToPosition(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation()); } + /** - * Gets the position of the closest object to a given pose on the field. + * Gets the position of the closest object to a given position on the field. * - * @param pose the pose to which the returned object is closest + * @param position the position to which the returned object is closest * @return the closest object's position on the field, or null if no objects are known */ - public Translation2d getClosestObjectToPose(Pose2d pose) { + public Translation2d getClosestObjectToPosition(Translation2d position) { final Translation2d[] objectsTranslations = knownObjectPositions.keySet().toArray(Translation2d[]::new); if (knownObjectPositions.isEmpty()) return null; Translation2d bestObjectTranslation = objectsTranslations[0]; + double closestObjectDistance = position.getDistance(bestObjectTranslation); for (int i = 1; i < objectsTranslations.length; i++) { final Translation2d currentObjectTranslation = objectsTranslations[i]; - final double bestObjectRating = calculateObjectDistanceRating(bestObjectTranslation, pose); - final double currentObjectRating = calculateObjectDistanceRating(currentObjectTranslation, pose); - if (currentObjectRating < bestObjectRating) + final double currentObjectDistance = position.getDistance(currentObjectTranslation); + if (currentObjectDistance < closestObjectDistance) { + closestObjectDistance = currentObjectDistance; bestObjectTranslation = currentObjectTranslation; + } } return bestObjectTranslation; } @@ -156,7 +166,7 @@ private void updateObjectPositions() { } if (closestObjectToVisibleObjectDistanceMeters < ObjectDetectionCameraConstants.TRACKED_OBJECT_TOLERANCE_METERS && knownObjectPositions.get(closestObjectToVisibleObject) != currentTimestamp) - knownObjectPositions.remove(closestObjectToVisibleObject); + removeObject(closestObjectToVisibleObject); knownObjectPositions.put(visibleObject, currentTimestamp); } } From 185e66f1f65cc07d5ce5658116b3bd6f985afd02 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon, 11 Aug 2025 17:31:03 +0300 Subject: [PATCH 61/65] Biggest commit I've ever done probably --- 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 521402af..e7057038 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -102,7 +102,7 @@ private void bindControllerCommands() { OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND); OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); OperatorConstants.DEBUGGING_TRIGGER.whileTrue(CommandConstants.WHEEL_RADIUS_CHARACTERIZATION_COMMAND); - + OperatorConstants.RESET_AMP_ALIGNER_TRIGGER.whileTrue(AlgaeManipulationCommands.getResetAmpAlignerCommand()); OperatorConstants.FLOOR_CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getFloorCoralCollectionCommand()); From c72d6898e70f9176844ed6c4ca8b3de30683a8d4 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 13 Aug 2025 16:50:17 +0300 Subject: [PATCH 62/65] L1 stuff and refactor --- .../robot/misc/objectdetectioncamera/ObjectPoseEstimator.java | 4 +++- .../robot/subsystems/coralintake/CoralIntakeCommands.java | 4 +++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java index eacae1f3..12ee399d 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java @@ -26,7 +26,9 @@ public class ObjectPoseEstimator extends SubsystemBase { * @param gamePieceType the type of game piece to track * @param cameras the cameras used for detecting objects */ - public ObjectPoseEstimator(double deletionThresholdSeconds, DistanceCalculationMethod distanceCalculationMethod, SimulatedGamePieceConstants.GamePieceType gamePieceType, ObjectDetectionCamera... cameras) { + public ObjectPoseEstimator(double deletionThresholdSeconds, DistanceCalculationMethod distanceCalculationMethod, + SimulatedGamePieceConstants.GamePieceType gamePieceType, + ObjectDetectionCamera... cameras) { this.deletionThresholdSeconds = deletionThresholdSeconds; this.gamePieceType = gamePieceType; this.cameras = cameras; diff --git a/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeCommands.java b/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeCommands.java index 9ef743eb..754bd625 100644 --- a/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeCommands.java @@ -6,7 +6,9 @@ import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandfactories.CoralCollectionCommands; +import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands; import frc.trigon.robot.commands.commandfactories.GeneralCommands; +import frc.trigon.robot.constants.OperatorConstants; import org.trigon.commands.GearRatioCalculationCommand; import org.trigon.commands.NetworkTablesCommand; @@ -41,7 +43,7 @@ public static Command getCoralIntakeDefaultCommand() { return GeneralCommands.getContinuousConditionalCommand( getPrepareForStateCommand(CoralIntakeConstants.CoralIntakeState.COLLECT_FROM_FLOOR), getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.REST), - () -> CoralCollectionCommands.SHOULD_KEEP_INTAKE_OPEN + () -> CoralCollectionCommands.SHOULD_KEEP_INTAKE_OPEN && !OperatorConstants.REEF_CHOOSER.getScoringLevel().equals(CoralPlacingCommands.ScoringLevel.L1_CORAL_INTAKE) ); } From b25c29841ef9be6af997dbb45f51fb13474e8daa Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Thu, 21 Aug 2025 20:38:29 +0300 Subject: [PATCH 63/65] it works now? thingy thing --- .../objectdetectioncamera/ObjectDetectionCamera.java | 1 - .../objectdetectioncamera/ObjectPoseEstimator.java | 2 +- .../io/PhotonObjectDetectionCameraIO.java | 2 +- .../io/SimulationObjectDetectionCameraIO.java | 11 ++--------- 4 files changed, 4 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java index e7ee4c25..2d3c8228 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java @@ -99,7 +99,6 @@ private Translation2d calculateObjectPositionFromRotation(Rotation3d objectRotat if (robotPoseAtResultTimestamp == null) return new Translation2d(); final Pose3d cameraPose = new Pose3d(robotPoseAtResultTimestamp).plus(robotCenterToCamera); - objectRotation = new Rotation3d(objectRotation.getX(), -objectRotation.getY(), objectRotation.getZ()); final Pose3d objectRotationStart = cameraPose.plus(new Transform3d(0, 0, 0, objectRotation)); final double cameraZ = cameraPose.getTranslation().getZ(); diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java index 12ee399d..22c433f5 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java @@ -44,7 +44,7 @@ public ObjectPoseEstimator(double deletionThresholdSeconds, DistanceCalculationM public void periodic() { updateObjectPositions(); removeOldObjects(); - Logger.recordOutput("ObjectPoseEstimator/knownObjectPositions", knownObjectPositions.keySet().toArray(Translation2d[]::new)); + Logger.recordOutput("ObjectPoseEstimator/knownObjectPositions", getObjectsOnField().toArray(Translation2d[]::new)); } /** diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java index 88236d8f..541f10e8 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java @@ -78,7 +78,7 @@ private Rotation3d[] toArray(List list) { private Rotation3d extractRotation3d(PhotonTrackedTarget target) { return new Rotation3d( 0, - Units.degreesToRadians(target.getPitch()), + Units.degreesToRadians(-target.getPitch()), Units.degreesToRadians(-target.getYaw()) ); } 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 4c524b2a..5df04f9f 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 @@ -103,15 +103,8 @@ private Rotation3d calculateCameraAngleToObject(Pose3d objectPose, Pose3d camera final Translation3d difference = cameraPosition.minus(objectPosition); final Rotation3d differenceAsAngle = getAngle(difference); - return minus(differenceAsAngle, cameraPose.getRotation()); - } - private Rotation3d minus(Rotation3d rotation, Rotation3d other) { - return new Rotation3d( - 0, - rotation.getY() + other.getY(), - rotation.getZ() - other.getZ() - ); + return differenceAsAngle.minus(cameraPose.getRotation()); } private Rotation3d getAngle(Translation3d translation) { @@ -135,7 +128,7 @@ private Rotation2d getYaw(Translation3d vector) { * @return the pitch of the vector */ private Rotation2d getPitch(Translation3d vector) { - return new Rotation2d(-Math.atan2(vector.getZ(), Math.hypot(vector.getX(), vector.getY()))); + return new Rotation2d(Math.atan2(vector.getZ(), Math.hypot(vector.getX(), vector.getY()))); } /** From 3133608c202b218cdb548d784e51d0ea227a5dd2 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Thu, 21 Aug 2025 20:40:45 +0300 Subject: [PATCH 64/65] remove option for multiple cameras on O.P.E --- .../ObjectPoseEstimator.java | 42 +++++++++---------- 1 file changed, 20 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java index 22c433f5..64ea2cfe 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java @@ -14,30 +14,30 @@ public class ObjectPoseEstimator extends SubsystemBase { private final HashMap knownObjectPositions; - private final ObjectDetectionCamera[] cameras; + private final ObjectDetectionCamera camera; private final double deletionThresholdSeconds; private final SimulatedGamePieceConstants.GamePieceType gamePieceType; private final double rotationToTranslation; /** - * Constructs an ObjectPoseEstimator for estimating the positions of objects detected by cameras. + * Constructs an ObjectPoseEstimator for estimating the positions of objects detected by camera. * * @param deletionThresholdSeconds the time in seconds after which an object is considered old and removed * @param gamePieceType the type of game piece to track - * @param cameras the cameras used for detecting objects + * @param camera the camera used for detecting objects */ public ObjectPoseEstimator(double deletionThresholdSeconds, DistanceCalculationMethod distanceCalculationMethod, SimulatedGamePieceConstants.GamePieceType gamePieceType, - ObjectDetectionCamera... cameras) { + ObjectDetectionCamera camera) { this.deletionThresholdSeconds = deletionThresholdSeconds; this.gamePieceType = gamePieceType; - this.cameras = cameras; + this.camera = camera; this.knownObjectPositions = new HashMap<>(); this.rotationToTranslation = distanceCalculationMethod.rotationToTranslation; } /** - * Updates the object positions based on the cameras detected objects. + * Updates the object positions based on the camera detected objects. * Removes objects that have not been detected for {@link ObjectPoseEstimator#deletionThresholdSeconds}. */ @Override @@ -99,7 +99,7 @@ public void removeObject(Translation2d objectPosition) { } /** - * Gets the position of the closest object on the field from the 3D rotation of the object relative to the cameras. + * Gets the position of the closest object on the field from the 3D rotation of the object relative to the camera. * This assumes the object is on the ground. * Once it is known that the object is on the ground, * one can simply find the transform from the camera to the ground and apply it to the object's rotation. @@ -154,23 +154,21 @@ private double calculateObjectDistanceRating(Translation2d objectTranslation, Po private void updateObjectPositions() { final double currentTimestamp = Timer.getTimestamp(); - for (ObjectDetectionCamera camera : this.cameras) { - for (Translation2d visibleObject : camera.getObjectPositionsOnField(gamePieceType)) { - Translation2d closestObjectToVisibleObject = new Translation2d(); - double closestObjectToVisibleObjectDistanceMeters = Double.POSITIVE_INFINITY; - - for (Translation2d knownObject : knownObjectPositions.keySet()) { - final double currentObjectDistanceMeters = visibleObject.getDistance(knownObject); - if (currentObjectDistanceMeters < closestObjectToVisibleObjectDistanceMeters) { - closestObjectToVisibleObjectDistanceMeters = currentObjectDistanceMeters; - closestObjectToVisibleObject = knownObject; - } + for (Translation2d visibleObject : camera.getObjectPositionsOnField(gamePieceType)) { + Translation2d closestObjectToVisibleObject = new Translation2d(); + double closestObjectToVisibleObjectDistanceMeters = Double.POSITIVE_INFINITY; + + for (Translation2d knownObject : knownObjectPositions.keySet()) { + final double currentObjectDistanceMeters = visibleObject.getDistance(knownObject); + if (currentObjectDistanceMeters < closestObjectToVisibleObjectDistanceMeters) { + closestObjectToVisibleObjectDistanceMeters = currentObjectDistanceMeters; + closestObjectToVisibleObject = knownObject; } - - if (closestObjectToVisibleObjectDistanceMeters < ObjectDetectionCameraConstants.TRACKED_OBJECT_TOLERANCE_METERS && knownObjectPositions.get(closestObjectToVisibleObject) != currentTimestamp) - removeObject(closestObjectToVisibleObject); - knownObjectPositions.put(visibleObject, currentTimestamp); } + + if (closestObjectToVisibleObjectDistanceMeters < ObjectDetectionCameraConstants.TRACKED_OBJECT_TOLERANCE_METERS && knownObjectPositions.get(closestObjectToVisibleObject) != currentTimestamp) + removeObject(closestObjectToVisibleObject); + knownObjectPositions.put(visibleObject, currentTimestamp); } } From d7f2c16a1bddb93f51c8b979c6ac1a8491524c1f Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Fri, 22 Aug 2025 17:57:41 +0300 Subject: [PATCH 65/65] This seemes to have fixed it for some reason --- .../io/SimulationObjectDetectionCameraIO.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 5df04f9f..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 @@ -145,7 +145,7 @@ private boolean isWithinFOV(Rotation3d objectRotation) { private void logVisibleGamePieces(ArrayList>[] visibleGamePieces) { for (int i = 0; i < visibleGamePieces.length; i++) { final String gamePieceTypeName = SimulatedGamePieceConstants.GamePieceType.getNameFromID(i); - Logger.recordOutput(hostname + "/Visible" + gamePieceTypeName, mapSimulatedGamePieceListToPoseArray(visibleGamePieces[i])); + Logger.recordOutput(hostname + "/Visible" + gamePieceTypeName + "poses", mapSimulatedGamePieceListToPoseArray(visibleGamePieces[i])); } }