From 6458b1294c13f8e148a9436ebd2480b39259c29a Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Fri, 19 Sep 2025 01:32:12 +0300 Subject: [PATCH 01/51] added temporary algae collection logic. Need to test and need targetScoring level logic --- .../java/frc/trigon/robot/RobotContainer.java | 16 +- .../robot/commands/CommandConstants.java | 4 +- .../GamePieceAutoDriveCommand.java | 14 +- .../AlgaeManipulationCommands.java | 165 ++++++++++++++++++ .../CoralCollectionCommands.java | 2 +- ...onstants.java => AutonomousConstants.java} | 8 +- .../robot/constants/FieldConstants.java | 48 ++++- .../trigon/robot/constants/LEDConstants.java | 10 +- .../robot/constants/OperatorConstants.java | 7 + .../frc/trigon/robot/subsystems/arm/Arm.java | 8 +- .../robot/subsystems/arm/ArmConstants.java | 11 +- .../robot/subsystems/elevator/Elevator.java | 8 +- .../elevator/ElevatorConstants.java | 27 +-- .../robot/subsystems/swerve/Swerve.java | 14 +- .../subsystems/swerve/SwerveConstants.java | 8 +- .../swervemodule/SwerveModuleConstants.java | 4 +- 16 files changed, 299 insertions(+), 55 deletions(-) create mode 100644 src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.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 37fda07c..639f94d7 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -10,31 +10,29 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.commands.CommandConstants; +import frc.trigon.robot.commands.commandfactories.AlgaeManipulationCommands; import frc.trigon.robot.commands.commandfactories.CoralCollectionCommands; import frc.trigon.robot.commands.commandfactories.GeneralCommands; +import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.LEDConstants; import frc.trigon.robot.constants.OperatorConstants; -import frc.trigon.robot.constants.PathPlannerConstants; import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; - -import frc.trigon.robot.subsystems.elevator.Elevator; -import frc.trigon.robot.subsystems.elevator.ElevatorCommands; -import frc.trigon.robot.subsystems.elevator.ElevatorConstants; import frc.trigon.robot.subsystems.arm.Arm; import frc.trigon.robot.subsystems.arm.ArmCommands; import frc.trigon.robot.subsystems.arm.ArmConstants; - import frc.trigon.robot.subsystems.climber.Climber; import frc.trigon.robot.subsystems.climber.ClimberCommands; import frc.trigon.robot.subsystems.climber.ClimberConstants; +import frc.trigon.robot.subsystems.elevator.Elevator; +import frc.trigon.robot.subsystems.elevator.ElevatorCommands; +import frc.trigon.robot.subsystems.elevator.ElevatorConstants; import frc.trigon.robot.subsystems.intake.Intake; import frc.trigon.robot.subsystems.intake.IntakeCommands; import frc.trigon.robot.subsystems.intake.IntakeConstants; - import frc.trigon.robot.subsystems.swerve.Swerve; import frc.trigon.robot.subsystems.transporter.Transporter; import frc.trigon.robot.subsystems.transporter.TransporterCommands; @@ -94,7 +92,7 @@ private void bindDefaultCommands() { private void initializeGeneralSystems() { Flippable.init(); LEDConstants.init(); - PathPlannerConstants.init(); + AutonomousConstants.init(); } private void bindControllerCommands() { @@ -103,6 +101,8 @@ private void bindControllerCommands() { OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); OperatorConstants.CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getCoralCollectionCommand()); + + OperatorConstants.ALGAE_COLLECTION_TRIGGER.whileTrue(AlgaeManipulationCommands.getAlgaeCollectionCommandCommand()); } 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 dc45c903..c3173164 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -7,8 +7,8 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandfactories.GeneralCommands; +import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.constants.OperatorConstants; -import frc.trigon.robot.constants.PathPlannerConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; import lib.commands.CameraPositionCalculationCommand; import lib.commands.WheelRadiusCharacterizationCommand; @@ -45,7 +45,7 @@ public class CommandConstants { () -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX()) ), 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), diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java index e0663de3..578feadc 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java @@ -6,12 +6,12 @@ import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandfactories.GeneralCommands; -import frc.trigon.robot.constants.PathPlannerConstants; +import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; -import org.littletonrobotics.junction.Logger; import lib.utilities.flippable.FlippableRotation2d; +import org.littletonrobotics.junction.Logger; import java.util.function.Supplier; @@ -47,16 +47,16 @@ public static Translation2d calculateDistanceFromTrackedGamePiece() { final Translation2d robotToGamePiece = robotPose.getTranslation().minus(trackedObjectPositionOnField); var distanceFromTrackedGamePiece = robotToGamePiece.rotateBy(robotPose.getRotation().unaryMinus()); Logger.recordOutput("GamePieceAutoDrive/DistanceFromTrackedGamePiece", distanceFromTrackedGamePiece); - Logger.recordOutput("GamePieceAutoDrive/XDistanceFromTrackedGamePiece", PathPlannerConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.getSetpoint().position); + Logger.recordOutput("GamePieceAutoDrive/XDistanceFromTrackedGamePiece", AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.getSetpoint().position); return distanceFromTrackedGamePiece; } public static Command getDriveToGamePieceCommand(Supplier distanceFromTrackedGamePiece) { return new SequentialCommandGroup( - new InstantCommand(() -> PathPlannerConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.get().getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().vxMetersPerSecond)), + new InstantCommand(() -> AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.get().getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().vxMetersPerSecond)), SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> PathPlannerConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.get().getX()), - () -> PathPlannerConstants.GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.get().getY()), + () -> AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.get().getX()), + () -> AutonomousConstants.GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.get().getY()), GamePieceAutoDriveCommand::calculateTargetAngle ) ); @@ -64,7 +64,7 @@ public static Command getDriveToGamePieceCommand(Supplier distanc public static boolean shouldMoveTowardsGamePiece(Translation2d distanceFromTrackedGamePiece) { return distanceFromTrackedGamePiece != null && - (distanceFromTrackedGamePiece.getNorm() > PathPlannerConstants.AUTO_COLLECTION_INTAKE_OPEN_CHECK_DISTANCE_METERS);//TODO: If intake is open + (distanceFromTrackedGamePiece.getNorm() > AutonomousConstants.AUTO_COLLECTION_INTAKE_OPEN_CHECK_DISTANCE_METERS);//TODO: If intake is open } public static FlippableRotation2d calculateTargetAngle() { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java new file mode 100644 index 00000000..c570769f --- /dev/null +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -0,0 +1,165 @@ +package frc.trigon.robot.commands.commandfactories; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +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.AutonomousConstants; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.constants.LEDConstants; +import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.subsystems.arm.ArmCommands; +import frc.trigon.robot.subsystems.arm.ArmConstants; +import frc.trigon.robot.subsystems.elevator.ElevatorCommands; +import frc.trigon.robot.subsystems.elevator.ElevatorConstants; +import frc.trigon.robot.subsystems.swerve.SwerveCommands; +import lib.hardware.misc.leds.LEDCommands; +import lib.utilities.flippable.FlippablePose2d; +import lib.utilities.flippable.FlippableRotation2d; +import lib.utilities.flippable.FlippableTranslation2d; + +import java.util.Map; + +public class AlgaeManipulationCommands { + public static boolean IS_HOLDING_ALGAE = false; + + public static Command getAlgaeCollectionCommandCommand() { + return new SequentialCommandGroup( + getInitiateAlgaeCollectionCommand().until(RobotContainer.ARM::isEndEffectorMovingSlowly), + new InstantCommand(() -> getScoreAlgaeCommand().schedule()).alongWith(getAlgaeCollectionConfirmationCommand()) //TODO: add coral unloading if needed + ).alongWith(getAlignToReefCommand()).finallyDo(AlgaeManipulationCommands::disableIsHoldingAlgae); + } + + private static Command getAlignToReefCommand() { + return new SequentialCommandGroup( + SwerveCommands.getDriveToPoseCommand( + AlgaeManipulationCommands::calculateClosestAlgaeCollectionPose, + AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS + ), + SwerveCommands.getClosedLoopSelfRelativeDriveCommand( + () -> fieldRelativePowersToSelfRelativeXPower(OperatorConstants.DRIVER_CONTROLLER.getLeftY(), OperatorConstants.DRIVER_CONTROLLER.getLeftX()), + () -> 0, + () -> calculateClosestAlgaeCollectionPose().getRotation() + ) + ).raceWith( + new WaitCommand(1).andThen(new WaitUntilCommand(RobotContainer.ARM::isEndEffectorMovingSlowly)), + new WaitUntilCommand(OperatorConstants.STOP_ALGAE_AUTO_ALIGN_OVERRIDE_TRIGGER) + ); // TODO: .onlyIf(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY).asProxy(); + } + + private static Command getScoreAlgaeCommand() { + return new SelectCommand<>( + Map.of( + 0, getHoldAlgaeCommand(), + 1, getScoreInNetCommand(), + 2, getScoreInProcessorCommand() + ), + AlgaeManipulationCommands::getAlgaeScoreMethodSelector + ).raceWith(new WaitUntilChangeCommand<>(AlgaeManipulationCommands::isScoreAlgaeButtonPressed)).repeatedly(); + } + + private static Command getScoreInNetCommand() { + return new ParallelRaceGroup( + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_NET), + getArmScoringSequenceCommand(ArmConstants.ArmState.PREPARE_NET_SCORE, ArmConstants.ArmState.SCORE_NET), + SwerveCommands.getClosedLoopFieldRelativeDriveCommand( + () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), + () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), + () -> new FlippableRotation2d(Rotation2d.k180deg, true) + ).asProxy()//.onlyWhile(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY) + ); + } + + private static Command getScoreInProcessorCommand() { + return new ParallelRaceGroup( + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_PROCESSOR), + getArmScoringSequenceCommand(ArmConstants.ArmState.PREPARE_PROCESSOR_SCORE, ArmConstants.ArmState.SCORE_PROCESSOR), + SwerveCommands.getClosedLoopFieldRelativeDriveCommand( + () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), + () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), + () -> new FlippableRotation2d(Rotation2d.kCW_90deg, true) + ).asProxy()//.onlyWhile(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY) + ); + } + + private static Command getArmScoringSequenceCommand(ArmConstants.ArmState prepareState, ArmConstants.ArmState scoreState) { + return new SequentialCommandGroup( + ArmCommands.getSetTargetStateCommand(prepareState).until(OperatorConstants.CONTINUE_TRIGGER), + ArmCommands.getSetTargetStateCommand(scoreState) + ); + } + + private static int getAlgaeScoreMethodSelector() { + if (OperatorConstants.SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean()) + return 1; + else if (OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean()) + return 2; + return 0; + } + + private static Command getInitiateAlgaeCollectionCommand() { + return new ParallelCommandGroup( + ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.COLLECT_ALGAE_FROM_REEF), //TODO: make this depend on the target scoring level + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.COLLECT_ALGAE_FROM_L3) + ); + } + + private static Command getHoldAlgaeCommand() { + return new ParallelCommandGroup( + ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.HOLD_ALGAE), + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST_WITH_ALGAE) + ); + } + + private static Command getAlgaeCollectionConfirmationCommand() { + return new ParallelCommandGroup( + new InstantCommand(() -> { + OperatorConstants.DRIVER_CONTROLLER.rumble(OperatorConstants.RUMBLE_DURATION_SECONDS, OperatorConstants.RUMBLE_POWER); + IS_HOLDING_ALGAE = true; + }), + LEDCommands.getAnimateCommand(LEDConstants.ALGAE_COLLECTION_CONFIRMATION_ANIMATION_SETTINGS) //TODO: add LEDs + ); + } + + private static FlippablePose2d calculateClosestAlgaeCollectionPose() { + final Translation2d robotPositionOnField = RobotContainer.ROBOT_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 reefCenterToBranchScoringPose = new Transform2d(FieldConstants.REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS, 0, new Rotation2d()); + + double distanceFromClosestScoringPoseMeters = Double.POSITIVE_INFINITY; + Pose2d closestScoringPose = new Pose2d(); + for (final Rotation2d targetRotation : reefClockAngles) { + final Pose2d reefCenterAtTargetRotation = new Pose2d(reefCenterPosition, targetRotation); + final Pose2d currentScoringPose = reefCenterAtTargetRotation.transformBy(reefCenterToBranchScoringPose); + final double distanceFromCurrentScoringPoseMeters = currentScoringPose.getTranslation().getDistance(robotPositionOnField); + if (distanceFromCurrentScoringPoseMeters < distanceFromClosestScoringPoseMeters) { + distanceFromClosestScoringPoseMeters = distanceFromCurrentScoringPoseMeters; + closestScoringPose = currentScoringPose; + } + } + + return new FlippablePose2d(closestScoringPose, false); + } + + private static void disableIsHoldingAlgae() { + new WaitUntilCommand(() -> RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.REST)).andThen(() -> IS_HOLDING_ALGAE = false).schedule(); + } + + private static boolean isScoreAlgaeButtonPressed() { + return OperatorConstants.SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean() || + OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean(); + } + + private static double fieldRelativePowersToSelfRelativeXPower(double xPower, double yPower) { + final Rotation2d robotHeading = RobotContainer.SWERVE.getDriveRelativeAngle(); + final double xValue = CommandConstants.calculateDriveStickAxisValue(xPower); + final double yValue = CommandConstants.calculateDriveStickAxisValue(yPower); + + return (xValue * robotHeading.getCos()) + (yValue * robotHeading.getSin()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java index e3786616..35660fb9 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -46,7 +46,7 @@ private static Command getAlignCoralCommand() { private static Command getCollectionConfirmationCommand() { return new ParallelCommandGroup( new InstantCommand(() -> OperatorConstants.DRIVER_CONTROLLER.rumble(OperatorConstants.RUMBLE_DURATION_SECONDS, OperatorConstants.RUMBLE_POWER)), - LEDCommands.getAnimateCommand(LEDConstants.COLLECTION_CONFIRMATION_ANIMATION_SETTINGS) //TODO: add LEDs + LEDCommands.getAnimateCommand(LEDConstants.CORAL_COLLECTION_CONFIRMATION_ANIMATION_SETTINGS) //TODO: add LEDs ); } } \ 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/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 8b93566e..74e7cfe5 100644 --- a/src/main/java/frc/trigon/robot/constants/PathPlannerConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -5,25 +5,29 @@ import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.pathfinding.Pathfinding; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; import frc.trigon.robot.RobotContainer; -import org.json.simple.parser.ParseException; import lib.hardware.RobotHardwareStats; import lib.utilities.LocalADStarAK; import lib.utilities.flippable.Flippable; +import org.json.simple.parser.ParseException; import java.io.IOException; /** * 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 String DEFAULT_AUTO_NAME = "DefaultAutoName"; public static final RobotConfig ROBOT_CONFIG = getRobotConfig(); public static final double FEEDFORWARD_SCALAR = 0.5;//TODO: Calibrate + public static final PathConstraints DRIVE_TO_REEF_CONSTRAINTS = new PathConstraints(2.5, 4, Units.degreesToRadians(450), Units.degreesToRadians(900)); + private static final PIDConstants AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 9061418c..3ce58707 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -4,9 +4,8 @@ import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.*; +import lib.utilities.Conversions; import lib.utilities.FilesHandler; import java.io.IOException; @@ -21,6 +20,15 @@ public class FieldConstants { //Tags to ignore ); + public static final int REEF_CLOCK_POSITIONS = 6; + public static final Rotation2d REEF_CLOCK_POSITION_DIFFERENCE = Rotation2d.fromDegrees(Conversions.DEGREES_PER_ROTATIONS / REEF_CLOCK_POSITIONS); + public static final Rotation2d[] REEF_CLOCK_ANGLES = ReefClockPosition.getClockAngles(); + public static final Translation2d BLUE_REEF_CENTER_TRANSLATION = new Translation2d(4.48945, FIELD_WIDTH_METERS / 2); + public static final double + REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS = 1.3, + REEF_CENTER_TO_TARGET_SCORING_POSITION_Y_TRANSFORM_METERS = 0.17, + REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS = 1.6; + private static final boolean SHOULD_USE_HOME_TAG_LAYOUT = false; public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = createAprilTagFieldLayout(); private static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0)); @@ -44,4 +52,36 @@ private static HashMap fieldLayoutToTagIDToPoseMap() { return tagIDToPose; } -} + + + public enum ReefClockPosition { + REEF_12_OCLOCK(true), + REEF_2_OCLOCK(true), + REEF_4_OCLOCK(true), + REEF_6_OCLOCK(true), + REEF_8_OCLOCK(true), + REEF_10_OCLOCK(true); + + public final Rotation2d clockAngle; + public final boolean isFacingDriverStation; + public final int clockPosition; + + ReefClockPosition(boolean isFacingDriverStation) { + this.clockAngle = calculateClockAngle(); + this.isFacingDriverStation = isFacingDriverStation; + this.clockPosition = ordinal() == 0 ? 12 : ordinal() * 2; + } + + public static Rotation2d[] getClockAngles() { + final Rotation2d[] clockAngles = new Rotation2d[ReefClockPosition.values().length]; + for (int i = 0; i < clockAngles.length; i++) + clockAngles[i] = ReefClockPosition.values()[i].clockAngle; + + return clockAngles; + } + + private Rotation2d calculateClockAngle() { + return REEF_CLOCK_POSITION_DIFFERENCE.times(-ordinal()); + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/LEDConstants.java b/src/main/java/frc/trigon/robot/constants/LEDConstants.java index 3d137879..76d2c164 100644 --- a/src/main/java/frc/trigon/robot/constants/LEDConstants.java +++ b/src/main/java/frc/trigon/robot/constants/LEDConstants.java @@ -5,11 +5,17 @@ public class LEDConstants { //TODO: Implement LEDConstants - public static LEDStripAnimationSettings.ColorFlowSettings COLLECTION_CONFIRMATION_ANIMATION_SETTINGS = new LEDStripAnimationSettings.ColorFlowSettings( + public static LEDStripAnimationSettings.ColorFlowSettings + CORAL_COLLECTION_CONFIRMATION_ANIMATION_SETTINGS = new LEDStripAnimationSettings.ColorFlowSettings( Color.kGreen, 0.5, false - ); + ), + ALGAE_COLLECTION_CONFIRMATION_ANIMATION_SETTINGS = new LEDStripAnimationSettings.ColorFlowSettings( + Color.kPurple, + 0.5, + false + ); public static final LEDStripAnimationSettings.BlinkSettings CLIMB_ANIMATION_SETTINGS = new LEDStripAnimationSettings.BlinkSettings(Color.kYellow, 0.1);//TODO: Add fade animation /** diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index f7506110..7742692d 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand; +import frc.trigon.robot.commands.commandfactories.AlgaeManipulationCommands; import lib.hardware.misc.KeyboardController; import lib.hardware.misc.XboxController; @@ -39,4 +40,10 @@ public class OperatorConstants { FORWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.up(), BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.down(); public static final Trigger CORAL_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftTrigger().or(OPERATOR_CONTROLLER.c()); + public static final Trigger + SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.n().or((DRIVER_CONTROLLER.rightStick().and(() -> AlgaeManipulationCommands.IS_HOLDING_ALGAE))), + SCORE_ALGAE_IN_PROCESSOR_TRIGGER = OPERATOR_CONTROLLER.j().or((DRIVER_CONTROLLER.leftStick().and(() -> AlgaeManipulationCommands.IS_HOLDING_ALGAE))), + STOP_ALGAE_AUTO_ALIGN_OVERRIDE_TRIGGER = DRIVER_CONTROLLER.x().and(() -> AlgaeManipulationCommands.IS_HOLDING_ALGAE), + ALGAE_COLLECTION_TRIGGER = DRIVER_CONTROLLER.a().or(OPERATOR_CONTROLLER.a()); + } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index 1b14eb1a..4d7b4e31 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -11,6 +11,7 @@ import lib.hardware.phoenix6.cancoder.CANcoderSignal; import lib.hardware.phoenix6.talonfx.TalonFXMotor; import lib.hardware.phoenix6.talonfx.TalonFXSignal; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Arm extends MotorSubsystem { @@ -108,6 +109,11 @@ public boolean hasGamePiece() { return ArmConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); } + @AutoLogOutput + public boolean isEndEffectorMovingSlowly() { + return ArmConstants.IS_MOVING_SLOWLY_BOOLEAN_EVENT.getAsBoolean(); + } + void setTargetState(ArmConstants.ArmState targetState) { this.isStateReversed = false; this.targetState = targetState; @@ -142,7 +148,7 @@ void prepareForState(ArmConstants.ArmState targetState) { void prepareForState(ArmConstants.ArmState targetState, boolean isStateReversed) { this.isStateReversed = isStateReversed; this.targetState = targetState; - + if (isStateReversed) { setTargetAngle(ArmConstants.FULL_ROTATION.minus(targetState.targetAngle)); return; diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 0bdb08ef..4f724618 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -114,6 +114,13 @@ public class ArmConstants { new Rotation3d(0, 0, 0) ); + static final double + IS_MOVING_SLOWLY_VELOCITY_THRESHOLD_ROTATION_PER_SECOND = 4, + IS_MOVING_SLOWLY_DEBOUNCE_TIME_SECONDS = 0.1; + static final BooleanEvent IS_MOVING_SLOWLY_BOOLEAN_EVENT = new BooleanEvent( + CommandScheduler.getInstance().getActiveButtonLoop(), + () -> Math.abs(ARM_MASTER_MOTOR.getSignal(TalonFXSignal.VELOCITY)) < IS_MOVING_SLOWLY_VELOCITY_THRESHOLD_ROTATION_PER_SECOND || (RobotHardwareStats.isSimulation() && SimulationFieldHandler.isHoldingGamePiece()) //TODO isHoldingAlgae + ).debounce(IS_MOVING_SLOWLY_DEBOUNCE_TIME_SECONDS); static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(0); private static final double COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.2; static final BooleanEvent COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( @@ -243,8 +250,8 @@ public enum ArmState { SCORE_NET(Rotation2d.fromDegrees(160), 4), PREPARE_PROCESSOR_SCORE(Rotation2d.fromDegrees(90), HOLD_ALGAE.targetEndEffectorVoltage), SCORE_PROCESSOR(Rotation2d.fromDegrees(90), 4), - COLLECT_ALGAE_L2(Rotation2d.fromDegrees(90), -4), - COLLECT_ALGAE_L3(Rotation2d.fromDegrees(90), -4); + COLLECT_ALGAE_FROM_REEF(Rotation2d.fromDegrees(90), -4), + COLLECT_ALGAE_GROUND(Rotation2d.fromDegrees(90), -4); public final Rotation2d targetAngle; public final double targetEndEffectorVoltage; diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index 7063a1d5..a7d6a226 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -10,10 +10,10 @@ import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.subsystems.MotorSubsystem; -import org.littletonrobotics.junction.Logger; import lib.hardware.phoenix6.talonfx.TalonFXMotor; import lib.hardware.phoenix6.talonfx.TalonFXSignal; import lib.utilities.Conversions; +import org.littletonrobotics.junction.Logger; public class Elevator extends MotorSubsystem { private final TalonFXMotor masterMotor = ElevatorConstants.MASTER_MOTOR; @@ -23,7 +23,7 @@ public class Elevator extends MotorSubsystem { ElevatorConstants.DEFAULT_MAXIMUM_VELOCITY, ElevatorConstants.DEFAULT_MAXIMUM_ACCELERATION, ElevatorConstants.DEFAULT_MAXIMUM_ACCELERATION * 10 - ).withEnableFOC(ElevatorConstants.FOC_ENABLED); + ).withEnableFOC(ElevatorConstants.FOC_ENABLED); private ElevatorConstants.ElevatorState targetState = ElevatorConstants.ElevatorState.REST; public Elevator() { @@ -75,6 +75,10 @@ public void updatePeriodically() { Logger.recordOutput("Elevator/CurrentPositionMeters", getPositionMeters()); } + public boolean atState(ElevatorConstants.ElevatorState state) { + return Math.abs(getPositionMeters() - state.targetPositionMeters) < ElevatorConstants.TOLERANCE_METERS; + } + void setTargetState(ElevatorConstants.ElevatorState targetState) { this.targetState = targetState; scalePositionRequestSpeed(targetState.speedScalar); diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index b56f6d03..0c4c31c2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -2,7 +2,10 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.signals.*; +import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.signals.StaticFeedforwardSignValue; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; @@ -71,7 +74,7 @@ public class ElevatorConstants { new Rotation3d(0, 0, 0) ); static final Pose3d ELEVATOR_SECOND_STAGE_VISUALIZATION_ORIGIN_POINT = new Pose3d( - new Translation3d(0, -0.17 ,0.0814), + new Translation3d(0, -0.17, 0.0814), new Rotation3d(0, 0, 0) ); static final ElevatorMechanism2d MECHANISM = new ElevatorMechanism2d( @@ -87,9 +90,10 @@ public class ElevatorConstants { CommandScheduler.getInstance().getActiveButtonLoop(), REVERSE_LIMIT_SENSOR::getBinaryValue ).debounce(REVERSE_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS); - private static final DoubleSupplier REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER = () -> 0; + static final double TOLERANCE_METERS = 0.05; + static { configureMasterMotor(); configureFollowerMotor(); @@ -106,13 +110,13 @@ private static void configureMasterMotor() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; config.Feedback.SensorToMechanismRatio = GEAR_RATIO; - config.Slot0.kP = RobotHardwareStats.isSimulation() ? 3.5:0; - config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0:0; - config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.4:0; - config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016165:0; - config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.4766:0; - config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.014239:0; - config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.58202:0; + config.Slot0.kP = RobotHardwareStats.isSimulation() ? 3.5 : 0; + config.Slot0.kI = RobotHardwareStats.isSimulation() ? 0 : 0; + config.Slot0.kD = RobotHardwareStats.isSimulation() ? 0.4 : 0; + config.Slot0.kS = RobotHardwareStats.isSimulation() ? 0.016165 : 0; + config.Slot0.kV = RobotHardwareStats.isSimulation() ? 0.4766 : 0; + config.Slot0.kA = RobotHardwareStats.isSimulation() ? 0.014239 : 0; + config.Slot0.kG = RobotHardwareStats.isSimulation() ? 0.58202 : 0; config.Slot0.GravityType = GravityTypeValue.Elevator_Static; config.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseVelocitySign; @@ -171,7 +175,8 @@ public enum ElevatorState { COLLECT_ALGAE_FROM_L3(0.953, 1), COLLECT_ALGAE_FROM_GROUND(0, 0.7), REST_WITH_ALGAE(0.603, 0.3), - SCORE_NET(1.382, 0.3); + SCORE_NET(1.382, 0.3), + SCORE_PROCESSOR(0.605, 0.3); public final double targetPositionMeters; final double speedScalar; diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java index cad6cfb2..c9da8ae8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -13,13 +13,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.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; import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModuleConstants; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; import lib.hardware.RobotHardwareStats; import lib.hardware.phoenix6.Phoenix6SignalThread; import lib.hardware.phoenix6.pigeon2.Pigeon2Gyro; @@ -27,15 +25,17 @@ import lib.utilities.flippable.Flippable; import lib.utilities.flippable.FlippablePose2d; import lib.utilities.flippable.FlippableRotation2d; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; public class Swerve extends MotorSubsystem { private final Pigeon2Gyro gyro = SwerveConstants.GYRO; 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 7dd5c254..4e2e430b 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 f5e999c8..e1f0c6c7 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 lib.hardware.RobotHardwareStats; import lib.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 24d64e78408e21770c998bcf0fee6544b395a596 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Fri, 19 Sep 2025 01:36:46 +0300 Subject: [PATCH 02/51] Added algae ejection --- .../AlgaeManipulationCommands.java | 14 ++++++++++++-- .../trigon/robot/constants/OperatorConstants.java | 1 + .../trigon/robot/subsystems/arm/ArmConstants.java | 3 ++- 3 files changed, 15 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 c570769f..b75bd02a 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -56,7 +56,8 @@ private static Command getScoreAlgaeCommand() { Map.of( 0, getHoldAlgaeCommand(), 1, getScoreInNetCommand(), - 2, getScoreInProcessorCommand() + 2, getScoreInProcessorCommand(), + 3, getEjectAlgaeCommand() ), AlgaeManipulationCommands::getAlgaeScoreMethodSelector ).raceWith(new WaitUntilChangeCommand<>(AlgaeManipulationCommands::isScoreAlgaeButtonPressed)).repeatedly(); @@ -86,6 +87,13 @@ private static Command getScoreInProcessorCommand() { ); } + private static Command getEjectAlgaeCommand() { + return new ParallelCommandGroup( + ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.EJECT_ALGAE), + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST) + ).until(() -> !RobotContainer.ARM.hasGamePiece()); + } + private static Command getArmScoringSequenceCommand(ArmConstants.ArmState prepareState, ArmConstants.ArmState scoreState) { return new SequentialCommandGroup( ArmCommands.getSetTargetStateCommand(prepareState).until(OperatorConstants.CONTINUE_TRIGGER), @@ -96,8 +104,10 @@ private static Command getArmScoringSequenceCommand(ArmConstants.ArmState prepar private static int getAlgaeScoreMethodSelector() { if (OperatorConstants.SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean()) return 1; - else if (OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean()) + if (OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean()) return 2; + if (OperatorConstants.EJECT_ALGAE_TRIGGER.getAsBoolean()) + return 3; return 0; } diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 7742692d..27d7c228 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 { public static final Trigger SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.n().or((DRIVER_CONTROLLER.rightStick().and(() -> AlgaeManipulationCommands.IS_HOLDING_ALGAE))), SCORE_ALGAE_IN_PROCESSOR_TRIGGER = OPERATOR_CONTROLLER.j().or((DRIVER_CONTROLLER.leftStick().and(() -> AlgaeManipulationCommands.IS_HOLDING_ALGAE))), + EJECT_ALGAE_TRIGGER = OPERATOR_CONTROLLER.p().and(() -> AlgaeManipulationCommands.IS_HOLDING_ALGAE), STOP_ALGAE_AUTO_ALIGN_OVERRIDE_TRIGGER = DRIVER_CONTROLLER.x().and(() -> AlgaeManipulationCommands.IS_HOLDING_ALGAE), ALGAE_COLLECTION_TRIGGER = DRIVER_CONTROLLER.a().or(OPERATOR_CONTROLLER.a()); diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 4f724618..e98fda5c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -251,7 +251,8 @@ public enum ArmState { PREPARE_PROCESSOR_SCORE(Rotation2d.fromDegrees(90), HOLD_ALGAE.targetEndEffectorVoltage), SCORE_PROCESSOR(Rotation2d.fromDegrees(90), 4), COLLECT_ALGAE_FROM_REEF(Rotation2d.fromDegrees(90), -4), - COLLECT_ALGAE_GROUND(Rotation2d.fromDegrees(90), -4); + COLLECT_ALGAE_GROUND(Rotation2d.fromDegrees(90), -4), + EJECT_ALGAE(Rotation2d.fromDegrees(90), 4); public final Rotation2d targetAngle; public final double targetEndEffectorVoltage; From bf36334a31c8de622e84504aebe94a0392415f98 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Fri, 19 Sep 2025 02:37:12 +0300 Subject: [PATCH 03/51] removed unnecacery things --- .../AlgaeManipulationCommands.java | 18 +++++------------- .../robot/constants/OperatorConstants.java | 10 +++++----- .../frc/trigon/robot/subsystems/arm/Arm.java | 6 +----- .../robot/subsystems/arm/ArmConstants.java | 8 -------- 4 files changed, 11 insertions(+), 31 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 b75bd02a..0c28ab5d 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -25,13 +25,12 @@ import java.util.Map; public class AlgaeManipulationCommands { - public static boolean IS_HOLDING_ALGAE = false; public static Command getAlgaeCollectionCommandCommand() { return new SequentialCommandGroup( - getInitiateAlgaeCollectionCommand().until(RobotContainer.ARM::isEndEffectorMovingSlowly), + getInitiateAlgaeCollectionCommand().until(RobotContainer.ARM::hasGamePiece), new InstantCommand(() -> getScoreAlgaeCommand().schedule()).alongWith(getAlgaeCollectionConfirmationCommand()) //TODO: add coral unloading if needed - ).alongWith(getAlignToReefCommand()).finallyDo(AlgaeManipulationCommands::disableIsHoldingAlgae); + ).alongWith(getAlignToReefCommand()); } private static Command getAlignToReefCommand() { @@ -46,7 +45,7 @@ private static Command getAlignToReefCommand() { () -> calculateClosestAlgaeCollectionPose().getRotation() ) ).raceWith( - new WaitCommand(1).andThen(new WaitUntilCommand(RobotContainer.ARM::isEndEffectorMovingSlowly)), + new WaitCommand(1).andThen(new WaitUntilCommand(RobotContainer.ARM::hasGamePiece)), new WaitUntilCommand(OperatorConstants.STOP_ALGAE_AUTO_ALIGN_OVERRIDE_TRIGGER) ); // TODO: .onlyIf(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY).asProxy(); } @@ -60,7 +59,7 @@ private static Command getScoreAlgaeCommand() { 3, getEjectAlgaeCommand() ), AlgaeManipulationCommands::getAlgaeScoreMethodSelector - ).raceWith(new WaitUntilChangeCommand<>(AlgaeManipulationCommands::isScoreAlgaeButtonPressed)).repeatedly(); + ).raceWith(new WaitUntilChangeCommand<>(AlgaeManipulationCommands::isScoreAlgaeButtonPressed)).repeatedly().until(() -> !RobotContainer.ARM.hasGamePiece()); } private static Command getScoreInNetCommand() { @@ -127,10 +126,7 @@ private static Command getHoldAlgaeCommand() { private static Command getAlgaeCollectionConfirmationCommand() { return new ParallelCommandGroup( - new InstantCommand(() -> { - OperatorConstants.DRIVER_CONTROLLER.rumble(OperatorConstants.RUMBLE_DURATION_SECONDS, OperatorConstants.RUMBLE_POWER); - IS_HOLDING_ALGAE = true; - }), + new InstantCommand(() -> OperatorConstants.DRIVER_CONTROLLER.rumble(OperatorConstants.RUMBLE_DURATION_SECONDS, OperatorConstants.RUMBLE_POWER)), LEDCommands.getAnimateCommand(LEDConstants.ALGAE_COLLECTION_CONFIRMATION_ANIMATION_SETTINGS) //TODO: add LEDs ); } @@ -156,10 +152,6 @@ private static FlippablePose2d calculateClosestAlgaeCollectionPose() { return new FlippablePose2d(closestScoringPose, false); } - private static void disableIsHoldingAlgae() { - new WaitUntilCommand(() -> RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.REST)).andThen(() -> IS_HOLDING_ALGAE = false).schedule(); - } - private static boolean isScoreAlgaeButtonPressed() { return OperatorConstants.SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean() || OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_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 27d7c228..6c9d8147 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -2,8 +2,8 @@ import edu.wpi.first.wpilibj.RobotController; 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 lib.hardware.misc.KeyboardController; import lib.hardware.misc.XboxController; @@ -41,10 +41,10 @@ public class OperatorConstants { BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.down(); public static final Trigger CORAL_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftTrigger().or(OPERATOR_CONTROLLER.c()); public static final Trigger - SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.n().or((DRIVER_CONTROLLER.rightStick().and(() -> AlgaeManipulationCommands.IS_HOLDING_ALGAE))), - SCORE_ALGAE_IN_PROCESSOR_TRIGGER = OPERATOR_CONTROLLER.j().or((DRIVER_CONTROLLER.leftStick().and(() -> AlgaeManipulationCommands.IS_HOLDING_ALGAE))), - EJECT_ALGAE_TRIGGER = OPERATOR_CONTROLLER.p().and(() -> AlgaeManipulationCommands.IS_HOLDING_ALGAE), - STOP_ALGAE_AUTO_ALIGN_OVERRIDE_TRIGGER = DRIVER_CONTROLLER.x().and(() -> AlgaeManipulationCommands.IS_HOLDING_ALGAE), + SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.n().or((DRIVER_CONTROLLER.rightStick().and(RobotContainer.ARM::hasGamePiece))), + SCORE_ALGAE_IN_PROCESSOR_TRIGGER = OPERATOR_CONTROLLER.j().or((DRIVER_CONTROLLER.leftStick().and(RobotContainer.ARM::hasGamePiece))), + EJECT_ALGAE_TRIGGER = OPERATOR_CONTROLLER.p().and(RobotContainer.ARM::hasGamePiece), + STOP_ALGAE_AUTO_ALIGN_OVERRIDE_TRIGGER = DRIVER_CONTROLLER.x().and(RobotContainer.ARM::hasGamePiece), ALGAE_COLLECTION_TRIGGER = DRIVER_CONTROLLER.a().or(OPERATOR_CONTROLLER.a()); } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index 4d7b4e31..e39f2256 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -105,15 +105,11 @@ public boolean atTargetAngle() { return currentToTargetStateDifferenceDegrees < ArmConstants.ANGLE_TOLERANCE.getDegrees(); } + @AutoLogOutput(key = "Arm/HasGamePiece") public boolean hasGamePiece() { return ArmConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); } - @AutoLogOutput - public boolean isEndEffectorMovingSlowly() { - return ArmConstants.IS_MOVING_SLOWLY_BOOLEAN_EVENT.getAsBoolean(); - } - void setTargetState(ArmConstants.ArmState targetState) { this.isStateReversed = false; this.targetState = targetState; diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index e98fda5c..63f2d60d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -108,19 +108,11 @@ public class ArmConstants { "EndEffectorMechanism", END_EFFECTOR_MAXIMUM_DISPLAYABLE_VELOCITY ); - static final Pose3d ARM_VISUALIZATION_ORIGIN_POINT = new Pose3d( new Translation3d(0, 0.0954, 0.9517), new Rotation3d(0, 0, 0) ); - static final double - IS_MOVING_SLOWLY_VELOCITY_THRESHOLD_ROTATION_PER_SECOND = 4, - IS_MOVING_SLOWLY_DEBOUNCE_TIME_SECONDS = 0.1; - static final BooleanEvent IS_MOVING_SLOWLY_BOOLEAN_EVENT = new BooleanEvent( - CommandScheduler.getInstance().getActiveButtonLoop(), - () -> Math.abs(ARM_MASTER_MOTOR.getSignal(TalonFXSignal.VELOCITY)) < IS_MOVING_SLOWLY_VELOCITY_THRESHOLD_ROTATION_PER_SECOND || (RobotHardwareStats.isSimulation() && SimulationFieldHandler.isHoldingGamePiece()) //TODO isHoldingAlgae - ).debounce(IS_MOVING_SLOWLY_DEBOUNCE_TIME_SECONDS); static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(0); private static final double COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.2; static final BooleanEvent COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( From 6842f2ce497d5dfcb952d634eaa28fc12f6f56b8 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Sun, 21 Sep 2025 13:21:51 +0300 Subject: [PATCH 04/51] no errors --- .../commandfactories/AlgaeManipulationCommands.java | 8 ++++---- .../frc/trigon/robot/subsystems/arm/ArmConstants.java | 3 ++- .../robot/subsystems/elevator/ElevatorConstants.java | 3 ++- 3 files changed, 8 insertions(+), 6 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 0c28ab5d..4fb0a8d9 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -65,7 +65,7 @@ private static Command getScoreAlgaeCommand() { private static Command getScoreInNetCommand() { return new ParallelRaceGroup( ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_NET), - getArmScoringSequenceCommand(ArmConstants.ArmState.PREPARE_NET_SCORE, ArmConstants.ArmState.SCORE_NET), + getArmScoringSequenceCommand(ArmConstants.ArmState.SCORE_NET), SwerveCommands.getClosedLoopFieldRelativeDriveCommand( () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), @@ -77,7 +77,7 @@ private static Command getScoreInNetCommand() { private static Command getScoreInProcessorCommand() { return new ParallelRaceGroup( ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_PROCESSOR), - getArmScoringSequenceCommand(ArmConstants.ArmState.PREPARE_PROCESSOR_SCORE, ArmConstants.ArmState.SCORE_PROCESSOR), + getArmScoringSequenceCommand(ArmConstants.ArmState.SCORE_PROCESSOR), SwerveCommands.getClosedLoopFieldRelativeDriveCommand( () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), @@ -93,9 +93,9 @@ private static Command getEjectAlgaeCommand() { ).until(() -> !RobotContainer.ARM.hasGamePiece()); } - private static Command getArmScoringSequenceCommand(ArmConstants.ArmState prepareState, ArmConstants.ArmState scoreState) { + private static Command getArmScoringSequenceCommand(ArmConstants.ArmState scoreState) { return new SequentialCommandGroup( - ArmCommands.getSetTargetStateCommand(prepareState).until(OperatorConstants.CONTINUE_TRIGGER), + ArmCommands.getPrepareForStateCommand(scoreState).until(OperatorConstants.CONTINUE_TRIGGER), ArmCommands.getSetTargetStateCommand(scoreState) ); } diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 68d73fc4..277ec4b8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -244,7 +244,8 @@ public enum ArmState { SCORE_NET(Rotation2d.fromDegrees(160), Rotation2d.fromDegrees(160), 4), SCORE_PROCESSOR(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), 4), COLLECT_ALGAE_L2(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), -4), - COLLECT_ALGAE_L3(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), -4); + COLLECT_ALGAE_L3(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), -4), + EJECT_ALGAE(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), 4); public final Rotation2d targetAngle; public final Rotation2d prepareAngle; 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 35ff2c7a..c2db6d35 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -183,7 +183,8 @@ public enum ElevatorState { COLLECT_ALGAE_FROM_L3(0.953, 0.953, 1), COLLECT_ALGAE_FROM_GROUND(0, 0, 0.7), REST_WITH_ALGAE(0.603, 0.603, 0.3), - SCORE_NET(1.382, 1.382, 0.3); + SCORE_NET(1.382, 1.382, 0.3), + SCORE_PROCESSOR(1.182, 1.182, 0.3); public final double targetPositionMeters; public final double prepareStatePositionMeters; From ec764d90b42eb8fd676148fa08973eb79ea7ca2c Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Sun, 21 Sep 2025 20:25:03 +0300 Subject: [PATCH 05/51] Added algae collection states and unload coral command --- .../AlgaeManipulationCommands.java | 7 +-- .../CoralCollectionCommands.java | 7 +++ .../CoralPlacingCommands.java | 52 ++++++++++++++----- .../frc/trigon/robot/misc/ReefChooser.java | 18 +++++-- .../robot/subsystems/arm/ArmConstants.java | 3 ++ .../elevator/ElevatorConstants.java | 8 +-- 6 files changed, 71 insertions(+), 24 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 4fb0a8d9..956c7080 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -28,6 +28,7 @@ public class AlgaeManipulationCommands { public static Command getAlgaeCollectionCommandCommand() { return new SequentialCommandGroup( + CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.ARM::hasGamePiece), getInitiateAlgaeCollectionCommand().until(RobotContainer.ARM::hasGamePiece), new InstantCommand(() -> getScoreAlgaeCommand().schedule()).alongWith(getAlgaeCollectionConfirmationCommand()) //TODO: add coral unloading if needed ).alongWith(getAlignToReefCommand()); @@ -47,7 +48,7 @@ private static Command getAlignToReefCommand() { ).raceWith( new WaitCommand(1).andThen(new WaitUntilCommand(RobotContainer.ARM::hasGamePiece)), new WaitUntilCommand(OperatorConstants.STOP_ALGAE_AUTO_ALIGN_OVERRIDE_TRIGGER) - ); // TODO: .onlyIf(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY).asProxy(); + ).onlyIf(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY).asProxy(); } private static Command getScoreAlgaeCommand() { @@ -112,8 +113,8 @@ private static int getAlgaeScoreMethodSelector() { private static Command getInitiateAlgaeCollectionCommand() { return new ParallelCommandGroup( - ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.COLLECT_ALGAE_FROM_REEF), //TODO: make this depend on the target scoring level - ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.COLLECT_ALGAE_FROM_L3) + ArmCommands.getSetTargetStateCommand(CoralPlacingCommands.REEF_CHOOSER.getArmAlgaeCollectionState()), + ElevatorCommands.getSetTargetStateCommand(CoralPlacingCommands.REEF_CHOOSER.getElevatorAlgaeCollectionState()) ); } 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 54f530dd..48526b9b 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -26,6 +26,13 @@ public static Command getLoadCoralCommand() { ).until(RobotContainer.ARM::hasGamePiece); } + public static Command getUnloadCoralCommand() { + return new ParallelCommandGroup( + ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.UNLOAD_CORAL), + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.UNLOAD_CORAL) + ).until(() -> !RobotContainer.ARM.hasGamePiece()); + } + public static Command getCoralCollectionCommand() { return new ParallelCommandGroup( getIntakeSequenceCommand(), 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 a366475f..388bcf41 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -23,7 +23,7 @@ public class CoralPlacingCommands { public static boolean SHOULD_SCORE_AUTONOMOUSLY = true; - private static final ReefChooser REEF_CHOOSER = OperatorConstants.REEF_CHOOSER; + static final ReefChooser REEF_CHOOSER = OperatorConstants.REEF_CHOOSER; public static Command getScoreInReefCommand(boolean shouldScoreRight) { return new ConditionalCommand( @@ -37,8 +37,8 @@ private static Command getAutonomouslyScoreCommand(boolean shouldScoreRight) { return new SequentialCommandGroup( getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(() -> isArmAndElevatorAtPrepareState(shouldScoreRight)), new ParallelCommandGroup( - ElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getElevatorState), - ArmCommands.getSetTargetStateCommand(REEF_CHOOSER::getArmState, CoralPlacingCommands::shouldReverseScore) + ElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getElevatorCoralState), + ArmCommands.getSetTargetStateCommand(REEF_CHOOSER::getArmCoralState, CoralPlacingCommands::shouldReverseScore) ) ); } @@ -47,16 +47,16 @@ private static Command getScoreCommand(boolean shouldScoreRight) { return new SequentialCommandGroup( getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(OperatorConstants.CONTINUE_TRIGGER), new ParallelCommandGroup( - ElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getElevatorState), - ArmCommands.getSetTargetStateCommand(REEF_CHOOSER::getArmState, CoralPlacingCommands::shouldReverseScore) + ElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getElevatorCoralState), + ArmCommands.getSetTargetStateCommand(REEF_CHOOSER::getArmCoralState, CoralPlacingCommands::shouldReverseScore) ) ); } private static Command getAutonomouslyPrepareScoreCommand(boolean shouldScoreRight) { return new ParallelCommandGroup( - ElevatorCommands.getPrepareStateCommand(REEF_CHOOSER::getElevatorState), - ArmCommands.getPrepareForStateCommand(REEF_CHOOSER::getArmState, CoralPlacingCommands::shouldReverseScore), + ElevatorCommands.getPrepareStateCommand(REEF_CHOOSER::getElevatorCoralState), + ArmCommands.getPrepareForStateCommand(REEF_CHOOSER::getArmCoralState, CoralPlacingCommands::shouldReverseScore), getAutonomousDriveToReefThenManualDriveCommand(shouldScoreRight).asProxy() ); } @@ -134,8 +134,12 @@ public enum ScoringLevel { L3(L2.xTransformMeters, L2.positiveYTransformMeters, Rotation2d.fromDegrees(0)), L4(L2.xTransformMeters, L2.positiveYTransformMeters, Rotation2d.fromDegrees(0)); - public final ElevatorConstants.ElevatorState elevatorState; - public final ArmConstants.ArmState armState; + public final ElevatorConstants.ElevatorState + elevatorCoralState, + elevatorAlgaeCollectionState; + public final ArmConstants.ArmState + armCoralState, + armAlgaeCollectionState; public final int level = calculateLevel(); final double xTransformMeters, positiveYTransformMeters; final Rotation2d rotationTransform; @@ -153,8 +157,10 @@ public enum ScoringLevel { this.xTransformMeters = xTransformMeters; this.positiveYTransformMeters = positiveYTransformMeters; this.rotationTransform = rotationTransform; - this.elevatorState = determineElevatorState(); - this.armState = determineArmState(); + this.elevatorCoralState = determineElevatorCoralState(); + this.elevatorAlgaeCollectionState = determineElevatorAlgaeCollectionState(); + this.armCoralState = determineArmCoralState(); + this.armAlgaeCollectionState = determineArmAlgaeCollectionState(); } /** @@ -176,7 +182,7 @@ public FlippablePose2d calculateTargetPlacingPosition(FieldConstants.ReefClockPo return new FlippablePose2d(reefCenterPose.plus(transform), true); } - private ElevatorConstants.ElevatorState determineElevatorState() { + private ElevatorConstants.ElevatorState determineElevatorCoralState() { return switch (level) { case 1 -> ElevatorConstants.ElevatorState.SCORE_L1; case 2 -> ElevatorConstants.ElevatorState.SCORE_L2; @@ -186,7 +192,17 @@ private ElevatorConstants.ElevatorState determineElevatorState() { }; } - private ArmConstants.ArmState determineArmState() { + private ElevatorConstants.ElevatorState determineElevatorAlgaeCollectionState() { + return switch (level) { + case 1 -> ElevatorConstants.ElevatorState.COLLECT_ALGAE_GROUND; + case 2 -> ElevatorConstants.ElevatorState.COLLECT_ALGAE_L2; + case 3 -> ElevatorConstants.ElevatorState.COLLECT_ALGAE_L3; + case 4 -> ElevatorConstants.ElevatorState.COLLECT_ALGAE_LOLLIPOP; + default -> throw new IllegalStateException("Unexpected value: " + ordinal()); + }; + } + + private ArmConstants.ArmState determineArmCoralState() { return switch (level) { case 1 -> ArmConstants.ArmState.SCORE_L1; case 2 -> ArmConstants.ArmState.SCORE_L2; @@ -196,6 +212,16 @@ private ArmConstants.ArmState determineArmState() { }; } + private ArmConstants.ArmState determineArmAlgaeCollectionState() { + return switch (level) { + case 1 -> ArmConstants.ArmState.COLLECT_ALGAE_FLOOR; + case 2 -> ArmConstants.ArmState.COLLECT_ALGAE_L2; + case 3 -> ArmConstants.ArmState.COLLECT_ALGAE_L3; + case 4 -> ArmConstants.ArmState.COLLECT_ALGAE_LOLLIPOP; + default -> throw new IllegalStateException("Unexpected value: " + ordinal()); + }; + } + private int calculateLevel() { return ordinal() + 1; } diff --git a/src/main/java/frc/trigon/robot/misc/ReefChooser.java b/src/main/java/frc/trigon/robot/misc/ReefChooser.java index 9a185914..3819e02d 100644 --- a/src/main/java/frc/trigon/robot/misc/ReefChooser.java +++ b/src/main/java/frc/trigon/robot/misc/ReefChooser.java @@ -7,8 +7,8 @@ import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.constants.OperatorConstants; -import frc.trigon.robot.subsystems.elevator.ElevatorConstants; import frc.trigon.robot.subsystems.arm.ArmConstants; +import frc.trigon.robot.subsystems.elevator.ElevatorConstants; import lib.utilities.flippable.FlippablePose2d; import java.util.function.Supplier; @@ -45,12 +45,20 @@ public FlippablePose2d calculateTargetScoringPose() { return scoringLevel.calculateTargetPlacingPosition(clockPosition, reefSide); } - public ArmConstants.ArmState getArmState() { - return scoringLevel.armState; + public ArmConstants.ArmState getArmCoralState() { + return scoringLevel.armCoralState; + } + + public ArmConstants.ArmState getArmAlgaeCollectionState() { + return scoringLevel.armAlgaeCollectionState; + } + + public ElevatorConstants.ElevatorState getElevatorCoralState() { + return scoringLevel.elevatorCoralState; } - public ElevatorConstants.ElevatorState getElevatorState() { - return scoringLevel.elevatorState; + public ElevatorConstants.ElevatorState getElevatorAlgaeCollectionState() { + return scoringLevel.elevatorAlgaeCollectionState; } private void configureBindings() { diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 277ec4b8..8023b91f 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -235,6 +235,7 @@ public enum ArmState { REST_WITH_CORAL(Rotation2d.fromDegrees(180), Rotation2d.fromDegrees(180), 0), REST_FOR_CLIMB(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0), 0), LOAD_CORAL(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0), -4), + UNLOAD_CORAL(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0), 4), HOLD_ALGAE(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), -4), EJECT(Rotation2d.fromDegrees(60), Rotation2d.fromDegrees(60), 4), SCORE_L1(Rotation2d.fromDegrees(110), Rotation2d.fromDegrees(110), 4), @@ -243,8 +244,10 @@ public enum ArmState { SCORE_L4(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(100), 4), SCORE_NET(Rotation2d.fromDegrees(160), Rotation2d.fromDegrees(160), 4), SCORE_PROCESSOR(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), 4), + COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0), -4), COLLECT_ALGAE_L2(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), -4), COLLECT_ALGAE_L3(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), -4), + COLLECT_ALGAE_LOLLIPOP(Rotation2d.fromDegrees(45), Rotation2d.fromDegrees(45), -4), EJECT_ALGAE(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), 4); public final Rotation2d targetAngle; 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 c2db6d35..9b36bc30 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -175,13 +175,15 @@ private static void configureReverseLimitSensor() { public enum ElevatorState { REST(0.603, 0.603, 0.7), LOAD_CORAL(0.5519, 0.5519, 0.7), + UNLOAD_CORAL(0.5519, 0.5519, 0.7), SCORE_L1(0.1, 0.1, 1), SCORE_L2(0.3, 0.4, 1), SCORE_L3(0.7, 0.8, 1), SCORE_L4(1.2, 1.3, 1), - COLLECT_ALGAE_FROM_L2(0.603, 0.603, 1), - COLLECT_ALGAE_FROM_L3(0.953, 0.953, 1), - COLLECT_ALGAE_FROM_GROUND(0, 0, 0.7), + COLLECT_ALGAE_L2(0.603, 0.603, 1), + COLLECT_ALGAE_L3(0.953, 0.953, 1), + COLLECT_ALGAE_GROUND(0, 0, 1), + COLLECT_ALGAE_LOLLIPOP(0.3, 0.3, 1), REST_WITH_ALGAE(0.603, 0.603, 0.3), SCORE_NET(1.382, 1.382, 0.3), SCORE_PROCESSOR(1.182, 1.182, 0.3); From d88d93bf5ec5c0069b56d69edc6be6691056f532 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Sun, 21 Sep 2025 20:33:59 +0300 Subject: [PATCH 06/51] oops --- .../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 956c7080..c1ccb1ff 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -71,7 +71,7 @@ private static Command getScoreInNetCommand() { () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), () -> new FlippableRotation2d(Rotation2d.k180deg, true) - ).asProxy()//.onlyWhile(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY) + ).asProxy().onlyWhile(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY) ); } From 0f9d7db1270079f661c5a9bc1d4f6ca75124e880 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Sun, 21 Sep 2025 20:36:55 +0300 Subject: [PATCH 07/51] oops x2 --- .../commands/commandfactories/AlgaeManipulationCommands.java | 4 ++-- 1 file changed, 2 insertions(+), 2 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 c1ccb1ff..d15c057b 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -30,7 +30,7 @@ public static Command getAlgaeCollectionCommandCommand() { return new SequentialCommandGroup( CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.ARM::hasGamePiece), getInitiateAlgaeCollectionCommand().until(RobotContainer.ARM::hasGamePiece), - new InstantCommand(() -> getScoreAlgaeCommand().schedule()).alongWith(getAlgaeCollectionConfirmationCommand()) //TODO: add coral unloading if needed + new InstantCommand(() -> getScoreAlgaeCommand().schedule()).alongWith(getAlgaeCollectionConfirmationCommand()) ).alongWith(getAlignToReefCommand()); } @@ -83,7 +83,7 @@ private static Command getScoreInProcessorCommand() { () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), () -> new FlippableRotation2d(Rotation2d.kCW_90deg, true) - ).asProxy()//.onlyWhile(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY) + ).asProxy().onlyWhile(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY) ); } From d906247736ea31e9f010d0b571bb1acf562561ed Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Sun, 21 Sep 2025 21:31:27 +0300 Subject: [PATCH 08/51] added reverse scoring --- .../AlgaeManipulationCommands.java | 7 +++---- .../CoralPlacingCommands.java | 20 +++++++++---------- 2 files changed, 13 insertions(+), 14 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 d15c057b..c711581e 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -25,7 +25,6 @@ import java.util.Map; public class AlgaeManipulationCommands { - public static Command getAlgaeCollectionCommandCommand() { return new SequentialCommandGroup( CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.ARM::hasGamePiece), @@ -96,8 +95,8 @@ private static Command getEjectAlgaeCommand() { private static Command getArmScoringSequenceCommand(ArmConstants.ArmState scoreState) { return new SequentialCommandGroup( - ArmCommands.getPrepareForStateCommand(scoreState).until(OperatorConstants.CONTINUE_TRIGGER), - ArmCommands.getSetTargetStateCommand(scoreState) + ArmCommands.getPrepareForStateCommand(scoreState, CoralPlacingCommands.shouldReverseScore()).until(OperatorConstants.CONTINUE_TRIGGER), + ArmCommands.getSetTargetStateCommand(scoreState, CoralPlacingCommands.shouldReverseScore()) ); } @@ -150,7 +149,7 @@ private static FlippablePose2d calculateClosestAlgaeCollectionPose() { } } - return new FlippablePose2d(closestScoringPose, false); + return new FlippablePose2d(closestScoringPose.rotateBy(CoralPlacingCommands.shouldReverseScore() ? Rotation2d.k180deg : new Rotation2d()), false); } private static boolean isScoreAlgaeButtonPressed() { 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 388bcf41..63b28bac 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -33,6 +33,16 @@ public static Command getScoreInReefCommand(boolean shouldScoreRight) { ); } + static boolean shouldReverseScore() { + final Rotation2d robotRotation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation(); + final Translation2d robotTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); + final Translation2d reefCenterTranslation = FieldConstants.FLIPPABLE_REEF_CENTER_TRANSLATION.get(); + final Translation2d difference = reefCenterTranslation.minus(robotTranslation); + final Rotation2d robotRotationRelativeToReef = difference.getAngle(); + final Rotation2d robotRotationFacingReef = robotRotation.minus(robotRotationRelativeToReef); + return robotRotationFacingReef.getDegrees() > Rotation2d.kCW_90deg.getDegrees() && robotRotationFacingReef.getDegrees() < Rotation2d.kCCW_90deg.getDegrees(); + } + private static Command getAutonomouslyScoreCommand(boolean shouldScoreRight) { return new SequentialCommandGroup( getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(() -> isArmAndElevatorAtPrepareState(shouldScoreRight)), @@ -112,16 +122,6 @@ private static boolean isArmAndElevatorAtPrepareState(boolean shouldScoreRight) && RobotContainer.SWERVE.atPose(calculateClosestScoringPose(shouldScoreRight)); } - private static boolean shouldReverseScore() { - final Rotation2d robotRotation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation(); - final Translation2d robotTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); - final Translation2d reefCenterTranslation = FieldConstants.FLIPPABLE_REEF_CENTER_TRANSLATION.get(); - final Translation2d difference = reefCenterTranslation.minus(robotTranslation); - final Rotation2d robotRotationRelativeToReef = difference.getAngle(); - final Rotation2d robotRotationFacingReef = robotRotation.minus(robotRotationRelativeToReef); - return robotRotationFacingReef.getDegrees() > Rotation2d.kCW_90deg.getDegrees() && robotRotationFacingReef.getDegrees() < Rotation2d.kCCW_90deg.getDegrees(); - } - /** * An enum that represents the different levels of scoring in the reef. * Each level has a different x and y transform from the reef center, From 63e618f34fff367746a1525cf516999f6e92c362 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Sun, 21 Sep 2025 21:38:51 +0300 Subject: [PATCH 09/51] Update AlgaeManipulationCommands.java --- .../commands/commandfactories/AlgaeManipulationCommands.java | 3 ++- 1 file changed, 2 insertions(+), 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 c711581e..3e04538c 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -154,7 +154,8 @@ private static FlippablePose2d calculateClosestAlgaeCollectionPose() { private static boolean isScoreAlgaeButtonPressed() { return OperatorConstants.SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean() || - OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean(); + OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean() || + OperatorConstants.EJECT_ALGAE_TRIGGER.getAsBoolean(); } private static double fieldRelativePowersToSelfRelativeXPower(double xPower, double yPower) { From 47684bfa4367439afe52999bb72698d6ffef58e6 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 25 Sep 2025 11:46:08 +0300 Subject: [PATCH 10/51] Logic is logiccccccccccing --- .../java/frc/trigon/robot/RobotContainer.java | 14 +- .../AlgaeManipulationCommands.java | 148 ++++++++++++------ .../CoralCollectionCommands.java | 18 ++- .../CoralPlacingCommands.java | 19 +-- .../commandfactories/GeneralCommands.java | 13 ++ .../robot/constants/FieldConstants.java | 2 + .../robot/constants/OperatorConstants.java | 51 ++++-- .../elevator/ElevatorConstants.java | 2 +- 8 files changed, 181 insertions(+), 86 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 1710cba3..ecf39988 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -97,16 +97,20 @@ private void initializeGeneralSystems() { private void bindControllerCommands() { OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND); - OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); +// OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); - OperatorConstants.SPAWN_CORAL_TRIGGER.onTrue(new InstantCommand(() -> SimulationFieldHandler.updateCoralSpawning(new Pose3d(ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose())))); - OperatorConstants.CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getCoralCollectionCommand()); + OperatorConstants.FLOOR_ALGAE_COLLECTION_TRIGGER.toggleOnTrue(AlgaeManipulationCommands.getFloorAlgaeCollectionCommand()); + OperatorConstants.REEF_ALGAE_COLLECTION_TRIGGER.toggleOnTrue(AlgaeManipulationCommands.getReefAlgaeCollectionCommand()); - OperatorConstants.ALGAE_COLLECTION_TRIGGER.whileTrue(AlgaeManipulationCommands.getAlgaeCollectionCommandCommand()); - OperatorConstants.EJECT_CORAL_TRIGGER.whileTrue(CoralEjectionCommands.getCoralEjectionCommand()); + OperatorConstants.CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getCoralCollectionCommand()); OperatorConstants.SCORE_CORAL_LEFT_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(false)); OperatorConstants.SCORE_CORAL_RIGHT_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(true)); + OperatorConstants.EJECT_CORAL_TRIGGER.whileTrue(CoralEjectionCommands.getCoralEjectionCommand()); + + OperatorConstants.SPAWN_CORAL_IN_SIMULATION_TRIGGER.onTrue(new InstantCommand(() -> SimulationFieldHandler.updateCoralSpawning(new Pose3d(ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose())))); + OperatorConstants.FLIP_ARM_TRIGGER.onTrue(new InstantCommand(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE = !OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE)); + OperatorConstants.LOLLIPOP_ALGAE_TOGGLE_TRIGGER.onTrue(new InstantCommand(() -> AlgaeManipulationCommands.toggleLollipopCollection())); } private void configureSysIDBindings(MotorSubsystem subsystem) { 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 3e04538c..44c6643f 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -1,5 +1,6 @@ package frc.trigon.robot.commands.commandfactories; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; @@ -17,20 +18,50 @@ import frc.trigon.robot.subsystems.elevator.ElevatorCommands; import frc.trigon.robot.subsystems.elevator.ElevatorConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; +import lib.hardware.RobotHardwareStats; import lib.hardware.misc.leds.LEDCommands; import lib.utilities.flippable.FlippablePose2d; import lib.utilities.flippable.FlippableRotation2d; import lib.utilities.flippable.FlippableTranslation2d; +import java.util.List; import java.util.Map; public class AlgaeManipulationCommands { - public static Command getAlgaeCollectionCommandCommand() { + private static boolean + IS_HOLDING_ALGAE = false, + SHOULD_COLLECT_FROM_LOLLIPOP = false; + private static final PIDController ALIGN_TO_REEF_Y_CONTROLLER = + RobotHardwareStats.isSimulation() ? + new PIDController(3, 0, 0) : + new PIDController(3, 0, 0); + + public static boolean isHoldingAlgae() { + return IS_HOLDING_ALGAE; + } + + public static void toggleLollipopCollection() { + SHOULD_COLLECT_FROM_LOLLIPOP = !SHOULD_COLLECT_FROM_LOLLIPOP; + } + + public static Command getFloorAlgaeCollectionCommand() { return new SequentialCommandGroup( + GeneralCommands.getResetFlipArmOverrideCommand(), + new InstantCommand(() -> IS_HOLDING_ALGAE = true), CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.ARM::hasGamePiece), - getInitiateAlgaeCollectionCommand().until(RobotContainer.ARM::hasGamePiece), + getInitiateReefAlgaeCollectionCommand().until(RobotContainer.ARM::hasGamePiece), new InstantCommand(() -> getScoreAlgaeCommand().schedule()).alongWith(getAlgaeCollectionConfirmationCommand()) - ).alongWith(getAlignToReefCommand()); + ).finallyDo(() -> IS_HOLDING_ALGAE = false); + } + + public static Command getReefAlgaeCollectionCommand() { + return new SequentialCommandGroup( + GeneralCommands.getResetFlipArmOverrideCommand(), + new InstantCommand(() -> IS_HOLDING_ALGAE = true), + CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.ARM::hasGamePiece), + getInitiateReefAlgaeCollectionCommand().until(RobotContainer.ARM::hasGamePiece), + getScoreAlgaeCommand().alongWith(getAlgaeCollectionConfirmationCommand()) + ).alongWith(getAlignToReefCommand()).finallyDo(() -> IS_HOLDING_ALGAE = false); } private static Command getAlignToReefCommand() { @@ -41,12 +72,12 @@ private static Command getAlignToReefCommand() { ), SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> fieldRelativePowersToSelfRelativeXPower(OperatorConstants.DRIVER_CONTROLLER.getLeftY(), OperatorConstants.DRIVER_CONTROLLER.getLeftX()), - () -> 0, + () -> ALIGN_TO_REEF_Y_CONTROLLER.calculate(calculateReefRelativeYOffset()), () -> calculateClosestAlgaeCollectionPose().getRotation() ) ).raceWith( new WaitCommand(1).andThen(new WaitUntilCommand(RobotContainer.ARM::hasGamePiece)), - new WaitUntilCommand(OperatorConstants.STOP_ALGAE_AUTO_ALIGN_OVERRIDE_TRIGGER) + new WaitUntilCommand(OperatorConstants.STOP_REEF_ALGAE_ALIGN_TRIGGER) ).onlyIf(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY).asProxy(); } @@ -55,21 +86,27 @@ private static Command getScoreAlgaeCommand() { Map.of( 0, getHoldAlgaeCommand(), 1, getScoreInNetCommand(), - 2, getScoreInProcessorCommand(), - 3, getEjectAlgaeCommand() + 2, getScoreInProcessorCommand() ), AlgaeManipulationCommands::getAlgaeScoreMethodSelector - ).raceWith(new WaitUntilChangeCommand<>(AlgaeManipulationCommands::isScoreAlgaeButtonPressed)).repeatedly().until(() -> !RobotContainer.ARM.hasGamePiece()); + ).raceWith(new WaitUntilChangeCommand<>(AlgaeManipulationCommands::isScoreAlgaeButtonPressed)).repeatedly().until(() -> !RobotContainer.ARM.hasGamePiece() && !isScoreAlgaeButtonPressed()); + } + + private static Command getHoldAlgaeCommand() { + return new ParallelCommandGroup( + ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.HOLD_ALGAE), + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST_WITH_ALGAE) + ); } private static Command getScoreInNetCommand() { return new ParallelRaceGroup( ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_NET), - getArmScoringSequenceCommand(ArmConstants.ArmState.SCORE_NET), + getArmScoringSequenceCommand(ArmConstants.ArmState.SCORE_NET, shouldReverseNetScore()), SwerveCommands.getClosedLoopFieldRelativeDriveCommand( () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), - () -> new FlippableRotation2d(Rotation2d.k180deg, true) + () -> new FlippableRotation2d(shouldReverseNetScore() ? Rotation2d.kZero : Rotation2d.k180deg, true) ).asProxy().onlyWhile(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY) ); } @@ -77,26 +114,19 @@ private static Command getScoreInNetCommand() { private static Command getScoreInProcessorCommand() { return new ParallelRaceGroup( ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_PROCESSOR), - getArmScoringSequenceCommand(ArmConstants.ArmState.SCORE_PROCESSOR), - SwerveCommands.getClosedLoopFieldRelativeDriveCommand( - () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), - () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), - () -> new FlippableRotation2d(Rotation2d.kCW_90deg, true) - ).asProxy().onlyWhile(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY) + getArmScoringSequenceCommand(ArmConstants.ArmState.SCORE_PROCESSOR, false), + SwerveCommands.getDriveToPoseCommand( + () -> FieldConstants.FLIPPABLE_PROCESSOR_SCORE_POSE, + AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS + ).until(() -> RobotContainer.SWERVE.atPose(FieldConstants.FLIPPABLE_PROCESSOR_SCORE_POSE)) + .asProxy().onlyWhile(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY) ); } - private static Command getEjectAlgaeCommand() { - return new ParallelCommandGroup( - ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.EJECT_ALGAE), - ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST) - ).until(() -> !RobotContainer.ARM.hasGamePiece()); - } - - private static Command getArmScoringSequenceCommand(ArmConstants.ArmState scoreState) { + private static Command getArmScoringSequenceCommand(ArmConstants.ArmState scoreState, boolean shouldReverseScore) { return new SequentialCommandGroup( - ArmCommands.getPrepareForStateCommand(scoreState, CoralPlacingCommands.shouldReverseScore()).until(OperatorConstants.CONTINUE_TRIGGER), - ArmCommands.getSetTargetStateCommand(scoreState, CoralPlacingCommands.shouldReverseScore()) + ArmCommands.getPrepareForStateCommand(scoreState, shouldReverseScore).until(OperatorConstants.CONTINUE_TRIGGER), + ArmCommands.getSetTargetStateCommand(scoreState, shouldReverseScore) ); } @@ -105,57 +135,73 @@ private static int getAlgaeScoreMethodSelector() { return 1; if (OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean()) return 2; - if (OperatorConstants.EJECT_ALGAE_TRIGGER.getAsBoolean()) - return 3; return 0; } - private static Command getInitiateAlgaeCollectionCommand() { + private static boolean shouldReverseNetScore() {//TODO: Implement + return false; + } + + private static Command getInitiateFloorAlgaeCollectionCommand() { + return new ConditionalCommand( + getCollectAlgaeFromLollipopSequenceCommand(), + getCollectAlgaeFromFloorSequenceCommand(), + () -> SHOULD_COLLECT_FROM_LOLLIPOP + ).raceWith( + new WaitUntilChangeCommand<>(() -> SHOULD_COLLECT_FROM_LOLLIPOP) + ).repeatedly(); + } + + private static Command getCollectAlgaeFromLollipopSequenceCommand() { return new ParallelCommandGroup( - ArmCommands.getSetTargetStateCommand(CoralPlacingCommands.REEF_CHOOSER.getArmAlgaeCollectionState()), - ElevatorCommands.getSetTargetStateCommand(CoralPlacingCommands.REEF_CHOOSER.getElevatorAlgaeCollectionState()) + GeneralCommands.getFlippableOverridableArmCommand(ArmConstants.ArmState.COLLECT_ALGAE_LOLLIPOP), + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.COLLECT_ALGAE_LOLLIPOP) ); } - private static Command getHoldAlgaeCommand() { + private static Command getCollectAlgaeFromFloorSequenceCommand() { return new ParallelCommandGroup( - ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.HOLD_ALGAE), - ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST_WITH_ALGAE) + GeneralCommands.getFlippableOverridableArmCommand(ArmConstants.ArmState.COLLECT_ALGAE_FLOOR), + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.COLLECT_ALGAE_GROUND) ); } + private static Command getInitiateReefAlgaeCollectionCommand() { + return new ParallelCommandGroup( + ArmCommands.getSetTargetStateCommand(OperatorConstants.REEF_CHOOSER.getArmAlgaeCollectionState(), CoralPlacingCommands.shouldReverseScore()), + ElevatorCommands.getSetTargetStateCommand(OperatorConstants.REEF_CHOOSER.getElevatorAlgaeCollectionState()) + ).raceWith( + new WaitUntilChangeCommand<>(OperatorConstants.REEF_CHOOSER::getArmAlgaeCollectionState), + new WaitUntilChangeCommand<>(OperatorConstants.REEF_CHOOSER::getElevatorAlgaeCollectionState) + ).repeatedly(); + } + private static Command getAlgaeCollectionConfirmationCommand() { return new ParallelCommandGroup( new InstantCommand(() -> OperatorConstants.DRIVER_CONTROLLER.rumble(OperatorConstants.RUMBLE_DURATION_SECONDS, OperatorConstants.RUMBLE_POWER)), - LEDCommands.getAnimateCommand(LEDConstants.ALGAE_COLLECTION_CONFIRMATION_ANIMATION_SETTINGS) //TODO: add LEDs + LEDCommands.getAnimateCommand(LEDConstants.ALGAE_COLLECTION_CONFIRMATION_ANIMATION_SETTINGS) ); } private static FlippablePose2d calculateClosestAlgaeCollectionPose() { - final Translation2d robotPositionOnField = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); + final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); final Translation2d reefCenterPosition = new FlippableTranslation2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, true).get(); final Rotation2d[] reefClockAngles = FieldConstants.REEF_CLOCK_ANGLES; final Transform2d reefCenterToBranchScoringPose = new Transform2d(FieldConstants.REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS, 0, new Rotation2d()); + final List scoringPoses = new java.util.ArrayList<>(List.of()); - double distanceFromClosestScoringPoseMeters = Double.POSITIVE_INFINITY; - Pose2d closestScoringPose = new Pose2d(); - for (final Rotation2d targetRotation : reefClockAngles) { - final Pose2d reefCenterAtTargetRotation = new Pose2d(reefCenterPosition, targetRotation); - final Pose2d currentScoringPose = reefCenterAtTargetRotation.transformBy(reefCenterToBranchScoringPose); - final double distanceFromCurrentScoringPoseMeters = currentScoringPose.getTranslation().getDistance(robotPositionOnField); - if (distanceFromCurrentScoringPoseMeters < distanceFromClosestScoringPoseMeters) { - distanceFromClosestScoringPoseMeters = distanceFromCurrentScoringPoseMeters; - closestScoringPose = currentScoringPose; - } + for (Rotation2d reefClockAngle : reefClockAngles) { + final Pose2d reefCenterAtTargetRotation = new Pose2d(reefCenterPosition, reefClockAngle); + scoringPoses.add(reefCenterAtTargetRotation.transformBy(reefCenterToBranchScoringPose)); } - return new FlippablePose2d(closestScoringPose.rotateBy(CoralPlacingCommands.shouldReverseScore() ? Rotation2d.k180deg : new Rotation2d()), false); + final Pose2d closestScoringPose = robotPose.nearest(scoringPoses); + return new FlippablePose2d(closestScoringPose.getTranslation(), closestScoringPose.getRotation().plus(CoralPlacingCommands.shouldReverseScore() ? Rotation2d.k180deg : new Rotation2d()).getRadians(), false); } private static boolean isScoreAlgaeButtonPressed() { return OperatorConstants.SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean() || - OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean() || - OperatorConstants.EJECT_ALGAE_TRIGGER.getAsBoolean(); + OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean(); } private static double fieldRelativePowersToSelfRelativeXPower(double xPower, double yPower) { @@ -165,4 +211,8 @@ private static double fieldRelativePowersToSelfRelativeXPower(double xPower, dou return (xValue * robotHeading.getCos()) + (yValue * robotHeading.getSin()); } + + private static double calculateReefRelativeYOffset() {//TODO: Implement + return 0; + } } \ No newline at end of file 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 82c344ea..6fe2578d 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -1,9 +1,6 @@ package frc.trigon.robot.commands.commandfactories; -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.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.LEDConstants; import frc.trigon.robot.constants.OperatorConstants; @@ -37,10 +34,15 @@ private static Command getLoadCoralCommand() { } public static Command getUnloadCoralCommand() { - return new ParallelCommandGroup( - ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.UNLOAD_CORAL), - ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.UNLOAD_CORAL) - ).until(() -> !RobotContainer.ARM.hasGamePiece()); + return new ParallelRaceGroup( + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.UNLOAD_CORAL), + new SequentialCommandGroup( + ArmCommands.getPrepareForStateCommand(ArmConstants.ArmState.UNLOAD_CORAL) + .until(() -> RobotContainer.ARM.atPrepareAngle() && RobotContainer.ELEVATOR.atTargetState()), + ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.UNLOAD_CORAL) + .raceWith(new WaitCommand(1)) + ) + ).until(() -> !RobotContainer.ARM.hasGamePiece() || RobotContainer.INTAKE.hasCoral()); } private static Command getIntakeSequenceCommand() { 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 63b28bac..76c8b689 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -34,13 +34,14 @@ public static Command getScoreInReefCommand(boolean shouldScoreRight) { } static boolean shouldReverseScore() { - final Rotation2d robotRotation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation(); - final Translation2d robotTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); + final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + final Rotation2d robotRotation = robotPose.getRotation(); + final Translation2d robotTranslation = robotPose.getTranslation(); final Translation2d reefCenterTranslation = FieldConstants.FLIPPABLE_REEF_CENTER_TRANSLATION.get(); final Translation2d difference = reefCenterTranslation.minus(robotTranslation); final Rotation2d robotRotationRelativeToReef = difference.getAngle(); final Rotation2d robotRotationFacingReef = robotRotation.minus(robotRotationRelativeToReef); - return robotRotationFacingReef.getDegrees() > Rotation2d.kCW_90deg.getDegrees() && robotRotationFacingReef.getDegrees() < Rotation2d.kCCW_90deg.getDegrees(); + return Math.abs(robotRotationFacingReef.getDegrees()) > Rotation2d.kCW_90deg.getDegrees(); } private static Command getAutonomouslyScoreCommand(boolean shouldScoreRight) { @@ -194,10 +195,8 @@ private ElevatorConstants.ElevatorState determineElevatorCoralState() { private ElevatorConstants.ElevatorState determineElevatorAlgaeCollectionState() { return switch (level) { - case 1 -> ElevatorConstants.ElevatorState.COLLECT_ALGAE_GROUND; - case 2 -> ElevatorConstants.ElevatorState.COLLECT_ALGAE_L2; - case 3 -> ElevatorConstants.ElevatorState.COLLECT_ALGAE_L3; - case 4 -> ElevatorConstants.ElevatorState.COLLECT_ALGAE_LOLLIPOP; + case 1, 2 -> ElevatorConstants.ElevatorState.COLLECT_ALGAE_L2; + case 3, 4 -> ElevatorConstants.ElevatorState.COLLECT_ALGAE_L3; default -> throw new IllegalStateException("Unexpected value: " + ordinal()); }; } @@ -214,10 +213,8 @@ private ArmConstants.ArmState determineArmCoralState() { private ArmConstants.ArmState determineArmAlgaeCollectionState() { return switch (level) { - case 1 -> ArmConstants.ArmState.COLLECT_ALGAE_FLOOR; - case 2 -> ArmConstants.ArmState.COLLECT_ALGAE_L2; - case 3 -> ArmConstants.ArmState.COLLECT_ALGAE_L3; - case 4 -> ArmConstants.ArmState.COLLECT_ALGAE_LOLLIPOP; + case 1, 2 -> ArmConstants.ArmState.COLLECT_ALGAE_L2; + case 3, 4 -> ArmConstants.ArmState.COLLECT_ALGAE_L3; default -> throw new IllegalStateException("Unexpected value: " + ordinal()); }; } diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java index 5fbedf61..a29e6e94 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java @@ -2,8 +2,11 @@ import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.commands.CommandConstants; +import frc.trigon.robot.commands.commandclasses.WaitUntilChangeCommand; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.MotorSubsystem; +import frc.trigon.robot.subsystems.arm.ArmCommands; +import frc.trigon.robot.subsystems.arm.ArmConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; import java.util.function.BooleanSupplier; @@ -62,4 +65,14 @@ public static Command runWhen(Command command, BooleanSupplier condition) { public static Command runWhen(Command command, BooleanSupplier condition, double debounceTimeSeconds) { return runWhen(new WaitCommand(debounceTimeSeconds).andThen(command.onlyIf(condition)), condition); } + + public static Command getResetFlipArmOverrideCommand() { + return new InstantCommand(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE = false); + } + + public static Command getFlippableOverridableArmCommand(ArmConstants.ArmState targetState) { + return ArmCommands.getSetTargetStateCommand(targetState, OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE).raceWith( + new WaitUntilChangeCommand<>(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE) + ).repeatedly(); + } } \ 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 dd9d1867..9097a0ea 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -47,6 +47,8 @@ public class FieldConstants { public static final Rotation2d LEFT_FEEDER_ANGLE = Rotation2d.fromDegrees(54); public static final FlippableTranslation2d FLIPPABLE_REEF_CENTER_TRANSLATION = new FlippableTranslation2d(BLUE_REEF_CENTER_TRANSLATION, true); + public static final FlippablePose2d FLIPPABLE_PROCESSOR_SCORE_POSE = new FlippablePose2d(0, 0, Rotation2d.kCW_90deg, true);//TODO: Find values + private static AprilTagFieldLayout createAprilTagFieldLayout() { try { return SHOULD_USE_HOME_TAG_LAYOUT ? diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 91251b28..0a2bfe9f 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -1,9 +1,11 @@ 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.commands.commandclasses.IntakeAssistCommand; +import frc.trigon.robot.commands.commandfactories.AlgaeManipulationCommands; import frc.trigon.robot.misc.ReefChooser; import lib.hardware.misc.KeyboardController; import lib.hardware.misc.XboxController; @@ -24,6 +26,10 @@ public class OperatorConstants { ); public static final KeyboardController OPERATOR_CONTROLLER = new KeyboardController(); public static final ReefChooser REEF_CHOOSER = new ReefChooser(REEF_CHOOSER_PORT); + public static boolean SHOULD_FLIP_ARM_OVERRIDE = false;//TODO: Implement everywhere + private static boolean + IS_LEFT_SCORE_BUTTON_PRESSED = false, + IS_RIGHT_SCORE_BUTTON_PRESSED = false; public static final double POV_DIVIDER = 2, @@ -34,27 +40,29 @@ public class OperatorConstants { public static final IntakeAssistCommand.AssistMode DEFAULT_INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.ALTERNATE_ASSIST; public static final Trigger - RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.rightStick(), + RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.povUp(), DRIVE_FROM_DPAD_TRIGGER = new Trigger(() -> DRIVER_CONTROLLER.getPov() != -1), TOGGLE_BRAKE_TRIGGER = OPERATOR_CONTROLLER.g().or(RobotController::getUserButton), DEBUGGING_TRIGGER = OPERATOR_CONTROLLER.f2(), - CONTINUE_TRIGGER = DRIVER_CONTROLLER.rightTrigger().or(OPERATOR_CONTROLLER.k()), + CONTINUE_TRIGGER = DRIVER_CONTROLLER.leftStick().and(DRIVER_CONTROLLER.rightStick()).or(OPERATOR_CONTROLLER.k()), FORWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.right(), BACKWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.left(), FORWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.up(), BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.down(); public static final Trigger - SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.n().or((DRIVER_CONTROLLER.rightStick().and(RobotContainer.ARM::hasGamePiece))), - SCORE_ALGAE_IN_PROCESSOR_TRIGGER = OPERATOR_CONTROLLER.j().or((DRIVER_CONTROLLER.leftStick().and(RobotContainer.ARM::hasGamePiece))), - EJECT_ALGAE_TRIGGER = OPERATOR_CONTROLLER.p().and(RobotContainer.ARM::hasGamePiece), - STOP_ALGAE_AUTO_ALIGN_OVERRIDE_TRIGGER = DRIVER_CONTROLLER.x().and(RobotContainer.ARM::hasGamePiece), - ALGAE_COLLECTION_TRIGGER = DRIVER_CONTROLLER.a().or(OPERATOR_CONTROLLER.a()); - public static final Trigger + FLOOR_ALGAE_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftBumper(),//TODO: Add operator control + REEF_ALGAE_COLLECTION_TRIGGER = DRIVER_CONTROLLER.rightBumper().or(OPERATOR_CONTROLLER.a()), + STOP_REEF_ALGAE_ALIGN_TRIGGER = DRIVER_CONTROLLER.x().and(RobotContainer.ARM::hasGamePiece), + SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.n().or(createScoreTrigger(true, true)), + SCORE_ALGAE_IN_PROCESSOR_TRIGGER = OPERATOR_CONTROLLER.j().or(createScoreTrigger(false, true)), CORAL_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftTrigger().or(OPERATOR_CONTROLLER.c()), - SPAWN_CORAL_TRIGGER = OPERATOR_CONTROLLER.equals(), - SCORE_CORAL_LEFT_TRIGGER = DRIVER_CONTROLLER.leftBumper(), - SCORE_CORAL_RIGHT_TRIGGER = DRIVER_CONTROLLER.rightBumper(), - EJECT_CORAL_TRIGGER = DRIVER_CONTROLLER.back().or(OPERATOR_CONTROLLER.e()); + SCORE_CORAL_RIGHT_TRIGGER = createScoreTrigger(true, false), + SCORE_CORAL_LEFT_TRIGGER = createScoreTrigger(false, false), + EJECT_CORAL_TRIGGER = OPERATOR_CONTROLLER.e(); + public static final Trigger + SPAWN_CORAL_IN_SIMULATION_TRIGGER = OPERATOR_CONTROLLER.equals(), + FLIP_ARM_TRIGGER = DRIVER_CONTROLLER.start(), + LOLLIPOP_ALGAE_TOGGLE_TRIGGER = DRIVER_CONTROLLER.a(); public static final Trigger SET_TARGET_SCORING_REEF_LEVEL_L1_TRIGGER = OPERATOR_CONTROLLER.numpad0().or(DRIVER_CONTROLLER.a()), @@ -69,4 +77,23 @@ 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(boolean isRight, boolean isAlgaeCommand) { + final Trigger scoreTrigger; + + if (isRight) + scoreTrigger = 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)); + else + scoreTrigger = DRIVER_CONTROLLER.leftStick() + .and(() -> !IS_RIGHT_SCORE_BUTTON_PRESSED) + .onTrue(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = true)) + .onFalse(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = false)); + + if (isAlgaeCommand) + return scoreTrigger.and(() -> AlgaeManipulationCommands.isHoldingAlgae()); + return scoreTrigger.and(() -> !AlgaeManipulationCommands.isHoldingAlgae()); + } } \ No newline at end of file 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 53b66983..de6c6371 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -176,7 +176,7 @@ private static void configureReverseLimitSensor() { public enum ElevatorState { REST(0.603, 0.603, 0.7), LOAD_CORAL(0.5519, 0.5519, 0.7), - UNLOAD_CORAL(0.5519, 0.5519, 0.7), + UNLOAD_CORAL(0.5, 0.65, 0.7), SCORE_L1(0.1, 0.1, 1), SCORE_L2(0.3, 0.4, 1), SCORE_L3(0.7, 0.8, 1), From 57208321b2e28b7374ca46f9e8e3cb49a31a4bb9 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Thu, 25 Sep 2025 13:30:05 +0300 Subject: [PATCH 11/51] lol --- .../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 44c6643f..ff2096ed 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -49,7 +49,7 @@ public static Command getFloorAlgaeCollectionCommand() { GeneralCommands.getResetFlipArmOverrideCommand(), new InstantCommand(() -> IS_HOLDING_ALGAE = true), CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.ARM::hasGamePiece), - getInitiateReefAlgaeCollectionCommand().until(RobotContainer.ARM::hasGamePiece), + getInitiateFloorAlgaeCollectionCommand().until(RobotContainer.ARM::hasGamePiece), new InstantCommand(() -> getScoreAlgaeCommand().schedule()).alongWith(getAlgaeCollectionConfirmationCommand()) ).finallyDo(() -> IS_HOLDING_ALGAE = false); } From 759205758247d80e974b7dce6cbc13dbc570aba1 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 25 Sep 2025 20:35:07 +0300 Subject: [PATCH 12/51] W --- .../robot/commands/commandfactories/CoralPlacingCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 76c8b689..e55d619f 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -41,7 +41,7 @@ static boolean shouldReverseScore() { final Translation2d difference = reefCenterTranslation.minus(robotTranslation); final Rotation2d robotRotationRelativeToReef = difference.getAngle(); final Rotation2d robotRotationFacingReef = robotRotation.minus(robotRotationRelativeToReef); - return Math.abs(robotRotationFacingReef.getDegrees()) > Rotation2d.kCW_90deg.getDegrees(); + return robotRotationFacingReef.getDegrees() > Rotation2d.kCW_90deg.getDegrees() && robotRotationFacingReef.getDegrees() < Rotation2d.kCCW_90deg.getDegrees(); } private static Command getAutonomouslyScoreCommand(boolean shouldScoreRight) { From 9537d3e892695b4c950fd1664b3121993c57e166 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Thu, 25 Sep 2025 21:34:19 +0300 Subject: [PATCH 13/51] temporary --- .../SimulatedGamePieceConstants.java | 50 ++++++++++++++----- .../SimulationFieldHandler.java | 5 +- .../robot/subsystems/arm/ArmConstants.java | 2 +- 3 files changed, 42 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 214357de..6a3bd918 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java @@ -42,13 +42,16 @@ public class SimulatedGamePieceConstants { 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())) - )); + )).addAll(createAlgaeOnReef()); private static final Translation3d REEF_CENTER_TO_L1_VECTOR = new Translation3d(0.652, 0.1643, 0.46), REEF_CENTER_TO_L2_VECTOR = new Translation3d(0.652, 0.1643, 0.6983), REEF_CENTER_TO_L3_VECTOR = new Translation3d(0.652, 0.1643, 1.1101), REEF_CENTER_TO_L4_VECTOR = new Translation3d(0.7796, 0.1643, 1.7345); + private static final Translation3d + REEF_CENTER_TO_L2_ALGAE_VECTOR = new Translation3d(0.652, 0.1643, 0.75), + REEF_CENTER_TO_L3_ALGAE_VECTOR = new Translation3d(0.652, 0.1643, 1.218); private static final Rotation3d REEF_TO_2_OCLOCK_ROTATION = new Rotation3d(0, 0, Math.PI / 3), REEF_TO_4_OCLOCK_ROTATION = new Rotation3d(0, 0, Math.PI / 3 * 2), @@ -61,9 +64,13 @@ public class SimulatedGamePieceConstants { CORAL_TO_L2_AND_L3_ALIGNMENT = new Transform3d(new Translation3d(0, 0, 0), new Rotation3d(0, -Units.degreesToRadians(35), 0)), CORAL_TO_L4_ALIGNMENT = new Transform3d(new Translation3d(0, 0, 0), CORAL_TO_VERTICAL_POSITION_ROTATION); private static final Transform3d - RIGHT_BRANCH_TO_LEFT_BRANCH = new Transform3d(new Translation3d(0, -0.33, 0), new Rotation3d()); + RIGHT_BRANCH_TO_LEFT_BRANCH = new Transform3d(new Translation3d(0, -0.33, 0), new Rotation3d()), + RIGHT_BRANCH_TO_BETWEEN_BRANCHES = new Transform3d(new Translation3d(0, -0.165, 0), new Rotation3d()); public static final ArrayList CORAL_SCORING_LOCATIONS = calculatedCoralScoringLocations(); - public static final Pose3d PROCESSOR_LOCATION = new Pose3d(0, 0, 0, new Rotation3d()); + public static final Pose3d + PROCESSOR_LOCATION = new Pose3d(11.60, 8.23, 0.18, new Rotation3d()), + NET_MINIMUM_X_LOCATION = new Pose3d(0, 0, 0, new Rotation3d()), + NET_MAXIMUM_X_LOCATION = new Pose3d(0, 0, 0, new Rotation3d()); public static final FlippableTranslation2d LEFT_FEEDER_POSITION = new FlippableTranslation2d(0.923, 7.370, true), RIGHT_FEEDER_POSITION = new FlippableTranslation2d(0.923, 0.668, true); @@ -90,9 +97,23 @@ private static ArrayList calculatedCoralScoringLocations() { return coralScoringPoses; } + private static ArrayList createAlgaeOnReef() { + final ArrayList algaeStartingPoses = new ArrayList<>(); + for (int clockFace = 2; clockFace <= 12; clockFace += 2) + algaeStartingPoses.add(new SimulatedGamePiece(calculateAlgaeStartingPose(clockFace), GamePieceType.ALGAE)); + return algaeStartingPoses; + } + + private static Pose3d calculateAlgaeStartingPose(int clockFace) { + final int level = clockFace % 4 == 0 ? 3 : 2; + final Translation3d reefCenterToLevelVector = level == 2 ? REEF_CENTER_TO_L2_ALGAE_VECTOR : REEF_CENTER_TO_L3_ALGAE_VECTOR; + final Rotation3d reefToClockFaceRotation = getReefClockFaceRotation(clockFace); + return new Pose3d(new Pose2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, new Rotation2d())).transformBy(new Transform3d(reefCenterToLevelVector.rotateBy(reefToClockFaceRotation), reefToClockFaceRotation)).transformBy(RIGHT_BRANCH_TO_BETWEEN_BRANCHES); + } + private static FlippablePose3d calculateCoralScorePose(int level, int clockFace, boolean isLeftBranch) { + final Rotation3d reefToClockFaceRotation = getReefClockFaceRotation(clockFace); final Translation3d reefCenterToLevelVector; - final Rotation3d reefToClockFaceRotation; final Transform3d coralAlignment; switch (level) { case 1 -> reefCenterToLevelVector = REEF_CENTER_TO_L1_VECTOR; @@ -101,15 +122,6 @@ private static FlippablePose3d calculateCoralScorePose(int level, int clockFace, case 4 -> reefCenterToLevelVector = REEF_CENTER_TO_L4_VECTOR; default -> reefCenterToLevelVector = new Translation3d(); } - switch (clockFace) { - case 2 -> reefToClockFaceRotation = REEF_TO_2_OCLOCK_ROTATION; - case 4 -> reefToClockFaceRotation = REEF_TO_4_OCLOCK_ROTATION; - case 6 -> reefToClockFaceRotation = REEF_TO_6_OCLOCK_ROTATION; - case 8 -> reefToClockFaceRotation = REEF_TO_8_OCLOCK_ROTATION; - case 10 -> reefToClockFaceRotation = REEF_TO_10_OCLOCK_ROTATION; - case 12 -> reefToClockFaceRotation = REEF_TO_12_OCLOCK_ROTATION; - default -> reefToClockFaceRotation = new Rotation3d(); - } switch (level) { case 1 -> coralAlignment = CORAL_TO_L1_ALIGNMENT; case 2, 3 -> coralAlignment = CORAL_TO_L2_AND_L3_ALIGNMENT; @@ -119,6 +131,18 @@ private static FlippablePose3d calculateCoralScorePose(int level, int clockFace, return new FlippablePose3d(new Pose3d(new Pose2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, new Rotation2d())).transformBy(new Transform3d(reefCenterToLevelVector.rotateBy(reefToClockFaceRotation), reefToClockFaceRotation)).transformBy(isLeftBranch ? RIGHT_BRANCH_TO_LEFT_BRANCH : new Transform3d()).transformBy(coralAlignment), true); } + private static Rotation3d getReefClockFaceRotation(int clockFace) { + return switch (clockFace) { + case 2 -> REEF_TO_2_OCLOCK_ROTATION; + case 4 -> REEF_TO_4_OCLOCK_ROTATION; + case 6 -> REEF_TO_6_OCLOCK_ROTATION; + case 8 -> REEF_TO_8_OCLOCK_ROTATION; + case 10 -> REEF_TO_10_OCLOCK_ROTATION; + case 12 -> REEF_TO_12_OCLOCK_ROTATION; + default -> new Rotation3d(); + }; + } + public enum GamePieceType { ALGAE(0.2, 0), CORAL(0.06, 1); 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 355fca6f..cbc85f07 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -124,7 +124,10 @@ private static boolean isCollectingCoral() { } private static boolean isCollectingAlgae() { - return RobotContainer.ARM.atState(ArmConstants.ArmState.COLLECT_ALGAE_L2) || RobotContainer.ARM.atState(ArmConstants.ArmState.COLLECT_ALGAE_L3); + return RobotContainer.ARM.atState(ArmConstants.ArmState.COLLECT_ALGAE_L2) + || RobotContainer.ARM.atState(ArmConstants.ArmState.COLLECT_ALGAE_L3) + || RobotContainer.ARM.atState(ArmConstants.ArmState.COLLECT_ALGAE_FLOOR) + || RobotContainer.ARM.atState(ArmConstants.ArmState.COLLECT_ALGAE_LOLLIPOP); } private static boolean isCoralLoading() { diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 8c9dee25..c120ce78 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -248,7 +248,7 @@ public enum ArmState { SCORE_L4(Rotation2d.fromDegrees(100), Rotation2d.fromDegrees(120), 4), SCORE_NET(Rotation2d.fromDegrees(160), Rotation2d.fromDegrees(160), 4), SCORE_PROCESSOR(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), 4), - COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0), -4), + COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(30), Rotation2d.fromDegrees(30), -4), COLLECT_ALGAE_L2(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), -4), COLLECT_ALGAE_L3(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), -4), COLLECT_ALGAE_LOLLIPOP(Rotation2d.fromDegrees(45), Rotation2d.fromDegrees(45), -4), From 60688dd84d02fd417cd3d0af7ee423c431f9c3f3 Mon Sep 17 00:00:00 2001 From: shmayaR <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 25 Sep 2025 14:49:29 -0400 Subject: [PATCH 14/51] some stuff Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../SimulatedGamePieceConstants.java | 17 ++++++++++++----- .../utilities/flippable/FlippablePose3d.java | 14 ++++++++++++++ 2 files changed, 26 insertions(+), 5 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 6a3bd918..1a3485ca 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java @@ -34,15 +34,15 @@ public class SimulatedGamePieceConstants { createNewCoral(new Pose3d(FIELD_LENGTH_METERS - 1.22, FIELD_WIDTH_METERS / 2, 0.15, CORAL_TO_VERTICAL_POSITION_ROTATION)), createNewCoral(new Pose3d(FIELD_LENGTH_METERS - 1.22, FIELD_WIDTH_METERS / 2 - 1.83, 0.15, CORAL_TO_VERTICAL_POSITION_ROTATION)), createNewCoral(new Pose3d(FIELD_LENGTH_METERS - 1.22, FIELD_WIDTH_METERS / 2 + 1.83, 0.15, CORAL_TO_VERTICAL_POSITION_ROTATION)) - )), - ALGAE_ON_FIELD = new ArrayList<>(List.of( + )); + public static final ArrayList ALGAE_ON_FIELD = new ArrayList<>(List.of( createNewAlgae(new Pose3d(1.22, FIELD_WIDTH_METERS / 2, 0.5, new Rotation3d())), createNewAlgae(new Pose3d(1.22, FIELD_WIDTH_METERS / 2 - 1.83, 0.5, new Rotation3d())), createNewAlgae(new Pose3d(1.22, FIELD_WIDTH_METERS / 2 + 1.83, 0.5, new Rotation3d())), createNewAlgae(new Pose3d(FIELD_LENGTH_METERS - 1.22, FIELD_WIDTH_METERS / 2, 0.5, new Rotation3d())), createNewAlgae(new Pose3d(FIELD_LENGTH_METERS - 1.22, FIELD_WIDTH_METERS / 2 - 1.83, 0.5, new Rotation3d())), createNewAlgae(new Pose3d(FIELD_LENGTH_METERS - 1.22, FIELD_WIDTH_METERS / 2 + 1.83, 0.5, new Rotation3d())) - )).addAll(createAlgaeOnReef()); + )); private static final Translation3d REEF_CENTER_TO_L1_VECTOR = new Translation3d(0.652, 0.1643, 0.46), @@ -78,6 +78,10 @@ public class SimulatedGamePieceConstants { RIGHT_CORAL_SPAWN_POSE = new FlippablePose3d(new Pose3d(1.5, 1.5, 0, new Rotation3d()), true), LEFT_CORAL_SPAWN_POSE = new FlippablePose3d(new Pose3d(1.5, 6.5, 0, new Rotation3d()), true); + static { + ALGAE_ON_FIELD.addAll(createAlgaeOnReef()); + } + private static SimulatedGamePiece createNewCoral(Pose3d startingPose) { return new SimulatedGamePiece(startingPose, GamePieceType.CORAL); } @@ -99,8 +103,11 @@ private static ArrayList calculatedCoralScoringLocations() { private static ArrayList createAlgaeOnReef() { final ArrayList algaeStartingPoses = new ArrayList<>(); - for (int clockFace = 2; clockFace <= 12; clockFace += 2) - algaeStartingPoses.add(new SimulatedGamePiece(calculateAlgaeStartingPose(clockFace), GamePieceType.ALGAE)); + for (int clockFace = 2; clockFace <= 12; clockFace += 2) { + final Pose3d pose = calculateAlgaeStartingPose(clockFace); + algaeStartingPoses.add(new SimulatedGamePiece(pose, GamePieceType.ALGAE)); + algaeStartingPoses.add(new SimulatedGamePiece(FlippablePose3d.flipPose(pose), GamePieceType.ALGAE)); + } return algaeStartingPoses; } diff --git a/src/main/java/lib/utilities/flippable/FlippablePose3d.java b/src/main/java/lib/utilities/flippable/FlippablePose3d.java index 0ac5b46c..6b8ac37f 100644 --- a/src/main/java/lib/utilities/flippable/FlippablePose3d.java +++ b/src/main/java/lib/utilities/flippable/FlippablePose3d.java @@ -58,4 +58,18 @@ protected Pose3d flip(Pose3d pose) { ) ); } + + public static Pose3d flipPose(Pose3d pose) { + final Pose2d flippedPose2d = FlippingUtil.flipFieldPose(pose.toPose2d()); + return new Pose3d( + flippedPose2d.getTranslation().getX(), + flippedPose2d.getTranslation().getY(), + pose.getZ(), + new Rotation3d( + pose.getRotation().getX(), + pose.getRotation().getY(), + flippedPose2d.getRotation().getRadians() + ) + ); + } } \ No newline at end of file From a4c41fac631a83b3b02d61fc242182e17ae5abe0 Mon Sep 17 00:00:00 2001 From: shmayaR Date: Thu, 25 Sep 2025 15:17:40 -0400 Subject: [PATCH 15/51] added algae to reef, and no loading when robot has algae --- .../commands/commandfactories/CoralCollectionCommands.java | 2 +- .../misc/simulatedfield/SimulatedGamePieceConstants.java | 5 +++-- 2 files changed, 4 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 6fe2578d..a6928a1e 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -30,7 +30,7 @@ private static Command getLoadCoralCommand() { return new ParallelCommandGroup( ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.LOAD_CORAL), ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.LOAD_CORAL) - ).until(RobotContainer.ARM::hasGamePiece); + ).unless(AlgaeManipulationCommands::isHoldingAlgae).until(RobotContainer.ARM::hasGamePiece); } public static Command getUnloadCoralCommand() { 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 1a3485ca..de7edfbe 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java @@ -49,9 +49,10 @@ public class SimulatedGamePieceConstants { REEF_CENTER_TO_L2_VECTOR = new Translation3d(0.652, 0.1643, 0.6983), REEF_CENTER_TO_L3_VECTOR = new Translation3d(0.652, 0.1643, 1.1101), REEF_CENTER_TO_L4_VECTOR = new Translation3d(0.7796, 0.1643, 1.7345); + private static final double ALGAE_RADIUS_METERS = 0.2032; private static final Translation3d - REEF_CENTER_TO_L2_ALGAE_VECTOR = new Translation3d(0.652, 0.1643, 0.75), - REEF_CENTER_TO_L3_ALGAE_VECTOR = new Translation3d(0.652, 0.1643, 1.218); + REEF_CENTER_TO_L2_ALGAE_VECTOR = new Translation3d(0.652, 0.1643, 0.70612 + ALGAE_RADIUS_METERS), + REEF_CENTER_TO_L3_ALGAE_VECTOR = new Translation3d(0.652, 0.1643, 1.01196 + ALGAE_RADIUS_METERS); private static final Rotation3d REEF_TO_2_OCLOCK_ROTATION = new Rotation3d(0, 0, Math.PI / 3), REEF_TO_4_OCLOCK_ROTATION = new Rotation3d(0, 0, Math.PI / 3 * 2), From 0f94518852d54ace5e3e31aff1977bb89142c1f7 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 25 Sep 2025 22:41:53 +0300 Subject: [PATCH 16/51] Flippable 2.0 --- .../AlgaeManipulationCommands.java | 10 ++--- .../CoralPlacingCommands.java | 37 ++++++++++++------- .../commandfactories/GeneralCommands.java | 22 ++++++++--- .../robot/subsystems/arm/ArmCommands.java | 30 ++++++++++++--- .../robot/subsystems/arm/ArmConstants.java | 4 +- 5 files changed, 72 insertions(+), 31 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 ff2096ed..c696d264 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -125,8 +125,8 @@ private static Command getScoreInProcessorCommand() { private static Command getArmScoringSequenceCommand(ArmConstants.ArmState scoreState, boolean shouldReverseScore) { return new SequentialCommandGroup( - ArmCommands.getPrepareForStateCommand(scoreState, shouldReverseScore).until(OperatorConstants.CONTINUE_TRIGGER), - ArmCommands.getSetTargetStateCommand(scoreState, shouldReverseScore) + GeneralCommands.getFlippableOverridableArmCommand(scoreState, true, shouldReverseScore).until(OperatorConstants.CONTINUE_TRIGGER), + GeneralCommands.getFlippableOverridableArmCommand(scoreState, false, shouldReverseScore) ); } @@ -154,21 +154,21 @@ private static Command getInitiateFloorAlgaeCollectionCommand() { private static Command getCollectAlgaeFromLollipopSequenceCommand() { return new ParallelCommandGroup( - GeneralCommands.getFlippableOverridableArmCommand(ArmConstants.ArmState.COLLECT_ALGAE_LOLLIPOP), + GeneralCommands.getFlippableOverridableArmCommand(ArmConstants.ArmState.COLLECT_ALGAE_LOLLIPOP, false, false), ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.COLLECT_ALGAE_LOLLIPOP) ); } private static Command getCollectAlgaeFromFloorSequenceCommand() { return new ParallelCommandGroup( - GeneralCommands.getFlippableOverridableArmCommand(ArmConstants.ArmState.COLLECT_ALGAE_FLOOR), + GeneralCommands.getFlippableOverridableArmCommand(ArmConstants.ArmState.COLLECT_ALGAE_FLOOR, false, false), ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.COLLECT_ALGAE_GROUND) ); } private static Command getInitiateReefAlgaeCollectionCommand() { return new ParallelCommandGroup( - ArmCommands.getSetTargetStateCommand(OperatorConstants.REEF_CHOOSER.getArmAlgaeCollectionState(), CoralPlacingCommands.shouldReverseScore()), + GeneralCommands.getFlippableOverridableArmCommand(OperatorConstants.REEF_CHOOSER.getArmAlgaeCollectionState(), false, CoralPlacingCommands.shouldReverseScore()), ElevatorCommands.getSetTargetStateCommand(OperatorConstants.REEF_CHOOSER.getElevatorAlgaeCollectionState()) ).raceWith( new WaitUntilChangeCommand<>(OperatorConstants.REEF_CHOOSER::getArmAlgaeCollectionState), 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 e55d619f..800bae65 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -13,7 +13,6 @@ import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.misc.ReefChooser; -import frc.trigon.robot.subsystems.arm.ArmCommands; import frc.trigon.robot.subsystems.arm.ArmConstants; import frc.trigon.robot.subsystems.elevator.ElevatorCommands; import frc.trigon.robot.subsystems.elevator.ElevatorConstants; @@ -26,10 +25,13 @@ public class CoralPlacingCommands { static final ReefChooser REEF_CHOOSER = OperatorConstants.REEF_CHOOSER; public static Command getScoreInReefCommand(boolean shouldScoreRight) { - return new ConditionalCommand( - getAutonomouslyScoreCommand(shouldScoreRight), - getScoreCommand(shouldScoreRight), - () -> SHOULD_SCORE_AUTONOMOUSLY + return new SequentialCommandGroup( + GeneralCommands.getResetFlipArmOverrideCommand(), + new ConditionalCommand( + getAutonomouslyScoreCommand(shouldScoreRight), + getScoreCommand(), + () -> SHOULD_SCORE_AUTONOMOUSLY + ) ); } @@ -46,32 +48,38 @@ static boolean shouldReverseScore() { private static Command getAutonomouslyScoreCommand(boolean shouldScoreRight) { return new SequentialCommandGroup( - getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(() -> isArmAndElevatorAtPrepareState(shouldScoreRight)), + getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(() -> isReadyToPlace(shouldScoreRight) || OperatorConstants.CONTINUE_TRIGGER.getAsBoolean()), new ParallelCommandGroup( ElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getElevatorCoralState), - ArmCommands.getSetTargetStateCommand(REEF_CHOOSER::getArmCoralState, CoralPlacingCommands::shouldReverseScore) + GeneralCommands.getFlippableOverridableArmCommand(REEF_CHOOSER::getArmCoralState, false, CoralPlacingCommands::shouldReverseScore) ) ); } - private static Command getScoreCommand(boolean shouldScoreRight) { + private static Command getScoreCommand() { return new SequentialCommandGroup( - getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(OperatorConstants.CONTINUE_TRIGGER), + getPrepareScoreCommand().until(OperatorConstants.CONTINUE_TRIGGER), new ParallelCommandGroup( ElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getElevatorCoralState), - ArmCommands.getSetTargetStateCommand(REEF_CHOOSER::getArmCoralState, CoralPlacingCommands::shouldReverseScore) + GeneralCommands.getFlippableOverridableArmCommand(REEF_CHOOSER::getArmCoralState, false, CoralPlacingCommands::shouldReverseScore) ) ); } private static Command getAutonomouslyPrepareScoreCommand(boolean shouldScoreRight) { return new ParallelCommandGroup( - ElevatorCommands.getPrepareStateCommand(REEF_CHOOSER::getElevatorCoralState), - ArmCommands.getPrepareForStateCommand(REEF_CHOOSER::getArmCoralState, CoralPlacingCommands::shouldReverseScore), + getPrepareScoreCommand(), getAutonomousDriveToReefThenManualDriveCommand(shouldScoreRight).asProxy() ); } + private static Command getPrepareScoreCommand() { + return new ParallelCommandGroup( + ElevatorCommands.getPrepareStateCommand(REEF_CHOOSER::getElevatorCoralState), + GeneralCommands.getFlippableOverridableArmCommand(REEF_CHOOSER::getArmCoralState, true, CoralPlacingCommands::shouldReverseScore) + ); + } + private static Command getAutonomousDriveToReefThenManualDriveCommand(boolean shouldScoreRight) { return new SequentialCommandGroup( SwerveCommands.getDriveToPoseCommand( @@ -117,10 +125,11 @@ public static FlippablePose2d calculateClosestScoringPose(boolean shouldScoreRig return new FlippablePose2d(closestScoringPose.transformBy(scoringPoseToBranch), false); } - private static boolean isArmAndElevatorAtPrepareState(boolean shouldScoreRight) { + private static boolean isReadyToPlace(boolean shouldScoreRight) { return RobotContainer.ELEVATOR.atPreparedTargetState() && RobotContainer.ARM.atPrepareAngle() - && RobotContainer.SWERVE.atPose(calculateClosestScoringPose(shouldScoreRight)); + && RobotContainer.SWERVE.atPose(calculateClosestScoringPose(shouldScoreRight)) + && !OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE; } /** diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java index a29e6e94..60aee369 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java @@ -2,7 +2,6 @@ import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.commands.CommandConstants; -import frc.trigon.robot.commands.commandclasses.WaitUntilChangeCommand; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.arm.ArmCommands; @@ -10,6 +9,7 @@ import frc.trigon.robot.subsystems.swerve.SwerveCommands; import java.util.function.BooleanSupplier; +import java.util.function.Supplier; /** * A class that contains the general commands of the robot, such as commands that alter a command or commands that affect all subsystems. @@ -70,9 +70,21 @@ public static Command getResetFlipArmOverrideCommand() { return new InstantCommand(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE = false); } - public static Command getFlippableOverridableArmCommand(ArmConstants.ArmState targetState) { - return ArmCommands.getSetTargetStateCommand(targetState, OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE).raceWith( - new WaitUntilChangeCommand<>(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE) - ).repeatedly(); + public static Command getFlippableOverridableArmCommand(ArmConstants.ArmState targetState, boolean isPrepareState, boolean shouldStartFlipped) { + return isPrepareState ? + ArmCommands.getPrepareForStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped) : + ArmCommands.getSetTargetStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped); + } + + public static Command getFlippableOverridableArmCommand(Supplier targetState, boolean isPrepareState, boolean shouldStartFlipped) { + return isPrepareState ? + ArmCommands.getPrepareForStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped) : + ArmCommands.getSetTargetStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped); + } + + public static Command getFlippableOverridableArmCommand(Supplier targetState, boolean isPrepareState, BooleanSupplier shouldStartFlipped) { + return isPrepareState ? + ArmCommands.getPrepareForStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped.getAsBoolean()) : + ArmCommands.getSetTargetStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped.getAsBoolean()); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java index 6bb7d384..6a54fe8c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java @@ -10,6 +10,7 @@ import lib.commands.NetworkTablesCommand; import java.util.Set; +import java.util.function.BooleanSupplier; import java.util.function.Supplier; public class ArmCommands { @@ -35,14 +36,25 @@ public static Command getGearRatioCalulationCommand() { ); } - public static Command getSetTargetStateCommand(Supplier targetState, Supplier isStateReversed) { + public static Command getSetTargetStateCommand(Supplier targetState, BooleanSupplier isStateReversed) { return new FunctionalCommand( () -> RobotContainer.ARM.setEndEffectorState(targetState.get()), - () -> RobotContainer.ARM.setArmState(targetState.get(), isStateReversed.get()), + () -> RobotContainer.ARM.setArmState(targetState.get(), isStateReversed.getAsBoolean()), interrupted -> RobotContainer.ARM.stop(), () -> false, RobotContainer.ARM - ); } + ); + } + + public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState, BooleanSupplier isStateReversed) { + return new FunctionalCommand( + () -> RobotContainer.ARM.setEndEffectorState(targetState), + () -> RobotContainer.ARM.setArmState(targetState, isStateReversed.getAsBoolean()), + interrupted -> RobotContainer.ARM.stop(), + () -> false, + RobotContainer.ARM + ); + } public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState) { return getSetTargetStateCommand(targetState, false); @@ -58,9 +70,17 @@ public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState ); } - public static Command getPrepareForStateCommand(Supplier targetState, Supplier isStateReversed) { + public static Command getPrepareForStateCommand(Supplier targetState, BooleanSupplier isStateReversed) { + return new ExecuteEndCommand( + () -> RobotContainer.ARM.setPrepareState(targetState.get(), isStateReversed.getAsBoolean()), + RobotContainer.ARM::stop, + RobotContainer.ARM + ); + } + + public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState, BooleanSupplier isStateReversed) { return new ExecuteEndCommand( - () -> RobotContainer.ARM.setPrepareState(targetState.get(), isStateReversed.get()), + () -> RobotContainer.ARM.setPrepareState(targetState, isStateReversed.getAsBoolean()), RobotContainer.ARM::stop, RobotContainer.ARM ); diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index c120ce78..43edea86 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -248,10 +248,10 @@ public enum ArmState { SCORE_L4(Rotation2d.fromDegrees(100), Rotation2d.fromDegrees(120), 4), SCORE_NET(Rotation2d.fromDegrees(160), Rotation2d.fromDegrees(160), 4), SCORE_PROCESSOR(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), 4), - COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(30), Rotation2d.fromDegrees(30), -4), + COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(70), Rotation2d.fromDegrees(70), -4), COLLECT_ALGAE_L2(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), -4), COLLECT_ALGAE_L3(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), -4), - COLLECT_ALGAE_LOLLIPOP(Rotation2d.fromDegrees(45), Rotation2d.fromDegrees(45), -4), + COLLECT_ALGAE_LOLLIPOP(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), -4), EJECT_ALGAE(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), 4); public final Rotation2d targetAngle; From ee10305442e44dbd8100f14a2bff45ab510dcac1 Mon Sep 17 00:00:00 2001 From: shmayaR Date: Thu, 25 Sep 2025 16:40:27 -0400 Subject: [PATCH 17/51] :) --- 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 0a2bfe9f..91829242 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -93,7 +93,7 @@ private static Trigger createScoreTrigger(boolean isRight, boolean isAlgaeComman .onFalse(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = false)); if (isAlgaeCommand) - return scoreTrigger.and(() -> AlgaeManipulationCommands.isHoldingAlgae()); + return scoreTrigger.and(AlgaeManipulationCommands::isHoldingAlgae); return scoreTrigger.and(() -> !AlgaeManipulationCommands.isHoldingAlgae()); } } \ No newline at end of file From 9a35efc2c6b14662f048767ebc8b2136d4dd9f87 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 25 Sep 2025 23:40:40 +0300 Subject: [PATCH 18/51] Fix algae problem and more --- .../AlgaeManipulationCommands.java | 15 +++++++++++---- .../commandfactories/CoralCollectionCommands.java | 7 +++++-- .../trigon/robot/constants/OperatorConstants.java | 4 ++-- .../subsystems/elevator/ElevatorConstants.java | 2 +- .../robot/subsystems/intake/IntakeConstants.java | 2 +- 5 files changed, 20 insertions(+), 10 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 c696d264..00974cc5 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -17,6 +17,8 @@ import frc.trigon.robot.subsystems.arm.ArmConstants; import frc.trigon.robot.subsystems.elevator.ElevatorCommands; import frc.trigon.robot.subsystems.elevator.ElevatorConstants; +import frc.trigon.robot.subsystems.intake.IntakeCommands; +import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; import lib.hardware.RobotHardwareStats; import lib.hardware.misc.leds.LEDCommands; @@ -47,10 +49,13 @@ public static void toggleLollipopCollection() { public static Command getFloorAlgaeCollectionCommand() { return new SequentialCommandGroup( GeneralCommands.getResetFlipArmOverrideCommand(), - new InstantCommand(() -> IS_HOLDING_ALGAE = true), + new InstantCommand(() -> { + IS_HOLDING_ALGAE = true; + SHOULD_COLLECT_FROM_LOLLIPOP = false; + }), CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.ARM::hasGamePiece), getInitiateFloorAlgaeCollectionCommand().until(RobotContainer.ARM::hasGamePiece), - new InstantCommand(() -> getScoreAlgaeCommand().schedule()).alongWith(getAlgaeCollectionConfirmationCommand()) + getScoreAlgaeCommand().alongWith(getAlgaeCollectionConfirmationCommand()) ).finallyDo(() -> IS_HOLDING_ALGAE = false); } @@ -155,14 +160,16 @@ private static Command getInitiateFloorAlgaeCollectionCommand() { private static Command getCollectAlgaeFromLollipopSequenceCommand() { return new ParallelCommandGroup( GeneralCommands.getFlippableOverridableArmCommand(ArmConstants.ArmState.COLLECT_ALGAE_LOLLIPOP, false, false), + CoralCollectionCommands.getCoralCollectionCommand().until(RobotContainer.TRANSPORTER::hasCoral) + .andThen(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.OPEN_REST)).onlyWhile(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE).repeatedly().asProxy(), ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.COLLECT_ALGAE_LOLLIPOP) ); } private static Command getCollectAlgaeFromFloorSequenceCommand() { return new ParallelCommandGroup( - GeneralCommands.getFlippableOverridableArmCommand(ArmConstants.ArmState.COLLECT_ALGAE_FLOOR, false, false), - ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.COLLECT_ALGAE_GROUND) + ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.COLLECT_ALGAE_FLOOR), + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.COLLECT_ALGAE_FLOOR) ); } 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 a6928a1e..d32e3ac5 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -20,7 +20,10 @@ public static Command getCoralCollectionCommand() { return new SequentialCommandGroup( getIntakeSequenceCommand(), new InstantCommand( - () -> getLoadCoralCommand().schedule() + () -> { + if (!AlgaeManipulationCommands.isHoldingAlgae()) + getLoadCoralCommand().schedule(); + } ) ); // new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE) @@ -30,7 +33,7 @@ private static Command getLoadCoralCommand() { return new ParallelCommandGroup( ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.LOAD_CORAL), ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.LOAD_CORAL) - ).unless(AlgaeManipulationCommands::isHoldingAlgae).until(RobotContainer.ARM::hasGamePiece); + ).until(RobotContainer.ARM::hasGamePiece); } public static Command getUnloadCoralCommand() { diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 0a2bfe9f..684d9e07 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -65,7 +65,7 @@ public class OperatorConstants { LOLLIPOP_ALGAE_TOGGLE_TRIGGER = DRIVER_CONTROLLER.a(); public static final Trigger - SET_TARGET_SCORING_REEF_LEVEL_L1_TRIGGER = OPERATOR_CONTROLLER.numpad0().or(DRIVER_CONTROLLER.a()), + SET_TARGET_SCORING_REEF_LEVEL_L1_TRIGGER = OPERATOR_CONTROLLER.numpad0().or(DRIVER_CONTROLLER.a().and(() -> !AlgaeManipulationCommands.isHoldingAlgae())), SET_TARGET_SCORING_REEF_LEVEL_L2_TRIGGER = OPERATOR_CONTROLLER.numpad1().or(DRIVER_CONTROLLER.b()), SET_TARGET_SCORING_REEF_LEVEL_L3_TRIGGER = OPERATOR_CONTROLLER.numpad2().or(DRIVER_CONTROLLER.x()), SET_TARGET_SCORING_REEF_LEVEL_L4_TRIGGER = OPERATOR_CONTROLLER.numpad3().or(DRIVER_CONTROLLER.y()), @@ -93,7 +93,7 @@ private static Trigger createScoreTrigger(boolean isRight, boolean isAlgaeComman .onFalse(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = false)); if (isAlgaeCommand) - return scoreTrigger.and(() -> AlgaeManipulationCommands.isHoldingAlgae()); + return scoreTrigger.and(AlgaeManipulationCommands::isHoldingAlgae); return scoreTrigger.and(() -> !AlgaeManipulationCommands.isHoldingAlgae()); } } \ No newline at end of file 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 de6c6371..436b9116 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -183,7 +183,7 @@ public enum ElevatorState { SCORE_L4(1.2, 1.3, 1), COLLECT_ALGAE_L2(0.603, 0.603, 1), COLLECT_ALGAE_L3(0.953, 0.953, 1), - COLLECT_ALGAE_GROUND(0, 0, 1), + COLLECT_ALGAE_FLOOR(0, 0, 1), COLLECT_ALGAE_LOLLIPOP(0.3, 0.3, 1), REST_WITH_ALGAE(0.603, 0.603, 0.3), SCORE_NET(1.382, 1.382, 0.3), diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index 9113d59d..cdb2d72a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -22,7 +22,6 @@ import lib.hardware.phoenix6.talonfx.TalonFXSignal; import lib.hardware.simulation.SimpleMotorSimulation; import lib.hardware.simulation.SingleJointedArmSimulation; -import lib.utilities.Conversions; import lib.utilities.mechanisms.SingleJointedArmMechanism2d; import lib.utilities.mechanisms.SpeedMechanism2d; @@ -219,6 +218,7 @@ private static void configureDistanceSensor() { public enum IntakeState { REST(0, MINIMUM_ANGLE), + OPEN_REST(0, MAXIMUM_ANGLE), COLLECT(5, MAXIMUM_ANGLE), EJECT(-5, MAXIMUM_ANGLE); From d58b8410a9be93bfb033a75e017bc2d28e8e9870 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 25 Sep 2025 23:56:24 +0300 Subject: [PATCH 19/51] Made work --- .../commandfactories/AlgaeManipulationCommands.java | 8 ++++++-- 1 file changed, 6 insertions(+), 2 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 00974cc5..fd5e7184 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -56,6 +56,7 @@ public static Command getFloorAlgaeCollectionCommand() { CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.ARM::hasGamePiece), getInitiateFloorAlgaeCollectionCommand().until(RobotContainer.ARM::hasGamePiece), getScoreAlgaeCommand().alongWith(getAlgaeCollectionConfirmationCommand()) + .until(() -> !RobotContainer.ARM.hasGamePiece() && !isScoreAlgaeButtonPressed()) ).finallyDo(() -> IS_HOLDING_ALGAE = false); } @@ -66,7 +67,10 @@ public static Command getReefAlgaeCollectionCommand() { CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.ARM::hasGamePiece), getInitiateReefAlgaeCollectionCommand().until(RobotContainer.ARM::hasGamePiece), getScoreAlgaeCommand().alongWith(getAlgaeCollectionConfirmationCommand()) - ).alongWith(getAlignToReefCommand()).finallyDo(() -> IS_HOLDING_ALGAE = false); + .until(() -> !RobotContainer.ARM.hasGamePiece() && !isScoreAlgaeButtonPressed()) + ) + .alongWith(getAlignToReefCommand()) + .finallyDo(() -> IS_HOLDING_ALGAE = false); } private static Command getAlignToReefCommand() { @@ -94,7 +98,7 @@ private static Command getScoreAlgaeCommand() { 2, getScoreInProcessorCommand() ), AlgaeManipulationCommands::getAlgaeScoreMethodSelector - ).raceWith(new WaitUntilChangeCommand<>(AlgaeManipulationCommands::isScoreAlgaeButtonPressed)).repeatedly().until(() -> !RobotContainer.ARM.hasGamePiece() && !isScoreAlgaeButtonPressed()); + ).raceWith(new WaitUntilChangeCommand<>(AlgaeManipulationCommands::isScoreAlgaeButtonPressed)).repeatedly(); } private static Command getHoldAlgaeCommand() { From 02f4c914d645da463e75df7a0347cc5b19515329 Mon Sep 17 00:00:00 2001 From: shmayaR Date: Thu, 25 Sep 2025 17:10:36 -0400 Subject: [PATCH 20/51] fixed algae collection ending bug --- .../misc/simulatedfield/SimulatedGamePieceConstants.java | 2 +- .../robot/misc/simulatedfield/SimulationFieldHandler.java | 4 ++-- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 6 +++++- .../java/frc/trigon/robot/subsystems/arm/ArmCommands.java | 3 ++- 4 files changed, 10 insertions(+), 5 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 de7edfbe..736998e8 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java @@ -113,7 +113,7 @@ private static ArrayList createAlgaeOnReef() { } private static Pose3d calculateAlgaeStartingPose(int clockFace) { - final int level = clockFace % 4 == 0 ? 3 : 2; + final int level = clockFace % 4 == 0 ? 2 : 3; final Translation3d reefCenterToLevelVector = level == 2 ? REEF_CENTER_TO_L2_ALGAE_VECTOR : REEF_CENTER_TO_L3_ALGAE_VECTOR; final Rotation3d reefToClockFaceRotation = getReefClockFaceRotation(clockFace); return new Pose3d(new Pose2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, new Rotation2d())).transformBy(new Transform3d(reefCenterToLevelVector.rotateBy(reefToClockFaceRotation), reefToClockFaceRotation)).transformBy(RIGHT_BRANCH_TO_BETWEEN_BRANCHES); 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 cbc85f07..a3002fbc 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -138,7 +138,7 @@ private static void updateEjection() { if (HELD_CORAL_INDEX != null) updateCoralEjection(); - if (HELD_ALGAE_INDEX != null && RobotContainer.ARM.isEjecting()) { + if (HELD_ALGAE_INDEX != null && RobotContainer.ARM.isEjectingAlgae()) { final SimulatedGamePiece heldAlgae = ALGAE_ON_FIELD.get(HELD_ALGAE_INDEX); heldAlgae.release(RobotContainer.ARM.calculateLinearArmAndEndEffectorVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d()); HELD_ALGAE_INDEX = null; @@ -152,7 +152,7 @@ private static void updateCoralEjection() { heldCoral.release(RobotContainer.INTAKE.calculateLinearIntakeVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d(), IntakeConstants.CORAL_COLLECTION_POSE.getTranslation()); HELD_CORAL_INDEX = null; } - if (isCoralInEndEffector() && RobotContainer.ARM.isEjecting()) { + if (isCoralInEndEffector() && RobotContainer.ARM.isEjectingCoral()) { heldCoral.release(RobotContainer.ARM.calculateLinearArmAndEndEffectorVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d(), RobotContainer.ARM.calculateGamePieceCollectionPose().getTranslation()); HELD_CORAL_INDEX = null; } diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index ae82e57e..4d903e2c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -143,10 +143,14 @@ public Translation3d calculateLinearArmAndEndEffectorVelocity() { ); } - public boolean isEjecting() { + public boolean isEjectingCoral() { return endEffectorMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE) > 2; } + public boolean isEjectingAlgae() { + return endEffectorMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE) >= 0; + } + public Translation3d calculateLinearArmVelocity() { double velocityRotationsPerSecond = armMasterMotor.getSignal(TalonFXSignal.VELOCITY); double velocityMagnitude = velocityRotationsPerSecond * 2 * Math.PI * ArmConstants.ARM_LENGTH_METERS; diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java index 6a54fe8c..ca39d2a7 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.commandfactories.GeneralCommands; import lib.commands.ExecuteEndCommand; import lib.commands.GearRatioCalculationCommand; import lib.commands.NetworkTablesCommand; @@ -99,7 +100,7 @@ public static Command getPrepareForStateCommand(ArmConstants.ArmState targetStat } public static Command getRestCommand() { - return new ConditionalCommand( + return GeneralCommands.getContinuousConditionalCommand( getSetTargetStateCommand(ArmConstants.ArmState.REST_WITH_CORAL), getSetTargetStateCommand(ArmConstants.ArmState.REST), RobotContainer.ARM::hasGamePiece From f6042fbf900d36c1dfafd7698bc890f2e0c41000 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 26 Sep 2025 00:42:37 +0300 Subject: [PATCH 21/51] It picks it up now --- .../commandfactories/AlgaeManipulationCommands.java | 12 ++++++++++-- .../java/frc/trigon/robot/subsystems/arm/Arm.java | 2 +- 2 files changed, 11 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 fd5e7184..9b6fc9f4 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -164,9 +164,8 @@ private static Command getInitiateFloorAlgaeCollectionCommand() { private static Command getCollectAlgaeFromLollipopSequenceCommand() { return new ParallelCommandGroup( GeneralCommands.getFlippableOverridableArmCommand(ArmConstants.ArmState.COLLECT_ALGAE_LOLLIPOP, false, false), - CoralCollectionCommands.getCoralCollectionCommand().until(RobotContainer.TRANSPORTER::hasCoral) - .andThen(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.OPEN_REST)).onlyWhile(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE).repeatedly().asProxy(), ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.COLLECT_ALGAE_LOLLIPOP) +// getIntakeCoralFromLollipopCommand().onlyWhile(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE).repeatedly() ); } @@ -177,6 +176,15 @@ private static Command getCollectAlgaeFromFloorSequenceCommand() { ); } + private static Command getIntakeCoralFromLollipopCommand() { + return new SequentialCommandGroup( + CoralCollectionCommands.getCoralCollectionCommand() + .until(RobotContainer.TRANSPORTER::hasCoral) + .unless(RobotContainer.TRANSPORTER::hasCoral), + IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.OPEN_REST) + ); + } + private static Command getInitiateReefAlgaeCollectionCommand() { return new ParallelCommandGroup( GeneralCommands.getFlippableOverridableArmCommand(OperatorConstants.REEF_CHOOSER.getArmAlgaeCollectionState(), false, CoralPlacingCommands.shouldReverseScore()), diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index ae82e57e..e8546dec 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -101,7 +101,7 @@ public boolean atState(ArmConstants.ArmState targetState, boolean isStateReverse } public boolean atState(ArmConstants.ArmState targetState) { - return this.targetState == targetState && atAngle(targetState.targetAngle); + return this.targetState == targetState && atAngle(isStateReversed ? subtractFrom360Degrees(targetState.targetAngle) : targetState.targetAngle); } public boolean atTargetAngle() { From 20ce80e5a92f4299262a88937769a666b5fa24f4 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 26 Sep 2025 00:49:54 +0300 Subject: [PATCH 22/51] Just the dropping..... --- .../commandfactories/AlgaeManipulationCommands.java | 13 ++++++++----- 1 file changed, 8 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 9b6fc9f4..3ef9d79c 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -55,6 +55,7 @@ public static Command getFloorAlgaeCollectionCommand() { }), CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.ARM::hasGamePiece), getInitiateFloorAlgaeCollectionCommand().until(RobotContainer.ARM::hasGamePiece), + GeneralCommands.getResetFlipArmOverrideCommand(), getScoreAlgaeCommand().alongWith(getAlgaeCollectionConfirmationCommand()) .until(() -> !RobotContainer.ARM.hasGamePiece() && !isScoreAlgaeButtonPressed()) ).finallyDo(() -> IS_HOLDING_ALGAE = false); @@ -66,6 +67,7 @@ public static Command getReefAlgaeCollectionCommand() { new InstantCommand(() -> IS_HOLDING_ALGAE = true), CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.ARM::hasGamePiece), getInitiateReefAlgaeCollectionCommand().until(RobotContainer.ARM::hasGamePiece), + GeneralCommands.getResetFlipArmOverrideCommand(), getScoreAlgaeCommand().alongWith(getAlgaeCollectionConfirmationCommand()) .until(() -> !RobotContainer.ARM.hasGamePiece() && !isScoreAlgaeButtonPressed()) ) @@ -98,7 +100,8 @@ private static Command getScoreAlgaeCommand() { 2, getScoreInProcessorCommand() ), AlgaeManipulationCommands::getAlgaeScoreMethodSelector - ).raceWith(new WaitUntilChangeCommand<>(AlgaeManipulationCommands::isScoreAlgaeButtonPressed)).repeatedly(); + ) + .raceWith(new WaitUntilChangeCommand<>(AlgaeManipulationCommands::isScoreAlgaeButtonPressed)).repeatedly(); } private static Command getHoldAlgaeCommand() { @@ -164,8 +167,8 @@ private static Command getInitiateFloorAlgaeCollectionCommand() { private static Command getCollectAlgaeFromLollipopSequenceCommand() { return new ParallelCommandGroup( GeneralCommands.getFlippableOverridableArmCommand(ArmConstants.ArmState.COLLECT_ALGAE_LOLLIPOP, false, false), - ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.COLLECT_ALGAE_LOLLIPOP) -// getIntakeCoralFromLollipopCommand().onlyWhile(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE).repeatedly() + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.COLLECT_ALGAE_LOLLIPOP), + getIntakeCoralFromLollipopCommand().onlyWhile(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE).repeatedly().asProxy() ); } @@ -179,8 +182,8 @@ private static Command getCollectAlgaeFromFloorSequenceCommand() { private static Command getIntakeCoralFromLollipopCommand() { return new SequentialCommandGroup( CoralCollectionCommands.getCoralCollectionCommand() - .until(RobotContainer.TRANSPORTER::hasCoral) - .unless(RobotContainer.TRANSPORTER::hasCoral), + .unless(RobotContainer.TRANSPORTER::hasCoral) + .until(RobotContainer.TRANSPORTER::hasCoral), IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.OPEN_REST) ); } From f59a54d250c31be2750a9348ccc3530604ff469e Mon Sep 17 00:00:00 2001 From: shmayaR Date: Thu, 25 Sep 2025 17:53:34 -0400 Subject: [PATCH 23/51] is adding a boolean event rlly that hard @Strflightmight09 ? --- .../java/frc/trigon/robot/subsystems/arm/Arm.java | 2 +- .../trigon/robot/subsystems/arm/ArmConstants.java | 13 ++++++++++--- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index 2d6e84b7..ba3ba561 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -148,7 +148,7 @@ public boolean isEjectingCoral() { } public boolean isEjectingAlgae() { - return endEffectorMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE) >= 0; + return ArmConstants.IS_EJECTING_ALGAE_BOOLEAN_EVENT.getAsBoolean(); } public Translation3d calculateLinearArmVelocity() { diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index 43edea86..bff82db5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -120,11 +120,18 @@ public class ArmConstants { */ static final Rotation2d MAXIMUM_ARM_SAFE_ANGLE = Rotation2d.fromDegrees(90); - private static final double COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.2; - static final BooleanEvent COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( + private static final double + COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.2, + IS_EJECTING_ALGAE_DEBOUNCE_TIME_SECONDS = 0.1; + static final BooleanEvent + COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), DISTANCE_SENSOR::getBinaryValue - ).debounce(COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS); + ).debounce(COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS), + IS_EJECTING_ALGAE_BOOLEAN_EVENT = new BooleanEvent( + CommandScheduler.getInstance().getActiveButtonLoop(), + () -> END_EFFECTOR_MOTOR.getSignal(TalonFXSignal.MOTOR_VOLTAGE) >= 0 + ).debounce(IS_EJECTING_ALGAE_DEBOUNCE_TIME_SECONDS); static final double WHEEL_RADIUS_METERS = edu.wpi.first.math.util.Units.inchesToMeters(1.5); static { From 8c7942445f09c253f1570b65c425ab6410340918 Mon Sep 17 00:00:00 2001 From: Nummun14 Date: Thu, 25 Sep 2025 17:58:58 -0400 Subject: [PATCH 24/51] lol --- .../commands/commandfactories/AlgaeManipulationCommands.java | 4 ++-- 1 file changed, 2 insertions(+), 2 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 9b6fc9f4..8d0956f6 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -187,8 +187,8 @@ private static Command getIntakeCoralFromLollipopCommand() { private static Command getInitiateReefAlgaeCollectionCommand() { return new ParallelCommandGroup( - GeneralCommands.getFlippableOverridableArmCommand(OperatorConstants.REEF_CHOOSER.getArmAlgaeCollectionState(), false, CoralPlacingCommands.shouldReverseScore()), - ElevatorCommands.getSetTargetStateCommand(OperatorConstants.REEF_CHOOSER.getElevatorAlgaeCollectionState()) + GeneralCommands.getFlippableOverridableArmCommand(OperatorConstants.REEF_CHOOSER::getArmAlgaeCollectionState, false, CoralPlacingCommands.shouldReverseScore()), + ElevatorCommands.getSetTargetStateCommand(OperatorConstants.REEF_CHOOSER::getElevatorAlgaeCollectionState) ).raceWith( new WaitUntilChangeCommand<>(OperatorConstants.REEF_CHOOSER::getArmAlgaeCollectionState), new WaitUntilChangeCommand<>(OperatorConstants.REEF_CHOOSER::getElevatorAlgaeCollectionState) From 0f756d248f4ffddcda6735ce7952387a4b441a0f Mon Sep 17 00:00:00 2001 From: Nummun14 Date: Thu, 25 Sep 2025 18:30:16 -0400 Subject: [PATCH 25/51] added proc and net locationsd --- .../java/frc/trigon/robot/constants/FieldConstants.java | 4 ++-- .../misc/simulatedfield/SimulatedGamePieceConstants.java | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 9097a0ea..876c0593 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -5,6 +5,7 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.*; +import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import lib.utilities.Conversions; import lib.utilities.FilesHandler; import lib.utilities.flippable.FlippablePose2d; @@ -46,8 +47,7 @@ public class FieldConstants { public static final Rotation2d LEFT_FEEDER_ANGLE = Rotation2d.fromDegrees(54); public static final FlippableTranslation2d FLIPPABLE_REEF_CENTER_TRANSLATION = new FlippableTranslation2d(BLUE_REEF_CENTER_TRANSLATION, true); - - public static final FlippablePose2d FLIPPABLE_PROCESSOR_SCORE_POSE = new FlippablePose2d(0, 0, Rotation2d.kCW_90deg, true);//TODO: Find values + public static final FlippablePose2d FLIPPABLE_PROCESSOR_SCORE_POSE = new FlippablePose2d(SimulatedGamePieceConstants.PROCESSOR_LOCATION.toPose2d(), true);//TODO: Find values private static AprilTagFieldLayout createAprilTagFieldLayout() { try { 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 736998e8..9222c6b0 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java @@ -69,9 +69,9 @@ public class SimulatedGamePieceConstants { RIGHT_BRANCH_TO_BETWEEN_BRANCHES = new Transform3d(new Translation3d(0, -0.165, 0), new Rotation3d()); public static final ArrayList CORAL_SCORING_LOCATIONS = calculatedCoralScoringLocations(); public static final Pose3d - PROCESSOR_LOCATION = new Pose3d(11.60, 8.23, 0.18, new Rotation3d()), - NET_MINIMUM_X_LOCATION = new Pose3d(0, 0, 0, new Rotation3d()), - NET_MAXIMUM_X_LOCATION = new Pose3d(0, 0, 0, new Rotation3d()); + PROCESSOR_LOCATION = new Pose3d( FIELD_LENGTH_METERS - 11.60, 8.23, 0.18, new Rotation3d(0, 0, Rotation2d.kCW_90deg.getRadians())), + NET_MINIMUM_X_LOCATION = new Pose3d(10.724, 4.730, 2.131, new Rotation3d()), + NET_MAXIMUM_X_LOCATION = new Pose3d(10.724, 8.429, 2.131, new Rotation3d()); public static final FlippableTranslation2d LEFT_FEEDER_POSITION = new FlippableTranslation2d(0.923, 7.370, true), RIGHT_FEEDER_POSITION = new FlippableTranslation2d(0.923, 0.668, true); From e452d1b70f2d72854f3544df35d273ae79c58236 Mon Sep 17 00:00:00 2001 From: Nummun14 Date: Thu, 25 Sep 2025 18:58:17 -0400 Subject: [PATCH 26/51] ugly solutions go brrrrrrr --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index ba3ba561..e83e3e1e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -50,6 +50,7 @@ public void setBrake(boolean brake) { public void stop() { armMasterMotor.stopMotor(); endEffectorMotor.stopMotor(); + ArmConstants.END_EFFECTOR_MECHANISM.setTargetVelocity(0); } @Override @@ -196,6 +197,11 @@ void setPrepareState(ArmConstants.ArmState targetState, boolean isStateReversed) this.isStateReversed = isStateReversed; this.targetState = targetState; + if (targetState == ArmConstants.ArmState.HOLD_ALGAE + || targetState == ArmConstants.ArmState.SCORE_NET + || targetState == ArmConstants.ArmState.SCORE_PROCESSOR) + setEndEffectorTargetVoltage(ArmConstants.ArmState.HOLD_ALGAE.targetEndEffectorVoltage); + if (isStateReversed) { setTargetAngle(subtractFrom360Degrees(targetState.prepareAngle)); return; From e942e88e7810d25e26183d710ac844c1535a60b4 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 26 Sep 2025 02:01:02 +0300 Subject: [PATCH 27/51] Commit because I'm goateaddd --- .../AlgaeManipulationCommands.java | 27 ++++++++++++------- .../commandfactories/GeneralCommands.java | 18 ++++++------- 2 files changed, 27 insertions(+), 18 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 a6f29f3b..37d6307d 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -28,6 +28,7 @@ import java.util.List; import java.util.Map; +import java.util.function.BooleanSupplier; public class AlgaeManipulationCommands { private static boolean @@ -114,7 +115,7 @@ private static Command getHoldAlgaeCommand() { private static Command getScoreInNetCommand() { return new ParallelRaceGroup( ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_NET), - getArmScoringSequenceCommand(ArmConstants.ArmState.SCORE_NET, shouldReverseNetScore()), + getArmNetSequenceCommand(AlgaeManipulationCommands::shouldReverseNetScore), SwerveCommands.getClosedLoopFieldRelativeDriveCommand( () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), @@ -126,7 +127,7 @@ private static Command getScoreInNetCommand() { private static Command getScoreInProcessorCommand() { return new ParallelRaceGroup( ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_PROCESSOR), - getArmScoringSequenceCommand(ArmConstants.ArmState.SCORE_PROCESSOR, false), + getArmProcessorSequenceCommand(), SwerveCommands.getDriveToPoseCommand( () -> FieldConstants.FLIPPABLE_PROCESSOR_SCORE_POSE, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS @@ -135,10 +136,17 @@ private static Command getScoreInProcessorCommand() { ); } - private static Command getArmScoringSequenceCommand(ArmConstants.ArmState scoreState, boolean shouldReverseScore) { + private static Command getArmNetSequenceCommand(BooleanSupplier shouldReverseScore) { return new SequentialCommandGroup( - GeneralCommands.getFlippableOverridableArmCommand(scoreState, true, shouldReverseScore).until(OperatorConstants.CONTINUE_TRIGGER), - GeneralCommands.getFlippableOverridableArmCommand(scoreState, false, shouldReverseScore) + GeneralCommands.getFlippableOverridableArmCommand(ArmConstants.ArmState.SCORE_NET, true, shouldReverseScore).until(OperatorConstants.CONTINUE_TRIGGER), + GeneralCommands.getFlippableOverridableArmCommand(ArmConstants.ArmState.SCORE_NET, false, shouldReverseScore) + ); + } + + private static Command getArmProcessorSequenceCommand() { + return new SequentialCommandGroup( + GeneralCommands.getFlippableOverridableArmCommand(ArmConstants.ArmState.SCORE_PROCESSOR, true).until(OperatorConstants.CONTINUE_TRIGGER), + GeneralCommands.getFlippableOverridableArmCommand(ArmConstants.ArmState.SCORE_PROCESSOR, false) ); } @@ -150,8 +158,9 @@ private static int getAlgaeScoreMethodSelector() { return 0; } - private static boolean shouldReverseNetScore() {//TODO: Implement - return false; + private static boolean shouldReverseNetScore() { + final Rotation2d swerveAngle = RobotContainer.SWERVE.getDriveRelativeAngle(); + return swerveAngle.getDegrees() > Rotation2d.kCW_90deg.getDegrees() && swerveAngle.getDegrees() < Rotation2d.kCCW_90deg.getDegrees(); } private static Command getInitiateFloorAlgaeCollectionCommand() { @@ -166,7 +175,7 @@ private static Command getInitiateFloorAlgaeCollectionCommand() { private static Command getCollectAlgaeFromLollipopSequenceCommand() { return new ParallelCommandGroup( - GeneralCommands.getFlippableOverridableArmCommand(ArmConstants.ArmState.COLLECT_ALGAE_LOLLIPOP, false, false), + GeneralCommands.getFlippableOverridableArmCommand(ArmConstants.ArmState.COLLECT_ALGAE_LOLLIPOP, false), ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.COLLECT_ALGAE_LOLLIPOP), getIntakeCoralFromLollipopCommand().onlyWhile(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE).repeatedly().asProxy() ); @@ -190,7 +199,7 @@ private static Command getIntakeCoralFromLollipopCommand() { private static Command getInitiateReefAlgaeCollectionCommand() { return new ParallelCommandGroup( - GeneralCommands.getFlippableOverridableArmCommand(OperatorConstants.REEF_CHOOSER::getArmAlgaeCollectionState, false, CoralPlacingCommands.shouldReverseScore()), + GeneralCommands.getFlippableOverridableArmCommand(OperatorConstants.REEF_CHOOSER::getArmAlgaeCollectionState, false, CoralPlacingCommands::shouldReverseScore), ElevatorCommands.getSetTargetStateCommand(OperatorConstants.REEF_CHOOSER::getElevatorAlgaeCollectionState) ).raceWith( new WaitUntilChangeCommand<>(OperatorConstants.REEF_CHOOSER::getArmAlgaeCollectionState), diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java index 60aee369..b8695955 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java @@ -70,16 +70,10 @@ public static Command getResetFlipArmOverrideCommand() { return new InstantCommand(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE = false); } - public static Command getFlippableOverridableArmCommand(ArmConstants.ArmState targetState, boolean isPrepareState, boolean shouldStartFlipped) { + public static Command getFlippableOverridableArmCommand(ArmConstants.ArmState targetState, boolean isPrepareState, BooleanSupplier shouldStartFlipped) { return isPrepareState ? - ArmCommands.getPrepareForStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped) : - ArmCommands.getSetTargetStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped); - } - - public static Command getFlippableOverridableArmCommand(Supplier targetState, boolean isPrepareState, boolean shouldStartFlipped) { - return isPrepareState ? - ArmCommands.getPrepareForStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped) : - ArmCommands.getSetTargetStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped); + ArmCommands.getPrepareForStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped.getAsBoolean()) : + ArmCommands.getSetTargetStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped.getAsBoolean()); } public static Command getFlippableOverridableArmCommand(Supplier targetState, boolean isPrepareState, BooleanSupplier shouldStartFlipped) { @@ -87,4 +81,10 @@ public static Command getFlippableOverridableArmCommand(Supplier OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped.getAsBoolean()) : ArmCommands.getSetTargetStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped.getAsBoolean()); } + + public static Command getFlippableOverridableArmCommand(ArmConstants.ArmState targetState, boolean isPrepareState) { + return isPrepareState ? + ArmCommands.getPrepareForStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE) : + ArmCommands.getSetTargetStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE); + } } \ No newline at end of file From b9fc291569567d476612e06755d42ae409b4ec4e Mon Sep 17 00:00:00 2001 From: Nummun14 Date: Thu, 25 Sep 2025 20:06:10 -0400 Subject: [PATCH 28/51] processor sim works! Updated SimulatedGamePieceConstants to define PROCESSOR_LOCATION and related fields as FlippablePose3d instead of Pose3d. Adjusted references in FieldConstants and SimulationScoringHandler to use the new FlippablePose3d API. Also fixed a minor variable naming issue in Elevator and added @AutoLogOutput to atTargetState. Updated SCORE_PROCESSOR target position in ElevatorConstants. --- .../commandfactories/AlgaeManipulationCommands.java | 5 ++--- .../java/frc/trigon/robot/constants/FieldConstants.java | 2 +- .../misc/simulatedfield/SimulatedGamePieceConstants.java | 8 ++++---- .../misc/simulatedfield/SimulationScoringHandler.java | 2 +- .../frc/trigon/robot/subsystems/elevator/Elevator.java | 6 ++++-- .../robot/subsystems/elevator/ElevatorConstants.java | 2 +- 6 files changed, 13 insertions(+), 12 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 37d6307d..dd4f5485 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -101,8 +101,7 @@ private static Command getScoreAlgaeCommand() { 2, getScoreInProcessorCommand() ), AlgaeManipulationCommands::getAlgaeScoreMethodSelector - ) - .raceWith(new WaitUntilChangeCommand<>(AlgaeManipulationCommands::isScoreAlgaeButtonPressed)).repeatedly(); + ).raceWith(new WaitUntilChangeCommand<>(AlgaeManipulationCommands::isScoreAlgaeButtonPressed)).repeatedly(); } private static Command getHoldAlgaeCommand() { @@ -125,7 +124,7 @@ private static Command getScoreInNetCommand() { } private static Command getScoreInProcessorCommand() { - return new ParallelRaceGroup( + return new ParallelCommandGroup( ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_PROCESSOR), getArmProcessorSequenceCommand(), SwerveCommands.getDriveToPoseCommand( diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 876c0593..3043a6ac 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -47,7 +47,7 @@ public class FieldConstants { public static final Rotation2d LEFT_FEEDER_ANGLE = Rotation2d.fromDegrees(54); public static final FlippableTranslation2d FLIPPABLE_REEF_CENTER_TRANSLATION = new FlippableTranslation2d(BLUE_REEF_CENTER_TRANSLATION, true); - public static final FlippablePose2d FLIPPABLE_PROCESSOR_SCORE_POSE = new FlippablePose2d(SimulatedGamePieceConstants.PROCESSOR_LOCATION.toPose2d(), true);//TODO: Find values + public static final FlippablePose2d FLIPPABLE_PROCESSOR_SCORE_POSE = new FlippablePose2d(SimulatedGamePieceConstants.PROCESSOR_LOCATION.get().toPose2d(), true);//TODO: Find values private static AprilTagFieldLayout createAprilTagFieldLayout() { try { 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 9222c6b0..735cfa24 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java @@ -68,10 +68,10 @@ public class SimulatedGamePieceConstants { RIGHT_BRANCH_TO_LEFT_BRANCH = new Transform3d(new Translation3d(0, -0.33, 0), new Rotation3d()), RIGHT_BRANCH_TO_BETWEEN_BRANCHES = new Transform3d(new Translation3d(0, -0.165, 0), new Rotation3d()); public static final ArrayList CORAL_SCORING_LOCATIONS = calculatedCoralScoringLocations(); - public static final Pose3d - PROCESSOR_LOCATION = new Pose3d( FIELD_LENGTH_METERS - 11.60, 8.23, 0.18, new Rotation3d(0, 0, Rotation2d.kCW_90deg.getRadians())), - NET_MINIMUM_X_LOCATION = new Pose3d(10.724, 4.730, 2.131, new Rotation3d()), - NET_MAXIMUM_X_LOCATION = new Pose3d(10.724, 8.429, 2.131, new Rotation3d()); + public static final FlippablePose3d + PROCESSOR_LOCATION = new FlippablePose3d(new Pose3d(11.6, 7.5, 0.18, new Rotation3d(0, 0, Rotation2d.kCW_90deg.getRadians())), true), + NET_MINIMUM_X_LOCATION = new FlippablePose3d(new Pose3d(10.724, 4.730, 2.131, new Rotation3d()), true), + NET_MAXIMUM_X_LOCATION = new FlippablePose3d(new Pose3d(10.724, 8.429, 2.131, new Rotation3d()), true); public static final FlippableTranslation2d LEFT_FEEDER_POSITION = new FlippableTranslation2d(0.923, 7.370, true), RIGHT_FEEDER_POSITION = new FlippableTranslation2d(0.923, 0.668, true); diff --git a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationScoringHandler.java b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationScoringHandler.java index f4891f11..adb39e15 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationScoringHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationScoringHandler.java @@ -27,7 +27,7 @@ private static void checkCoralScored(SimulatedGamePiece coral) { } private static void checkAlgaeScored(SimulatedGamePiece algae) { - if (!isGamePieceScored(algae, SimulatedGamePieceConstants.PROCESSOR_LOCATION, SimulatedGamePieceConstants.ALGAE_SCORING_TOLERANCE_METERS)) + if (!isGamePieceScored(algae, SimulatedGamePieceConstants.PROCESSOR_LOCATION.get(), SimulatedGamePieceConstants.ALGAE_SCORING_TOLERANCE_METERS)) return; algae.isScored = true; 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 080739d1..fdd92882 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -15,6 +15,7 @@ import lib.hardware.phoenix6.talonfx.TalonFXMotor; import lib.hardware.phoenix6.talonfx.TalonFXSignal; import lib.utilities.Conversions; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Elevator extends MotorSubsystem { @@ -81,6 +82,7 @@ public boolean atState(ElevatorConstants.ElevatorState targetState) { return targetState == this.targetState && atTargetState(); } + @AutoLogOutput(key = "Elevator/atTargetState") public boolean atTargetState() { final double currentToTargetStateDifferenceMeters = Math.abs(targetState.targetPositionMeters - getPositionMeters()); return currentToTargetStateDifferenceMeters < ElevatorConstants.HEIGHT_TOLERANCE_METERS; @@ -124,8 +126,8 @@ void setTargetPositionRotations(double targetPositionRotations) { } private double calculateMinimumSafeElevatorHeightRotations() { - final double armCos = RobotContainer.ARM.getAngle().getRadians(); - final double elevatorHeightFromArm = Math.cos(armCos) * ArmConstants.ARM_LENGTH_METERS; + final double armAngleRadians = RobotContainer.ARM.getAngle().getRadians(); + final double elevatorHeightFromArm = Math.cos(armAngleRadians) * ArmConstants.ARM_LENGTH_METERS; final double minimumElevatorSafeZone = ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; final double minimumSafeHeightMeters = (RobotContainer.ARM.isArmAboveSafeAngle() ? 0 : elevatorHeightFromArm) 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 436b9116..4c577259 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -187,7 +187,7 @@ public enum ElevatorState { COLLECT_ALGAE_LOLLIPOP(0.3, 0.3, 1), REST_WITH_ALGAE(0.603, 0.603, 0.3), SCORE_NET(1.382, 1.382, 0.3), - SCORE_PROCESSOR(1.182, 1.182, 0.3); + SCORE_PROCESSOR(0.1, 1.182, 0.3); public final double targetPositionMeters; public final double prepareStatePositionMeters; From 3ac3afbc708fa8427b9945953ccb6e991600d201 Mon Sep 17 00:00:00 2001 From: Nummun14 Date: Thu, 25 Sep 2025 21:20:45 -0400 Subject: [PATCH 29/51] net works --- .../AlgaeManipulationCommands.java | 54 +++++++++++++++---- .../robot/constants/FieldConstants.java | 4 +- .../SimulatedGamePieceConstants.java | 5 +- 3 files changed, 49 insertions(+), 14 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 dd4f5485..36edcf96 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -22,6 +22,7 @@ import frc.trigon.robot.subsystems.swerve.SwerveCommands; import lib.hardware.RobotHardwareStats; import lib.hardware.misc.leds.LEDCommands; +import lib.utilities.flippable.Flippable; import lib.utilities.flippable.FlippablePose2d; import lib.utilities.flippable.FlippableRotation2d; import lib.utilities.flippable.FlippableTranslation2d; @@ -115,11 +116,7 @@ private static Command getScoreInNetCommand() { return new ParallelRaceGroup( ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_NET), getArmNetSequenceCommand(AlgaeManipulationCommands::shouldReverseNetScore), - SwerveCommands.getClosedLoopFieldRelativeDriveCommand( - () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), - () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), - () -> new FlippableRotation2d(shouldReverseNetScore() ? Rotation2d.kZero : Rotation2d.k180deg, true) - ).asProxy().onlyWhile(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY) + getDriveToNetCommand() ); } @@ -127,11 +124,7 @@ private static Command getScoreInProcessorCommand() { return new ParallelCommandGroup( ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_PROCESSOR), getArmProcessorSequenceCommand(), - SwerveCommands.getDriveToPoseCommand( - () -> FieldConstants.FLIPPABLE_PROCESSOR_SCORE_POSE, - AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS - ).until(() -> RobotContainer.SWERVE.atPose(FieldConstants.FLIPPABLE_PROCESSOR_SCORE_POSE)) - .asProxy().onlyWhile(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY) + getDriveToProcessorCommand() ); } @@ -149,6 +142,34 @@ private static Command getArmProcessorSequenceCommand() { ); } + private static Command getDriveToNetCommand() { + return new SequentialCommandGroup( + SwerveCommands.getDriveToPoseCommand( + AlgaeManipulationCommands::calculateClosestNetScoringPose, + AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS + ).until(() -> RobotContainer.SWERVE.atPose(calculateClosestNetScoringPose())), + SwerveCommands.getClosedLoopFieldRelativeDriveCommand( + () -> 0, + () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), + () -> new FlippableRotation2d(shouldReverseNetScore() ? Rotation2d.kZero : Rotation2d.k180deg, true) + ) + ).asProxy().onlyWhile(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY); + } + + private static Command getDriveToProcessorCommand() { + return new SequentialCommandGroup( + SwerveCommands.getDriveToPoseCommand( + () -> FieldConstants.FLIPPABLE_PROCESSOR_SCORE_POSE, + AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS + ).until(() -> RobotContainer.SWERVE.atPose(FieldConstants.FLIPPABLE_PROCESSOR_SCORE_POSE)), + SwerveCommands.getClosedLoopFieldRelativeDriveCommand( + () -> 0, + () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), + FieldConstants.FLIPPABLE_PROCESSOR_SCORE_POSE::getRotation + ) + ).asProxy().onlyWhile(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY); + } + private static int getAlgaeScoreMethodSelector() { if (OperatorConstants.SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean()) return 1; @@ -159,7 +180,7 @@ private static int getAlgaeScoreMethodSelector() { private static boolean shouldReverseNetScore() { final Rotation2d swerveAngle = RobotContainer.SWERVE.getDriveRelativeAngle(); - return swerveAngle.getDegrees() > Rotation2d.kCW_90deg.getDegrees() && swerveAngle.getDegrees() < Rotation2d.kCCW_90deg.getDegrees(); + return swerveAngle.getDegrees() < 90 && swerveAngle.getDegrees() > -90; } private static Command getInitiateFloorAlgaeCollectionCommand() { @@ -213,6 +234,17 @@ private static Command getAlgaeCollectionConfirmationCommand() { ); } + private static FlippablePose2d calculateClosestNetScoringPose() { + final Translation2d robotTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); + if (Flippable.isRedAlliance() ? robotTranslation.getY() < FieldConstants.FLIPPABLE_NET_SCORE_POSE.get().getY() : robotTranslation.getY() > FieldConstants.FLIPPABLE_NET_SCORE_POSE.get().getY()) + return new FlippablePose2d(new Pose2d( + FieldConstants.FLIPPABLE_NET_SCORE_POSE.get().getX(), + robotTranslation.getY(), + new Rotation2d() + ), false); + return FieldConstants.FLIPPABLE_NET_SCORE_POSE; + } + private static FlippablePose2d calculateClosestAlgaeCollectionPose() { final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); final Translation2d reefCenterPosition = new FlippableTranslation2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, true).get(); diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 3043a6ac..eee836e8 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -47,7 +47,9 @@ public class FieldConstants { public static final Rotation2d LEFT_FEEDER_ANGLE = Rotation2d.fromDegrees(54); public static final FlippableTranslation2d FLIPPABLE_REEF_CENTER_TRANSLATION = new FlippableTranslation2d(BLUE_REEF_CENTER_TRANSLATION, true); - public static final FlippablePose2d FLIPPABLE_PROCESSOR_SCORE_POSE = new FlippablePose2d(SimulatedGamePieceConstants.PROCESSOR_LOCATION.get().toPose2d(), true);//TODO: Find values + public static final FlippablePose2d + FLIPPABLE_PROCESSOR_SCORE_POSE = new FlippablePose2d(SimulatedGamePieceConstants.PROCESSOR_LOCATION.get().toPose2d(), true), + FLIPPABLE_NET_SCORE_POSE = new FlippablePose2d(SimulatedGamePieceConstants.NET_MINIMUM_X_LOCATION.get().toPose2d(), true); private static AprilTagFieldLayout createAprilTagFieldLayout() { try { 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 735cfa24..93a90793 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.util.Units; import frc.trigon.robot.constants.FieldConstants; +import lib.utilities.flippable.Flippable; import lib.utilities.flippable.FlippablePose3d; import lib.utilities.flippable.FlippableTranslation2d; @@ -70,8 +71,8 @@ public class SimulatedGamePieceConstants { public static final ArrayList CORAL_SCORING_LOCATIONS = calculatedCoralScoringLocations(); public static final FlippablePose3d PROCESSOR_LOCATION = new FlippablePose3d(new Pose3d(11.6, 7.5, 0.18, new Rotation3d(0, 0, Rotation2d.kCW_90deg.getRadians())), true), - NET_MINIMUM_X_LOCATION = new FlippablePose3d(new Pose3d(10.724, 4.730, 2.131, new Rotation3d()), true), - NET_MAXIMUM_X_LOCATION = new FlippablePose3d(new Pose3d(10.724, 8.429, 2.131, new Rotation3d()), true); + NET_MINIMUM_X_LOCATION = new FlippablePose3d(FlippablePose3d.flipPose(new Pose3d(FIELD_LENGTH_METERS - 10.724 + 0.82, 4.730, 2.131, new Rotation3d())), true), + NET_MAXIMUM_X_LOCATION = new FlippablePose3d(FlippablePose3d.flipPose(new Pose3d(FIELD_LENGTH_METERS - 10.724 + 0.82, 8.429, 2.131, new Rotation3d())), true); public static final FlippableTranslation2d LEFT_FEEDER_POSITION = new FlippableTranslation2d(0.923, 7.370, true), RIGHT_FEEDER_POSITION = new FlippableTranslation2d(0.923, 0.668, true); From eba36bb679c83f7b28ea5a2c4e22baeb7847c821 Mon Sep 17 00:00:00 2001 From: Nummun14 Date: Thu, 25 Sep 2025 21:56:37 -0400 Subject: [PATCH 30/51] added pid controller --- .../commands/commandfactories/AlgaeManipulationCommands.java | 4 ++-- 1 file changed, 2 insertions(+), 2 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 36edcf96..28db8514 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -149,7 +149,7 @@ private static Command getDriveToNetCommand() { AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS ).until(() -> RobotContainer.SWERVE.atPose(calculateClosestNetScoringPose())), SwerveCommands.getClosedLoopFieldRelativeDriveCommand( - () -> 0, + () -> AutonomousConstants.GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER.calculate(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getY(), calculateClosestNetScoringPose().get().getY()), () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), () -> new FlippableRotation2d(shouldReverseNetScore() ? Rotation2d.kZero : Rotation2d.k180deg, true) ) @@ -163,7 +163,7 @@ private static Command getDriveToProcessorCommand() { AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS ).until(() -> RobotContainer.SWERVE.atPose(FieldConstants.FLIPPABLE_PROCESSOR_SCORE_POSE)), SwerveCommands.getClosedLoopFieldRelativeDriveCommand( - () -> 0, + () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), FieldConstants.FLIPPABLE_PROCESSOR_SCORE_POSE::getRotation ) From 6e36c6f99e0142e8b66867531986ef773a79d9f6 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 26 Sep 2025 05:24:46 +0300 Subject: [PATCH 31/51] ver yyes --- .../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 36edcf96..ad373362 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -89,7 +89,7 @@ private static Command getAlignToReefCommand() { () -> calculateClosestAlgaeCollectionPose().getRotation() ) ).raceWith( - new WaitCommand(1).andThen(new WaitUntilCommand(RobotContainer.ARM::hasGamePiece)), + new WaitUntilCommand(RobotContainer.ARM::hasGamePiece), new WaitUntilCommand(OperatorConstants.STOP_REEF_ALGAE_ALIGN_TRIGGER) ).onlyIf(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY).asProxy(); } From bc006a4e3d59b3d879c15230ba3d4f2740125a53 Mon Sep 17 00:00:00 2001 From: Nummun14 Date: Thu, 25 Sep 2025 22:47:51 -0400 Subject: [PATCH 32/51] code rabbit is actually op --- .../commandfactories/AlgaeManipulationCommands.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 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 28db8514..7b85ea11 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -274,7 +274,9 @@ private static double fieldRelativePowersToSelfRelativeXPower(double xPower, dou return (xValue * robotHeading.getCos()) + (yValue * robotHeading.getSin()); } - private static double calculateReefRelativeYOffset() {//TODO: Implement - return 0; + private static double calculateReefRelativeYOffset() { + final Pose2d target = calculateClosestAlgaeCollectionPose().get(); + final Pose2d robot = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + return robot.relativeTo(target).getY(); } } \ No newline at end of file From 7ff39e8a721767ef895673d2c47949324f00b7e1 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 26 Sep 2025 05:54:45 +0300 Subject: [PATCH 33/51] s --- .../java/frc/trigon/robot/constants/OperatorConstants.java | 3 +-- 1 file changed, 1 insertion(+), 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 684d9e07..375e2214 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -3,7 +3,6 @@ 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; @@ -52,7 +51,7 @@ public class OperatorConstants { public static final Trigger FLOOR_ALGAE_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftBumper(),//TODO: Add operator control REEF_ALGAE_COLLECTION_TRIGGER = DRIVER_CONTROLLER.rightBumper().or(OPERATOR_CONTROLLER.a()), - STOP_REEF_ALGAE_ALIGN_TRIGGER = DRIVER_CONTROLLER.x().and(RobotContainer.ARM::hasGamePiece), + STOP_REEF_ALGAE_ALIGN_TRIGGER = DRIVER_CONTROLLER.povLeft(), SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.n().or(createScoreTrigger(true, true)), SCORE_ALGAE_IN_PROCESSOR_TRIGGER = OPERATOR_CONTROLLER.j().or(createScoreTrigger(false, true)), CORAL_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftTrigger().or(OPERATOR_CONTROLLER.c()), From 586157167e0c11c7eee5cce1747faf16302ad65a Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 26 Sep 2025 07:09:31 +0300 Subject: [PATCH 34/51] Networks --- .../AlgaeManipulationCommands.java | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 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 c2b0594a..445ec410 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -149,7 +149,7 @@ private static Command getDriveToNetCommand() { AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS ).until(() -> RobotContainer.SWERVE.atPose(calculateClosestNetScoringPose())), SwerveCommands.getClosedLoopFieldRelativeDriveCommand( - () -> AutonomousConstants.GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER.calculate(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getY(), calculateClosestNetScoringPose().get().getY()), + () -> -AutonomousConstants.GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER.calculate(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getX(), calculateClosestNetScoringPose().get().getX()), () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), () -> new FlippableRotation2d(shouldReverseNetScore() ? Rotation2d.kZero : Rotation2d.k180deg, true) ) @@ -157,17 +157,13 @@ private static Command getDriveToNetCommand() { } private static Command getDriveToProcessorCommand() { - return new SequentialCommandGroup( - SwerveCommands.getDriveToPoseCommand( + return SwerveCommands.getDriveToPoseCommand( () -> FieldConstants.FLIPPABLE_PROCESSOR_SCORE_POSE, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS - ).until(() -> RobotContainer.SWERVE.atPose(FieldConstants.FLIPPABLE_PROCESSOR_SCORE_POSE)), - SwerveCommands.getClosedLoopFieldRelativeDriveCommand( - () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), - () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), - FieldConstants.FLIPPABLE_PROCESSOR_SCORE_POSE::getRotation ) - ).asProxy().onlyWhile(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY); + .asProxy() + .until(() -> RobotContainer.SWERVE.atPose(FieldConstants.FLIPPABLE_PROCESSOR_SCORE_POSE)) + .onlyWhile(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY); } private static int getAlgaeScoreMethodSelector() { @@ -240,7 +236,7 @@ private static FlippablePose2d calculateClosestNetScoringPose() { return new FlippablePose2d(new Pose2d( FieldConstants.FLIPPABLE_NET_SCORE_POSE.get().getX(), robotTranslation.getY(), - new Rotation2d() + shouldReverseNetScore() ? Rotation2d.k180deg : Rotation2d.kZero ), false); return FieldConstants.FLIPPABLE_NET_SCORE_POSE; } From a49c49f1cef460df9ee0a297a06634c2ac9db9fa Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 26 Sep 2025 07:10:35 +0300 Subject: [PATCH 35/51] Super cool stuff --- .../commandfactories/AlgaeManipulationCommands.java | 6 +++--- 1 file 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 445ec410..49aa358b 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 class AlgaeManipulationCommands { private static boolean IS_HOLDING_ALGAE = false, SHOULD_COLLECT_FROM_LOLLIPOP = false; - private static final PIDController ALIGN_TO_REEF_Y_CONTROLLER = + private static final PIDController SMALL_POSITION_ALIGN_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? new PIDController(3, 0, 0) : new PIDController(3, 0, 0); @@ -85,7 +85,7 @@ private static Command getAlignToReefCommand() { ), SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> fieldRelativePowersToSelfRelativeXPower(OperatorConstants.DRIVER_CONTROLLER.getLeftY(), OperatorConstants.DRIVER_CONTROLLER.getLeftX()), - () -> ALIGN_TO_REEF_Y_CONTROLLER.calculate(calculateReefRelativeYOffset()), + () -> SMALL_POSITION_ALIGN_PID_CONTROLLER.calculate(calculateReefRelativeYOffset()), () -> calculateClosestAlgaeCollectionPose().getRotation() ) ).raceWith( @@ -149,7 +149,7 @@ private static Command getDriveToNetCommand() { AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS ).until(() -> RobotContainer.SWERVE.atPose(calculateClosestNetScoringPose())), SwerveCommands.getClosedLoopFieldRelativeDriveCommand( - () -> -AutonomousConstants.GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER.calculate(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getX(), calculateClosestNetScoringPose().get().getX()), + () -> -SMALL_POSITION_ALIGN_PID_CONTROLLER.calculate(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getX(), calculateClosestNetScoringPose().get().getX()), () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), () -> new FlippableRotation2d(shouldReverseNetScore() ? Rotation2d.kZero : Rotation2d.k180deg, true) ) From 6662ca8413349c40471a57d049357ad7ae64fcfd Mon Sep 17 00:00:00 2001 From: Nummun14 Date: Fri, 26 Sep 2025 01:14:49 -0400 Subject: [PATCH 36/51] dumb stuff --- .../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 49aa358b..82a23530 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -85,7 +85,7 @@ private static Command getAlignToReefCommand() { ), SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> fieldRelativePowersToSelfRelativeXPower(OperatorConstants.DRIVER_CONTROLLER.getLeftY(), OperatorConstants.DRIVER_CONTROLLER.getLeftX()), - () -> SMALL_POSITION_ALIGN_PID_CONTROLLER.calculate(calculateReefRelativeYOffset()), + () -> SMALL_POSITION_ALIGN_PID_CONTROLLER.calculate(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getY(), calculateClosestAlgaeCollectionPose().get().getY()), () -> calculateClosestAlgaeCollectionPose().getRotation() ) ).raceWith( From 2e4beb95260999d1fb8a96bbcbfff2af978ebadd Mon Sep 17 00:00:00 2001 From: Nummun14 Date: Fri, 26 Sep 2025 04:39:47 -0400 Subject: [PATCH 37/51] no need --- .../commandfactories/AlgaeManipulationCommands.java | 6 ------ 1 file changed, 6 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 82a23530..c9210049 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -269,10 +269,4 @@ private static double fieldRelativePowersToSelfRelativeXPower(double xPower, dou return (xValue * robotHeading.getCos()) + (yValue * robotHeading.getSin()); } - - private static double calculateReefRelativeYOffset() { - final Pose2d target = calculateClosestAlgaeCollectionPose().get(); - final Pose2d robot = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); - return robot.relativeTo(target).getY(); - } } \ No newline at end of file From 62dd44671f34a7e3cf400b38054d07de001a4c01 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 26 Sep 2025 17:41:34 +0300 Subject: [PATCH 38/51] The commit to fix all commits --- .../AlgaeManipulationCommands.java | 29 ++++++++++++------- .../transporter/TransporterConstants.java | 3 +- 2 files changed, 20 insertions(+), 12 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 c9210049..f6ba5b44 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -85,7 +85,7 @@ private static Command getAlignToReefCommand() { ), SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> fieldRelativePowersToSelfRelativeXPower(OperatorConstants.DRIVER_CONTROLLER.getLeftY(), OperatorConstants.DRIVER_CONTROLLER.getLeftX()), - () -> SMALL_POSITION_ALIGN_PID_CONTROLLER.calculate(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getY(), calculateClosestAlgaeCollectionPose().get().getY()), + () -> SMALL_POSITION_ALIGN_PID_CONTROLLER.calculate(calculateReefRelativeYOffset(), 0), () -> calculateClosestAlgaeCollectionPose().getRotation() ) ).raceWith( @@ -125,7 +125,7 @@ private static Command getScoreInProcessorCommand() { ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_PROCESSOR), getArmProcessorSequenceCommand(), getDriveToProcessorCommand() - ); + ).finallyDo(GeneralCommands.getFieldRelativeDriveCommand()::schedule); } private static Command getArmNetSequenceCommand(BooleanSupplier shouldReverseScore) { @@ -160,8 +160,7 @@ private static Command getDriveToProcessorCommand() { return SwerveCommands.getDriveToPoseCommand( () -> FieldConstants.FLIPPABLE_PROCESSOR_SCORE_POSE, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS - ) - .asProxy() + ).asProxy() .until(() -> RobotContainer.SWERVE.atPose(FieldConstants.FLIPPABLE_PROCESSOR_SCORE_POSE)) .onlyWhile(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY); } @@ -193,7 +192,10 @@ private static Command getCollectAlgaeFromLollipopSequenceCommand() { return new ParallelCommandGroup( GeneralCommands.getFlippableOverridableArmCommand(ArmConstants.ArmState.COLLECT_ALGAE_LOLLIPOP, false), ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.COLLECT_ALGAE_LOLLIPOP), - getIntakeCoralFromLollipopCommand().onlyWhile(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE).repeatedly().asProxy() + getIntakeCoralFromLollipopCommand() + .onlyIf(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE) + .until(() -> !OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE) + .repeatedly() ); } @@ -205,12 +207,11 @@ private static Command getCollectAlgaeFromFloorSequenceCommand() { } private static Command getIntakeCoralFromLollipopCommand() { - return new SequentialCommandGroup( - CoralCollectionCommands.getCoralCollectionCommand() - .unless(RobotContainer.TRANSPORTER::hasCoral) - .until(RobotContainer.TRANSPORTER::hasCoral), - IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.OPEN_REST) - ); + return GeneralCommands.getContinuousConditionalCommand( + IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.OPEN_REST), + CoralCollectionCommands.getCoralCollectionCommand(), + RobotContainer.TRANSPORTER::hasCoral + ).asProxy(); } private static Command getInitiateReefAlgaeCollectionCommand() { @@ -269,4 +270,10 @@ private static double fieldRelativePowersToSelfRelativeXPower(double xPower, dou return (xValue * robotHeading.getCos()) + (yValue * robotHeading.getSin()); } + + private static double calculateReefRelativeYOffset() { + final Pose2d target = calculateClosestAlgaeCollectionPose().get(); + final Pose2d robot = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + return robot.relativeTo(target).getY(); + } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java b/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java index 3b2c2e57..4ec9b904 100644 --- a/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java @@ -9,6 +9,7 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.event.BooleanEvent; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; import lib.hardware.misc.simplesensor.SimpleSensor; import lib.hardware.phoenix6.talonfx.TalonFXMotor; import lib.hardware.phoenix6.talonfx.TalonFXSignal; @@ -47,7 +48,7 @@ public class TransporterConstants { GEAR_RATIO, MOMENT_OF_INERTIA ); - private static final DoubleSupplier BEAM_BREAK_SIMULATION_SUPPLIER = () -> 0; //TODO: implement + private static final DoubleSupplier BEAM_BREAK_SIMULATION_SUPPLIER = () -> SimulationFieldHandler.isHoldingCoral() && !SimulationFieldHandler.isCoralInEndEffector() ? 1 : 0; private static final double MAXIMUM_DISPLAYABLE_VELOCITY = 12; static final SpeedMechanism2d From cf9ddeafb4933e4238add197c5a1744830cf8843 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 26 Sep 2025 17:48:29 +0300 Subject: [PATCH 39/51] lamb duh --- 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 ecf39988..5797af37 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -110,7 +110,7 @@ private void bindControllerCommands() { OperatorConstants.SPAWN_CORAL_IN_SIMULATION_TRIGGER.onTrue(new InstantCommand(() -> SimulationFieldHandler.updateCoralSpawning(new Pose3d(ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose())))); OperatorConstants.FLIP_ARM_TRIGGER.onTrue(new InstantCommand(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE = !OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE)); - OperatorConstants.LOLLIPOP_ALGAE_TOGGLE_TRIGGER.onTrue(new InstantCommand(() -> AlgaeManipulationCommands.toggleLollipopCollection())); + OperatorConstants.LOLLIPOP_ALGAE_TOGGLE_TRIGGER.onTrue(new InstantCommand(AlgaeManipulationCommands::toggleLollipopCollection)); } private void configureSysIDBindings(MotorSubsystem subsystem) { From c6ddc98295961ba146bd8f02c6abe73707d95af5 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Sun, 28 Sep 2025 00:23:25 +0300 Subject: [PATCH 40/51] Update ArmElevatorConstants.java --- .../frc/trigon/robot/subsystems/arm/ArmElevatorConstants.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorConstants.java index 1ee51cef..10a705b6 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorConstants.java @@ -344,6 +344,7 @@ public enum ArmElevatorState { SCORE_PROCESSOR(Rotation2d.fromDegrees(90), 0.603, null, false, 0.7), COLLECT_ALGAE_L2(Rotation2d.fromDegrees(90), 0.603, null, false, 1), COLLECT_ALGAE_L3(Rotation2d.fromDegrees(90), 0.953, null, false, 1), + COLLECT_ALGAE_LOLLIPOP(Rotation2d.fromDegrees(70), 0.3, null, false, 1), PREPARE_COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(60), 0.2, null, false, 1), COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(50), 0.2, PREPARE_COLLECT_ALGAE_FLOOR, true, 1); From 8c05c04b48ba2b4f8f6c20fed25c1217c2e7e231 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Sun, 28 Sep 2025 00:46:29 +0300 Subject: [PATCH 41/51] no erros, still need to test though --- .../AlgaeManipulationCommands.java | 54 +++++++++---------- .../CoralCollectionCommands.java | 7 +++ .../CoralPlacingCommands.java | 22 +++----- .../commandfactories/GeneralCommands.java | 22 ++++---- .../frc/trigon/robot/misc/ReefChooser.java | 4 ++ .../subsystems/arm/ArmElevatorConstants.java | 1 + .../endeffector/EndEffectorConstants.java | 1 + 7 files changed, 58 insertions(+), 53 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 f6ba5b44..9ee5e899 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -13,10 +13,10 @@ import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.constants.LEDConstants; import frc.trigon.robot.constants.OperatorConstants; -import frc.trigon.robot.subsystems.arm.ArmCommands; -import frc.trigon.robot.subsystems.arm.ArmConstants; -import frc.trigon.robot.subsystems.elevator.ElevatorCommands; -import frc.trigon.robot.subsystems.elevator.ElevatorConstants; +import frc.trigon.robot.subsystems.arm.ArmElevatorCommands; +import frc.trigon.robot.subsystems.arm.ArmElevatorConstants; +import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands; +import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants; import frc.trigon.robot.subsystems.intake.IntakeCommands; import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; @@ -55,11 +55,11 @@ public static Command getFloorAlgaeCollectionCommand() { IS_HOLDING_ALGAE = true; SHOULD_COLLECT_FROM_LOLLIPOP = false; }), - CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.ARM::hasGamePiece), - getInitiateFloorAlgaeCollectionCommand().until(RobotContainer.ARM::hasGamePiece), + CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.END_EFFECTOR::hasGamePiece), + getInitiateFloorAlgaeCollectionCommand().until(RobotContainer.END_EFFECTOR::hasGamePiece), GeneralCommands.getResetFlipArmOverrideCommand(), getScoreAlgaeCommand().alongWith(getAlgaeCollectionConfirmationCommand()) - .until(() -> !RobotContainer.ARM.hasGamePiece() && !isScoreAlgaeButtonPressed()) + .until(() -> !RobotContainer.END_EFFECTOR.hasGamePiece() && !isScoreAlgaeButtonPressed()) ).finallyDo(() -> IS_HOLDING_ALGAE = false); } @@ -67,11 +67,11 @@ public static Command getReefAlgaeCollectionCommand() { return new SequentialCommandGroup( GeneralCommands.getResetFlipArmOverrideCommand(), new InstantCommand(() -> IS_HOLDING_ALGAE = true), - CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.ARM::hasGamePiece), - getInitiateReefAlgaeCollectionCommand().until(RobotContainer.ARM::hasGamePiece), + CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.END_EFFECTOR::hasGamePiece), + getInitiateReefAlgaeCollectionCommand().until(RobotContainer.END_EFFECTOR::hasGamePiece), GeneralCommands.getResetFlipArmOverrideCommand(), getScoreAlgaeCommand().alongWith(getAlgaeCollectionConfirmationCommand()) - .until(() -> !RobotContainer.ARM.hasGamePiece() && !isScoreAlgaeButtonPressed()) + .until(() -> !RobotContainer.END_EFFECTOR.hasGamePiece() && !isScoreAlgaeButtonPressed()) ) .alongWith(getAlignToReefCommand()) .finallyDo(() -> IS_HOLDING_ALGAE = false); @@ -89,7 +89,7 @@ private static Command getAlignToReefCommand() { () -> calculateClosestAlgaeCollectionPose().getRotation() ) ).raceWith( - new WaitUntilCommand(RobotContainer.ARM::hasGamePiece), + new WaitUntilCommand(RobotContainer.END_EFFECTOR::hasGamePiece), new WaitUntilCommand(OperatorConstants.STOP_REEF_ALGAE_ALIGN_TRIGGER) ).onlyIf(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY).asProxy(); } @@ -107,14 +107,13 @@ private static Command getScoreAlgaeCommand() { private static Command getHoldAlgaeCommand() { return new ParallelCommandGroup( - ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.HOLD_ALGAE), - ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST_WITH_ALGAE) + ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.REST_WITH_ALGAE), + EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.HOLD_ALGAE) ); } private static Command getScoreInNetCommand() { return new ParallelRaceGroup( - ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_NET), getArmNetSequenceCommand(AlgaeManipulationCommands::shouldReverseNetScore), getDriveToNetCommand() ); @@ -122,7 +121,6 @@ private static Command getScoreInNetCommand() { private static Command getScoreInProcessorCommand() { return new ParallelCommandGroup( - ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_PROCESSOR), getArmProcessorSequenceCommand(), getDriveToProcessorCommand() ).finallyDo(GeneralCommands.getFieldRelativeDriveCommand()::schedule); @@ -130,15 +128,15 @@ private static Command getScoreInProcessorCommand() { private static Command getArmNetSequenceCommand(BooleanSupplier shouldReverseScore) { return new SequentialCommandGroup( - GeneralCommands.getFlippableOverridableArmCommand(ArmConstants.ArmState.SCORE_NET, true, shouldReverseScore).until(OperatorConstants.CONTINUE_TRIGGER), - GeneralCommands.getFlippableOverridableArmCommand(ArmConstants.ArmState.SCORE_NET, false, shouldReverseScore) + GeneralCommands.getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState.SCORE_NET, true, shouldReverseScore).until(OperatorConstants.CONTINUE_TRIGGER), + EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_ALGAE) ); } private static Command getArmProcessorSequenceCommand() { return new SequentialCommandGroup( - GeneralCommands.getFlippableOverridableArmCommand(ArmConstants.ArmState.SCORE_PROCESSOR, true).until(OperatorConstants.CONTINUE_TRIGGER), - GeneralCommands.getFlippableOverridableArmCommand(ArmConstants.ArmState.SCORE_PROCESSOR, false) + GeneralCommands.getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState.SCORE_PROCESSOR, true).until(OperatorConstants.CONTINUE_TRIGGER), + EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_ALGAE) ); } @@ -190,8 +188,10 @@ private static Command getInitiateFloorAlgaeCollectionCommand() { private static Command getCollectAlgaeFromLollipopSequenceCommand() { return new ParallelCommandGroup( - GeneralCommands.getFlippableOverridableArmCommand(ArmConstants.ArmState.COLLECT_ALGAE_LOLLIPOP, false), - ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.COLLECT_ALGAE_LOLLIPOP), + new SequentialCommandGroup( + GeneralCommands.getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_LOLLIPOP, false), + EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.COLLECT_ALGAE) + ), getIntakeCoralFromLollipopCommand() .onlyIf(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE) .until(() -> !OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE) @@ -201,8 +201,8 @@ private static Command getCollectAlgaeFromLollipopSequenceCommand() { private static Command getCollectAlgaeFromFloorSequenceCommand() { return new ParallelCommandGroup( - ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.COLLECT_ALGAE_FLOOR), - ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.COLLECT_ALGAE_FLOOR) + ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_FLOOR), + EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.COLLECT_ALGAE) ); } @@ -216,11 +216,11 @@ private static Command getIntakeCoralFromLollipopCommand() { private static Command getInitiateReefAlgaeCollectionCommand() { return new ParallelCommandGroup( - GeneralCommands.getFlippableOverridableArmCommand(OperatorConstants.REEF_CHOOSER::getArmAlgaeCollectionState, false, CoralPlacingCommands::shouldReverseScore), - ElevatorCommands.getSetTargetStateCommand(OperatorConstants.REEF_CHOOSER::getElevatorAlgaeCollectionState) + GeneralCommands.getFlippableOverridableArmCommand(OperatorConstants.REEF_CHOOSER::getArmElevatorAlgaeCollectionState, false, CoralPlacingCommands::shouldReverseScore), + EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.COLLECT_ALGAE) ).raceWith( - new WaitUntilChangeCommand<>(OperatorConstants.REEF_CHOOSER::getArmAlgaeCollectionState), - new WaitUntilChangeCommand<>(OperatorConstants.REEF_CHOOSER::getElevatorAlgaeCollectionState) + new WaitUntilChangeCommand<>(OperatorConstants.REEF_CHOOSER::getArmElevatorAlgaeCollectionState), + new WaitUntilChangeCommand<>(OperatorConstants.REEF_CHOOSER::getArmElevatorAlgaeCollectionState) ).repeatedly(); } 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 cb765841..5340d913 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -39,6 +39,13 @@ public static Command getLoadCoralCommand() { ).until(RobotContainer.END_EFFECTOR::hasGamePiece).onlyWhile(() -> !RobotContainer.END_EFFECTOR.hasGamePiece()); } + public static Command getUnloadCoralCommand() { + return new ParallelCommandGroup( + ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.UNLOAD_CORAL), + EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.UNLOAD_CORAL) + ).until(() -> !RobotContainer.END_EFFECTOR.hasGamePiece() && RobotContainer.INTAKE.hasCoral()); + } + private static Command getIntakeSequenceCommand() { return new SequentialCommandGroup( getInitiateCollectionCommand().until(RobotContainer.INTAKE::hasCoral), 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 2eb8121a..f96b1bf7 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -13,7 +13,6 @@ import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.misc.ReefChooser; -import frc.trigon.robot.subsystems.arm.ArmConstants; import frc.trigon.robot.subsystems.arm.ArmElevatorCommands; import frc.trigon.robot.subsystems.arm.ArmElevatorConstants; import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands; @@ -57,16 +56,6 @@ private static Command getScoreCommand(boolean shouldScoreRight) { ); } - private static Command getAutonomouslyScoreCommand(boolean shouldScoreRight) { - return new SequentialCommandGroup( - getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(() -> isReadyToPlace(shouldScoreRight) || OperatorConstants.CONTINUE_TRIGGER.getAsBoolean()), - new ParallelCommandGroup( - ArmElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getArmElevatorState, CoralPlacingCommands::shouldReverseScore), - EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_CORAL) - ) - ); - } - private static Command getAutonomouslyPrepareScoreCommand(boolean shouldScoreRight) { return new ParallelCommandGroup( ArmElevatorCommands.getPrepareForStateCommand(REEF_CHOOSER::getArmElevatorState, CoralPlacingCommands::shouldReverseScore), @@ -146,7 +135,9 @@ public enum ScoringLevel { L3(L2.xTransformMeters, L2.positiveYTransformMeters, Rotation2d.fromDegrees(0)), L4(L2.xTransformMeters, L2.positiveYTransformMeters, Rotation2d.fromDegrees(0)); - public final ArmElevatorConstants.ArmElevatorState armElevatorState; + public final ArmElevatorConstants.ArmElevatorState + armElevatorState, + armElevatorAlgaeCollectionState; public final int level = calculateLevel(); final double xTransformMeters, positiveYTransformMeters; final Rotation2d rotationTransform; @@ -165,6 +156,7 @@ public enum ScoringLevel { this.positiveYTransformMeters = positiveYTransformMeters; this.rotationTransform = rotationTransform; this.armElevatorState = determineArmElevatorState(); + this.armElevatorAlgaeCollectionState = determineArmElevatorAlgaeCollectionState(); } /** @@ -196,10 +188,10 @@ private ArmElevatorConstants.ArmElevatorState determineArmElevatorState() { }; } - private ArmConstants.ArmState determineArmAlgaeCollectionState() { + private ArmElevatorConstants.ArmElevatorState determineArmElevatorAlgaeCollectionState() { return switch (level) { - case 1, 2 -> ArmConstants.ArmState.COLLECT_ALGAE_L2; - case 3, 4 -> ArmConstants.ArmState.COLLECT_ALGAE_L3; + case 1, 2 -> ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_L2; + case 3, 4 -> ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_L3; default -> throw new IllegalStateException("Unexpected value: " + ordinal()); }; } diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java index b8695955..c9346520 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java @@ -4,8 +4,8 @@ import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.MotorSubsystem; -import frc.trigon.robot.subsystems.arm.ArmCommands; -import frc.trigon.robot.subsystems.arm.ArmConstants; +import frc.trigon.robot.subsystems.arm.ArmElevatorCommands; +import frc.trigon.robot.subsystems.arm.ArmElevatorConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; import java.util.function.BooleanSupplier; @@ -70,21 +70,21 @@ public static Command getResetFlipArmOverrideCommand() { return new InstantCommand(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE = false); } - public static Command getFlippableOverridableArmCommand(ArmConstants.ArmState targetState, boolean isPrepareState, BooleanSupplier shouldStartFlipped) { + public static Command getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState targetState, boolean isPrepareState, BooleanSupplier shouldStartFlipped) { return isPrepareState ? - ArmCommands.getPrepareForStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped.getAsBoolean()) : - ArmCommands.getSetTargetStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped.getAsBoolean()); + ArmElevatorCommands.getPrepareForStateCommand(() -> targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped.getAsBoolean()) : + ArmElevatorCommands.getSetTargetStateCommand(() -> targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped.getAsBoolean()); } - public static Command getFlippableOverridableArmCommand(Supplier targetState, boolean isPrepareState, BooleanSupplier shouldStartFlipped) { + public static Command getFlippableOverridableArmCommand(Supplier targetState, boolean isPrepareState, BooleanSupplier shouldStartFlipped) { return isPrepareState ? - ArmCommands.getPrepareForStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped.getAsBoolean()) : - ArmCommands.getSetTargetStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped.getAsBoolean()); + ArmElevatorCommands.getPrepareForStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped.getAsBoolean()) : + ArmElevatorCommands.getSetTargetStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped.getAsBoolean()); } - public static Command getFlippableOverridableArmCommand(ArmConstants.ArmState targetState, boolean isPrepareState) { + public static Command getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState targetState, boolean isPrepareState) { return isPrepareState ? - ArmCommands.getPrepareForStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE) : - ArmCommands.getSetTargetStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE); + ArmElevatorCommands.getPrepareForStateCommand(() -> targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE) : + ArmElevatorCommands.getSetTargetStateCommand(() -> targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE); } } \ 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 3c02d966..c2275a77 100644 --- a/src/main/java/frc/trigon/robot/misc/ReefChooser.java +++ b/src/main/java/frc/trigon/robot/misc/ReefChooser.java @@ -48,6 +48,10 @@ public ArmElevatorConstants.ArmElevatorState getArmElevatorState() { return scoringLevel.armElevatorState; } + public ArmElevatorConstants.ArmElevatorState getArmElevatorAlgaeCollectionState() { + return scoringLevel.armElevatorAlgaeCollectionState; + } + private void configureBindings() { configureScoringLevelBindings(); configureReefHIDBindings(); diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorConstants.java index 10a705b6..de2f7494 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorConstants.java @@ -335,6 +335,7 @@ public enum ArmElevatorState { REST_WITH_ALGAE(Rotation2d.fromDegrees(90), 0.603, null, false, 0.7), REST_FOR_CLIMB(Rotation2d.fromDegrees(0), 0.603, null, false, 0.7), LOAD_CORAL(Rotation2d.fromDegrees(0), 0.5519, REST, true, 0.7), + UNLOAD_CORAL(Rotation2d.fromDegrees(0), 0.5519, null, false, 0.7), EJECT(Rotation2d.fromDegrees(60), 0.603, null, false, 0.7), SCORE_L1(Rotation2d.fromDegrees(45), 0.603, null, false, 1), SCORE_L2(Rotation2d.fromDegrees(90), 0.3, PREPARE_SCORE_L2, false, 1), diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java index e0c3afbe..d9634fe5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java @@ -87,6 +87,7 @@ public enum EndEffectorState { REST(0), EJECT(4), LOAD_CORAL(-4), + UNLOAD_CORAL(4), SCORE_CORAL(4), COLLECT_ALGAE(-4), SCORE_ALGAE(4), From d8947fd29da37d077021ab1e5a0be657a309454f Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 28 Sep 2025 01:30:47 +0300 Subject: [PATCH 42/51] Some small things racked up --- src/main/java/frc/trigon/robot/RobotContainer.java | 3 +-- .../commandfactories/AlgaeManipulationCommands.java | 3 +-- .../commands/commandfactories/GeneralCommands.java | 8 +++++++- .../frc/trigon/robot/constants/OperatorConstants.java | 11 ++--------- .../misc/simulatedfield/SimulationFieldHandler.java | 7 ++++--- 5 files changed, 15 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 129c2a24..1860152a 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -6,7 +6,6 @@ package frc.trigon.robot; import com.pathplanner.lib.auto.AutoBuilder; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -108,7 +107,7 @@ private void bindControllerCommands() { OperatorConstants.SCORE_CORAL_RIGHT_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(true)); OperatorConstants.EJECT_CORAL_TRIGGER.whileTrue(CoralEjectionCommands.getCoralEjectionCommand()); - OperatorConstants.SPAWN_CORAL_IN_SIMULATION_TRIGGER.onTrue(new InstantCommand(() -> SimulationFieldHandler.updateCoralSpawning(new Pose3d(ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose())))); + OperatorConstants.SPAWN_CORAL_IN_SIMULATION_TRIGGER.onTrue(new InstantCommand(() -> SimulationFieldHandler.updateCoralSpawning(ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose()))); OperatorConstants.FLIP_ARM_TRIGGER.onTrue(new InstantCommand(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE = !OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE)); OperatorConstants.LOLLIPOP_ALGAE_TOGGLE_TRIGGER.onTrue(new InstantCommand(AlgaeManipulationCommands::toggleLollipopCollection)); } 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 9ee5e899..3b4605c9 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -219,7 +219,6 @@ private static Command getInitiateReefAlgaeCollectionCommand() { GeneralCommands.getFlippableOverridableArmCommand(OperatorConstants.REEF_CHOOSER::getArmElevatorAlgaeCollectionState, false, CoralPlacingCommands::shouldReverseScore), EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.COLLECT_ALGAE) ).raceWith( - new WaitUntilChangeCommand<>(OperatorConstants.REEF_CHOOSER::getArmElevatorAlgaeCollectionState), new WaitUntilChangeCommand<>(OperatorConstants.REEF_CHOOSER::getArmElevatorAlgaeCollectionState) ).repeatedly(); } @@ -247,7 +246,7 @@ private static FlippablePose2d calculateClosestAlgaeCollectionPose() { final Translation2d reefCenterPosition = new FlippableTranslation2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, true).get(); final Rotation2d[] reefClockAngles = FieldConstants.REEF_CLOCK_ANGLES; final Transform2d reefCenterToBranchScoringPose = new Transform2d(FieldConstants.REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS, 0, new Rotation2d()); - final List scoringPoses = new java.util.ArrayList<>(List.of()); + final List scoringPoses = new java.util.ArrayList<>(); for (Rotation2d reefClockAngle : reefClockAngles) { final Pose2d reefCenterAtTargetRotation = new Pose2d(reefCenterPosition, reefClockAngle); diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java index c9346520..6828de9a 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java @@ -1,5 +1,6 @@ package frc.trigon.robot.commands.commandfactories; +import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.constants.OperatorConstants; @@ -55,6 +56,7 @@ public static Command runWhen(Command command, BooleanSupplier condition) { } /** + * ---- UNTESTED ---- * A command that only runs when a condition is met for a certain amount of time. * * @param command the command to run @@ -63,7 +65,11 @@ public static Command runWhen(Command command, BooleanSupplier condition) { * @return the command */ public static Command runWhen(Command command, BooleanSupplier condition, double debounceTimeSeconds) { - return runWhen(new WaitCommand(debounceTimeSeconds).andThen(command.onlyIf(condition)), condition); + final Debouncer debouncer = new Debouncer(0, Debouncer.DebounceType.kRising); + return new SequentialCommandGroup( + new InstantCommand(() -> debouncer.setDebounceTime(debounceTimeSeconds)), + runWhen(command, () -> debouncer.calculate(condition.getAsBoolean())) + ); } public static Command getResetFlipArmOverrideCommand() { diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 375e2214..29fe13ae 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -1,7 +1,6 @@ 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.commands.commandclasses.IntakeAssistCommand; import frc.trigon.robot.commands.commandfactories.AlgaeManipulationCommands; @@ -81,15 +80,9 @@ private static Trigger createScoreTrigger(boolean isRight, boolean isAlgaeComman final Trigger scoreTrigger; if (isRight) - scoreTrigger = 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)); + scoreTrigger = DRIVER_CONTROLLER.rightStick().and(DRIVER_CONTROLLER.leftStick().negate()); else - scoreTrigger = DRIVER_CONTROLLER.leftStick() - .and(() -> !IS_RIGHT_SCORE_BUTTON_PRESSED) - .onTrue(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = true)) - .onFalse(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = false)); + scoreTrigger = DRIVER_CONTROLLER.leftStick().and(DRIVER_CONTROLLER.rightStick().negate()); if (isAlgaeCommand) return scoreTrigger.and(AlgaeManipulationCommands::isHoldingAlgae); 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 6f2e4a7f..b7ed0f7b 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -1,5 +1,6 @@ package frc.trigon.robot.misc.simulatedfield; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform3d; import frc.trigon.robot.RobotContainer; @@ -89,10 +90,10 @@ private static void updateCollection() { HELD_ALGAE_INDEX = getIndexOfCollectedGamePiece(algaeCollectionPose, ALGAE_ON_FIELD, SimulatedGamePieceConstants.ALGAE_INTAKE_TOLERANCE_METERS); } - public static void updateCoralSpawning(Pose3d robotPose) { + public static void updateCoralSpawning(Pose2d robotPose) { final double - distanceFromLeftFeeder = robotPose.toPose2d().getTranslation().getDistance(SimulatedGamePieceConstants.LEFT_FEEDER_POSITION.get()), - distanceFromRightFeeder = robotPose.toPose2d().getTranslation().getDistance(SimulatedGamePieceConstants.RIGHT_FEEDER_POSITION.get()); + distanceFromLeftFeeder = robotPose.getTranslation().getDistance(SimulatedGamePieceConstants.LEFT_FEEDER_POSITION.get()), + distanceFromRightFeeder = robotPose.getTranslation().getDistance(SimulatedGamePieceConstants.RIGHT_FEEDER_POSITION.get()); final FlippablePose3d coralSpawnPose = distanceFromLeftFeeder < distanceFromRightFeeder ? SimulatedGamePieceConstants.LEFT_CORAL_SPAWN_POSE : SimulatedGamePieceConstants.RIGHT_CORAL_SPAWN_POSE; From 64db523ab2f65edbe71487c0f2d2ae173aff5c9f Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 28 Sep 2025 12:42:52 +0300 Subject: [PATCH 43/51] Fix --- .../robot/commands/commandfactories/CoralPlacingCommands.java | 4 ++-- src/main/java/frc/trigon/robot/constants/FieldConstants.java | 4 +++- 2 files changed, 5 insertions(+), 3 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 2cfbe7c6..dfc4f86d 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -81,9 +81,9 @@ private static Command getAutonomousDriveToNoHitReefPose(boolean shouldScoreRigh } private static Command getOpenArmElevatorIfWontHitReef(boolean shouldScoreRight) { - return new GeneralCommands().runWhen( + return GeneralCommands.runWhen( ArmElevatorCommands.getPrepareForStateCommand(REEF_CHOOSER::getArmElevatorState, CoralPlacingCommands::shouldReverseScore), - () -> CoralPlacingCommands.isPrepareArmAngleAboveCurrentArmAngle() || calculateDistanceToTargetScoringPose(shouldScoreRight) > SAFE_DISTANCE_FROM_SCORING_POSE_METERS + () -> CoralPlacingCommands.isPrepareArmAngleAboveCurrentArmAngle() || calculateDistanceToTargetScoringPose(shouldScoreRight) > FieldConstants.SAFE_DISTANCE_FROM_SCORING_POSE_METERS ); } diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index eee836e8..70f2ee0f 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -30,7 +30,9 @@ public class FieldConstants { public static final double REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS = 1.3, REEF_CENTER_TO_TARGET_SCORING_POSITION_Y_TRANSFORM_METERS = 0.17, - REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS = 1.6; + REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS = 1.6, + REEF_CENTER_TO_TARGET_NO_HIT_REEF_POSITION_X_TRANSFORM_METERS = REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS + 0.3; + public static final double SAFE_DISTANCE_FROM_SCORING_POSE_METERS = 2.2; private static final boolean SHOULD_USE_HOME_TAG_LAYOUT = false; public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = createAprilTagFieldLayout(); From 3e7189ad3b17651052e1d23844a21d3b755bc489 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 28 Sep 2025 12:45:51 +0300 Subject: [PATCH 44/51] Cool stuff --- .../trigon/robot/commands/commandfactories/GeneralCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java index 6828de9a..4b2e637f 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java @@ -56,7 +56,7 @@ public static Command runWhen(Command command, BooleanSupplier condition) { } /** - * ---- UNTESTED ---- + * ---- UNTESTED ----
* A command that only runs when a condition is met for a certain amount of time. * * @param command the command to run From 7cdf12cab854017e316e66fd863515d20dbd5218 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 28 Sep 2025 12:50:53 +0300 Subject: [PATCH 45/51] Remove LEDs --- .../commands/commandfactories/CoralCollectionCommands.java | 5 +---- 1 file changed, 1 insertion(+), 4 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 795cd081..8b2c0075 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -5,7 +5,6 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.constants.LEDConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.arm.ArmElevatorCommands; import frc.trigon.robot.subsystems.arm.ArmElevatorConstants; @@ -15,7 +14,6 @@ import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.transporter.TransporterCommands; import frc.trigon.robot.subsystems.transporter.TransporterConstants; -import lib.hardware.misc.leds.LEDCommands; public class CoralCollectionCommands { @@ -69,8 +67,7 @@ private static Command getAlignCoralCommand() { private static Command getCollectionConfirmationCommand() { return new ParallelCommandGroup( - new InstantCommand(() -> OperatorConstants.DRIVER_CONTROLLER.rumble(OperatorConstants.RUMBLE_DURATION_SECONDS, OperatorConstants.RUMBLE_POWER)), - LEDCommands.getAnimateCommand(LEDConstants.CORAL_COLLECTION_CONFIRMATION_ANIMATION_SETTINGS) //TODO: add LEDs + new InstantCommand(() -> OperatorConstants.DRIVER_CONTROLLER.rumble(OperatorConstants.RUMBLE_DURATION_SECONDS, OperatorConstants.RUMBLE_POWER)) ); } } \ No newline at end of file From 0a34c02904ac2ba664414b6b99a6f2184591a98f Mon Sep 17 00:00:00 2001 From: ShmayaR Date: Mon, 29 Sep 2025 17:25:01 +0300 Subject: [PATCH 46/51] updated coral placing commands to use flippable arm overide command --- .../commandfactories/CoralPlacingCommands.java | 11 +++++------ 1 file changed, 5 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 dfc4f86d..519d62de 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -13,7 +13,6 @@ import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.misc.ReefChooser; -import frc.trigon.robot.subsystems.arm.ArmElevatorCommands; import frc.trigon.robot.subsystems.arm.ArmElevatorConstants; import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands; import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants; @@ -40,7 +39,7 @@ private static Command getAutonomouslyScoreCommand(boolean shouldScoreRight) { return new SequentialCommandGroup( getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(() -> isReadyToScore(shouldScoreRight)), new ParallelCommandGroup( - ArmElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getArmElevatorState, CoralPlacingCommands::shouldReverseScore), + GeneralCommands.getFlippableOverridableArmCommand(REEF_CHOOSER::getArmElevatorState, false, CoralPlacingCommands::shouldReverseScore), EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_CORAL) ) ); @@ -50,7 +49,7 @@ private static Command getScoreCommand(boolean shouldScoreRight) { return new SequentialCommandGroup( getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(OperatorConstants.CONTINUE_TRIGGER), new ParallelCommandGroup( - ArmElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getArmElevatorState, CoralPlacingCommands::shouldReverseScore), + GeneralCommands.getFlippableOverridableArmCommand(REEF_CHOOSER::getArmElevatorState, false, CoralPlacingCommands::shouldReverseScore), EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_CORAL) ) ); @@ -58,7 +57,7 @@ private static Command getScoreCommand(boolean shouldScoreRight) { private static Command getAutonomouslyPrepareScoreCommand(boolean shouldScoreRight) { return new ParallelCommandGroup( - getOpenArmElevatorIfWontHitReef(shouldScoreRight), + getPrepareArmElevatorIfWontHitReef(shouldScoreRight), new SequentialCommandGroup( getAutonomousDriveToNoHitReefPose(shouldScoreRight).asProxy().onlyWhile(() -> !isPrepareArmAngleAboveCurrentArmAngle()), getAutonomousDriveToReef(shouldScoreRight).asProxy() @@ -80,9 +79,9 @@ private static Command getAutonomousDriveToNoHitReefPose(boolean shouldScoreRigh ); } - private static Command getOpenArmElevatorIfWontHitReef(boolean shouldScoreRight) { + private static Command getPrepareArmElevatorIfWontHitReef(boolean shouldScoreRight) { return GeneralCommands.runWhen( - ArmElevatorCommands.getPrepareForStateCommand(REEF_CHOOSER::getArmElevatorState, CoralPlacingCommands::shouldReverseScore), + GeneralCommands.getFlippableOverridableArmCommand(REEF_CHOOSER::getArmElevatorState, true, CoralPlacingCommands::shouldReverseScore), () -> CoralPlacingCommands.isPrepareArmAngleAboveCurrentArmAngle() || calculateDistanceToTargetScoringPose(shouldScoreRight) > FieldConstants.SAFE_DISTANCE_FROM_SCORING_POSE_METERS ); } From 8491f006e433af0128003480fb4c05f52e464f9d Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Tue, 30 Sep 2025 23:03:33 +0300 Subject: [PATCH 47/51] scores algae again Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../java/frc/trigon/robot/RobotContainer.java | 4 +-- .../AlgaeManipulationCommands.java | 27 +++++-------------- .../CoralCollectionCommands.java | 4 +-- .../CoralEjectionCommands.java | 4 +-- .../CoralPlacingCommands.java | 2 +- .../commandfactories/GeneralCommands.java | 4 +-- .../robot/constants/OperatorConstants.java | 11 ++++++-- .../frc/trigon/robot/misc/ReefChooser.java | 3 +-- .../SimulationFieldHandler.java | 2 +- .../{arm => armelevator}/ArmElevator.java | 3 ++- .../ArmElevatorCommands.java | 2 +- .../ArmElevatorConstants.java | 4 +-- .../subsystems/endeffector/EndEffector.java | 2 ++ 13 files changed, 34 insertions(+), 38 deletions(-) rename src/main/java/frc/trigon/robot/subsystems/{arm => armelevator}/ArmElevator.java (99%) rename src/main/java/frc/trigon/robot/subsystems/{arm => armelevator}/ArmElevatorCommands.java (98%) rename src/main/java/frc/trigon/robot/subsystems/{arm => armelevator}/ArmElevatorConstants.java (99%) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 1860152a..6649fdc9 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -21,8 +21,8 @@ import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; -import frc.trigon.robot.subsystems.arm.ArmElevator; -import frc.trigon.robot.subsystems.arm.ArmElevatorCommands; +import frc.trigon.robot.subsystems.armelevator.ArmElevator; +import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands; import frc.trigon.robot.subsystems.climber.Climber; import frc.trigon.robot.subsystems.climber.ClimberCommands; import frc.trigon.robot.subsystems.climber.ClimberConstants; 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 3b4605c9..b978329d 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,8 @@ import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.constants.LEDConstants; import frc.trigon.robot.constants.OperatorConstants; -import frc.trigon.robot.subsystems.arm.ArmElevatorCommands; -import frc.trigon.robot.subsystems.arm.ArmElevatorConstants; +import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands; +import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants; import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands; import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants; import frc.trigon.robot.subsystems.intake.IntakeCommands; @@ -29,7 +29,6 @@ import java.util.List; import java.util.Map; -import java.util.function.BooleanSupplier; public class AlgaeManipulationCommands { private static boolean @@ -113,33 +112,21 @@ private static Command getHoldAlgaeCommand() { } private static Command getScoreInNetCommand() { - return new ParallelRaceGroup( - getArmNetSequenceCommand(AlgaeManipulationCommands::shouldReverseNetScore), + return new ParallelCommandGroup( + GeneralCommands.getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState.SCORE_NET, false, AlgaeManipulationCommands::shouldReverseNetScore), + GeneralCommands.runWhen(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_ALGAE), OperatorConstants.CONTINUE_TRIGGER), getDriveToNetCommand() ); } private static Command getScoreInProcessorCommand() { return new ParallelCommandGroup( - getArmProcessorSequenceCommand(), + GeneralCommands.getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState.SCORE_PROCESSOR, false), + GeneralCommands.runWhen(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_ALGAE), OperatorConstants.CONTINUE_TRIGGER), getDriveToProcessorCommand() ).finallyDo(GeneralCommands.getFieldRelativeDriveCommand()::schedule); } - private static Command getArmNetSequenceCommand(BooleanSupplier shouldReverseScore) { - return new SequentialCommandGroup( - GeneralCommands.getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState.SCORE_NET, true, shouldReverseScore).until(OperatorConstants.CONTINUE_TRIGGER), - EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_ALGAE) - ); - } - - private static Command getArmProcessorSequenceCommand() { - return new SequentialCommandGroup( - GeneralCommands.getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState.SCORE_PROCESSOR, true).until(OperatorConstants.CONTINUE_TRIGGER), - EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_ALGAE) - ); - } - private static Command getDriveToNetCommand() { return new SequentialCommandGroup( SwerveCommands.getDriveToPoseCommand( 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 8b2c0075..bdac85f7 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -6,8 +6,8 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.OperatorConstants; -import frc.trigon.robot.subsystems.arm.ArmElevatorCommands; -import frc.trigon.robot.subsystems.arm.ArmElevatorConstants; +import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands; +import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants; import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands; import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants; import frc.trigon.robot.subsystems.intake.IntakeCommands; diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java index b0a9a4ed..2287965d 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java @@ -5,8 +5,8 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; -import frc.trigon.robot.subsystems.arm.ArmElevatorCommands; -import frc.trigon.robot.subsystems.arm.ArmElevatorConstants; +import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands; +import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants; import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands; import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants; import frc.trigon.robot.subsystems.intake.IntakeCommands; 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 519d62de..18d6d7c5 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -13,7 +13,7 @@ import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.misc.ReefChooser; -import frc.trigon.robot.subsystems.arm.ArmElevatorConstants; +import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants; import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands; import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java index 4b2e637f..2eb31504 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java @@ -5,8 +5,8 @@ import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.MotorSubsystem; -import frc.trigon.robot.subsystems.arm.ArmElevatorCommands; -import frc.trigon.robot.subsystems.arm.ArmElevatorConstants; +import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands; +import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; import java.util.function.BooleanSupplier; diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 29fe13ae..375e2214 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.commands.commandclasses.IntakeAssistCommand; import frc.trigon.robot.commands.commandfactories.AlgaeManipulationCommands; @@ -80,9 +81,15 @@ private static Trigger createScoreTrigger(boolean isRight, boolean isAlgaeComman final Trigger scoreTrigger; if (isRight) - scoreTrigger = DRIVER_CONTROLLER.rightStick().and(DRIVER_CONTROLLER.leftStick().negate()); + scoreTrigger = 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)); else - scoreTrigger = DRIVER_CONTROLLER.leftStick().and(DRIVER_CONTROLLER.rightStick().negate()); + scoreTrigger = DRIVER_CONTROLLER.leftStick() + .and(() -> !IS_RIGHT_SCORE_BUTTON_PRESSED) + .onTrue(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = true)) + .onFalse(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = false)); if (isAlgaeCommand) return scoreTrigger.and(AlgaeManipulationCommands::isHoldingAlgae); diff --git a/src/main/java/frc/trigon/robot/misc/ReefChooser.java b/src/main/java/frc/trigon/robot/misc/ReefChooser.java index d04bd741..87d1d644 100644 --- a/src/main/java/frc/trigon/robot/misc/ReefChooser.java +++ b/src/main/java/frc/trigon/robot/misc/ReefChooser.java @@ -3,11 +3,10 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.button.CommandGenericHID; import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.constants.OperatorConstants; -import frc.trigon.robot.subsystems.arm.ArmElevatorConstants; +import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants; import java.util.function.Supplier; 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 b7ed0f7b..c55b272a 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -4,7 +4,7 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform3d; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.subsystems.arm.ArmElevatorConstants; +import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants; import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.transporter.TransporterConstants; import lib.utilities.flippable.FlippablePose3d; diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevator.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java similarity index 99% rename from src/main/java/frc/trigon/robot/subsystems/arm/ArmElevator.java rename to src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java index 73b5fd78..23a3954c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.subsystems.arm; +package frc.trigon.robot.subsystems.armelevator; import com.ctre.phoenix6.controls.DynamicMotionMagicVoltage; import com.ctre.phoenix6.controls.VoltageOut; @@ -85,6 +85,7 @@ public void updateMechanism() { Logger.recordOutput("Poses/Components/ArmPose", calculateVisualizationPose()); Logger.recordOutput("Poses/Components/ElevatorFirstPose", getFirstStageComponentPose()); Logger.recordOutput("Poses/Components/ElevatorSecondPose", getSecondStageComponentPose()); + System.out.println("Arm elevator state " + targetState.name()); } @Override diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java similarity index 98% rename from src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorCommands.java rename to src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java index 87a0655d..1363f515 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorCommands.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.subsystems.arm; +package frc.trigon.robot.subsystems.armelevator; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java similarity index 99% rename from src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorConstants.java rename to src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index abaae7e6..4c180ef6 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -1,4 +1,4 @@ -package frc.trigon.robot.subsystems.arm; +package frc.trigon.robot.subsystems.armelevator; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -343,7 +343,7 @@ public enum ArmElevatorState { SCORE_L3(Rotation2d.fromDegrees(90), 0.7, PREPARE_SCORE_L3, false, 1), SCORE_L4(Rotation2d.fromDegrees(100), 1.2, PREPARE_SCORE_L4, false, 1), SCORE_NET(Rotation2d.fromDegrees(160), 1.382, null, false, 0.3), - SCORE_PROCESSOR(Rotation2d.fromDegrees(90), 0.603, null, false, 0.7), + SCORE_PROCESSOR(Rotation2d.fromDegrees(90), 0.1, null, false, 0.7), COLLECT_ALGAE_L2(Rotation2d.fromDegrees(90), 0.603, null, false, 1), COLLECT_ALGAE_L3(Rotation2d.fromDegrees(90), 0.953, null, false, 1), COLLECT_ALGAE_LOLLIPOP(Rotation2d.fromDegrees(70), 0.3, null, false, 1), diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java index 1b669e41..86fca1ba 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java @@ -50,12 +50,14 @@ public Translation3d calculateLinearEndEffectorVelocity() { ); } + @AutoLogOutput(key = "EndEffector/IsEjecting") public boolean isEjecting() { return endEffectorMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE) > 2; } void setTargetState(EndEffectorConstants.EndEffectorState targetState) { setEndEffectorTargetVoltage(targetState.targetVoltage); + System.out.println("End effector state " + targetState.name()); } void setTargetState(double targetVoltage) { From fbedb8f8d5b1e5a9ef5569c3d6facd1bcc059d41 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 1 Oct 2025 00:06:16 +0300 Subject: [PATCH 48/51] collection works Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../commandfactories/AlgaeManipulationCommands.java | 2 +- .../robot/misc/simulatedfield/SimulationFieldHandler.java | 6 +++++- .../trigon/robot/subsystems/armelevator/ArmElevator.java | 1 - .../robot/subsystems/armelevator/ArmElevatorConstants.java | 3 +-- .../trigon/robot/subsystems/endeffector/EndEffector.java | 1 - 5 files changed, 7 insertions(+), 6 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 b978329d..0e3b84be 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -112,7 +112,7 @@ private static Command getHoldAlgaeCommand() { } private static Command getScoreInNetCommand() { - return new ParallelCommandGroup( + return new ParallelRaceGroup( GeneralCommands.getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState.SCORE_NET, false, AlgaeManipulationCommands::shouldReverseNetScore), GeneralCommands.runWhen(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_ALGAE), OperatorConstants.CONTINUE_TRIGGER), getDriveToNetCommand() 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 c55b272a..103fad9d 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -126,9 +126,13 @@ private static boolean isCollectingCoral() { private static boolean isCollectingAlgae() { return RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_L2) + || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_L2, true) || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_L3) + || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_L3, true) || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_FLOOR) - || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_LOLLIPOP); + || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_FLOOR, true) + || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_LOLLIPOP) + || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_LOLLIPOP, true); } private static boolean isCoralLoading() { diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java index 23a3954c..567e26e9 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java @@ -85,7 +85,6 @@ public void updateMechanism() { Logger.recordOutput("Poses/Components/ArmPose", calculateVisualizationPose()); Logger.recordOutput("Poses/Components/ElevatorFirstPose", getFirstStageComponentPose()); Logger.recordOutput("Poses/Components/ElevatorSecondPose", getSecondStageComponentPose()); - System.out.println("Arm elevator state " + targetState.name()); } @Override diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index 4c180ef6..3bd07e01 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -347,8 +347,7 @@ public enum ArmElevatorState { COLLECT_ALGAE_L2(Rotation2d.fromDegrees(90), 0.603, null, false, 1), COLLECT_ALGAE_L3(Rotation2d.fromDegrees(90), 0.953, null, false, 1), COLLECT_ALGAE_LOLLIPOP(Rotation2d.fromDegrees(70), 0.3, null, false, 1), - PREPARE_COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(60), 0.2, null, false, 1), - COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(50), 0.2, PREPARE_COLLECT_ALGAE_FLOOR, true, 1); + COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(70), 0.1, null, true, 1); public final Rotation2d targetAngle; public final double targetPositionMeters; diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java index 86fca1ba..8d4ecfe5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java @@ -57,7 +57,6 @@ public boolean isEjecting() { void setTargetState(EndEffectorConstants.EndEffectorState targetState) { setEndEffectorTargetVoltage(targetState.targetVoltage); - System.out.println("End effector state " + targetState.name()); } void setTargetState(double targetVoltage) { From 0fc0c0163d1cc1f4cd7064fafc051c67074d28ff Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 1 Oct 2025 00:12:05 +0300 Subject: [PATCH 49/51] no more jerking Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../robot/commands/commandfactories/CoralPlacingCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 18d6d7c5..c271ecaf 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -32,7 +32,7 @@ public static Command getScoreInReefCommand(boolean shouldScoreRight) { getScoreCommand(shouldScoreRight), () -> SHOULD_SCORE_AUTONOMOUSLY && REEF_CHOOSER.getScoringLevel() != ScoringLevel.L1 ) - ); + ).onlyIf(() -> RobotContainer.END_EFFECTOR.hasGamePiece() || RobotContainer.INTAKE.hasCoral()); } private static Command getAutonomouslyScoreCommand(boolean shouldScoreRight) { From a65a34b6691212f1aeac359b73149eaf95a11c88 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 1 Oct 2025 00:21:10 +0300 Subject: [PATCH 50/51] no jerk Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../commands/commandfactories/CoralCollectionCommands.java | 5 +---- .../commands/commandfactories/CoralPlacingCommands.java | 2 +- 2 files changed, 2 insertions(+), 5 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 bdac85f7..f36282bb 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -45,10 +45,7 @@ public static Command getUnloadCoralCommand() { } private static Command getIntakeSequenceCommand() { - return new SequentialCommandGroup( - getInitiateCollectionCommand().until(RobotContainer.INTAKE::hasCoral), - new InstantCommand(() -> getAlignCoralCommand().schedule()).alongWith(getCollectionConfirmationCommand()) - ).until(RobotContainer.INTAKE::hasCoral); + return getInitiateCollectionCommand().until(RobotContainer.TRANSPORTER::hasCoral); } private static Command getInitiateCollectionCommand() { 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 c271ecaf..496874a4 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -32,7 +32,7 @@ public static Command getScoreInReefCommand(boolean shouldScoreRight) { getScoreCommand(shouldScoreRight), () -> SHOULD_SCORE_AUTONOMOUSLY && REEF_CHOOSER.getScoringLevel() != ScoringLevel.L1 ) - ).onlyIf(() -> RobotContainer.END_EFFECTOR.hasGamePiece() || RobotContainer.INTAKE.hasCoral()); + ).onlyIf(() -> RobotContainer.END_EFFECTOR.hasGamePiece() || RobotContainer.TRANSPORTER.hasCoral()); } private static Command getAutonomouslyScoreCommand(boolean shouldScoreRight) { From 07840d4f58e1acdf963144c06c62c0dc3c576665 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 1 Oct 2025 00:29:24 +0300 Subject: [PATCH 51/51] no erros Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../commandfactories/AlgaeManipulationCommands.java | 7 +------ 1 file changed, 1 insertion(+), 6 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 0e3b84be..8553061f 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -11,7 +11,6 @@ import frc.trigon.robot.commands.commandclasses.WaitUntilChangeCommand; import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.constants.FieldConstants; -import frc.trigon.robot.constants.LEDConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands; import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants; @@ -21,7 +20,6 @@ import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; import lib.hardware.RobotHardwareStats; -import lib.hardware.misc.leds.LEDCommands; import lib.utilities.flippable.Flippable; import lib.utilities.flippable.FlippablePose2d; import lib.utilities.flippable.FlippableRotation2d; @@ -211,10 +209,7 @@ private static Command getInitiateReefAlgaeCollectionCommand() { } private static Command getAlgaeCollectionConfirmationCommand() { - return new ParallelCommandGroup( - new InstantCommand(() -> OperatorConstants.DRIVER_CONTROLLER.rumble(OperatorConstants.RUMBLE_DURATION_SECONDS, OperatorConstants.RUMBLE_POWER)), - LEDCommands.getAnimateCommand(LEDConstants.ALGAE_COLLECTION_CONFIRMATION_ANIMATION_SETTINGS) - ); + return new InstantCommand(() -> OperatorConstants.DRIVER_CONTROLLER.rumble(OperatorConstants.RUMBLE_DURATION_SECONDS, OperatorConstants.RUMBLE_POWER)); } private static FlippablePose2d calculateClosestNetScoringPose() {