From 430e21fb2e6237f48f3e864294e2a67a987c3cc2 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Fri, 19 Sep 2025 04:48:15 +0300 Subject: [PATCH 01/27] added auton stuff temporarily. Need the integration to be done to finish --- .../java/frc/trigon/robot/RobotContainer.java | 58 +++++- .../robot/commands/CommandConstants.java | 4 +- .../commandclasses/IntakeAssistCommand.java | 20 +- .../commandfactories/AutonomousCommands.java | 194 +++++++++++++++++- ...onstants.java => AutonomousConstants.java} | 8 +- .../robot/constants/FieldConstants.java | 10 + .../frc/trigon/robot/subsystems/arm/Arm.java | 4 + .../robot/subsystems/arm/ArmCommands.java | 1 + .../robot/subsystems/elevator/Elevator.java | 4 + .../elevator/ElevatorConstants.java | 1 + .../robot/subsystems/intake/Intake.java | 9 + .../subsystems/intake/IntakeCommands.java | 8 + .../robot/subsystems/swerve/Swerve.java | 14 +- .../subsystems/swerve/SwerveConstants.java | 8 +- .../swervemodule/SwerveModuleConstants.java | 4 +- 15 files changed, 307 insertions(+), 40 deletions(-) rename src/main/java/frc/trigon/robot/constants/{PathPlannerConstants.java => AutonomousConstants.java} (90%) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 37fda07c..8061c248 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -6,35 +6,34 @@ package frc.trigon.robot; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.wpilibj2.command.Command; 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.AutonomousCommands; 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; @@ -42,9 +41,11 @@ import lib.utilities.flippable.Flippable; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; +import java.util.List; + public class RobotContainer { public static final PoseEstimator ROBOT_POSE_ESTIMATOR = new PoseEstimator(); - public static final ObjectPoseEstimator OBJECT_POSE_ESTIMATOR = new ObjectPoseEstimator( + public static final ObjectPoseEstimator CORAL_POSE_ESTIMATOR = new ObjectPoseEstimator( CameraConstants.OBJECT_POSE_ESTIMATOR_DELETION_THRESHOLD_SECONDS, ObjectPoseEstimator.DistanceCalculationMethod.ROTATION_AND_TRANSLATION, SimulatedGamePieceConstants.GamePieceType.GAME_PIECE_TYPE, @@ -94,7 +95,7 @@ private void bindDefaultCommands() { private void initializeGeneralSystems() { Flippable.init(); LEDConstants.init(); - PathPlannerConstants.init(); + AutonomousConstants.init(); } private void bindControllerCommands() { @@ -113,7 +114,44 @@ private void configureSysIDBindings(MotorSubsystem subsystem) { subsystem.setDefaultCommand(Commands.idle(subsystem)); } + @SuppressWarnings("All") private void buildAutoChooser() { - autoChooser = new LoggedDashboardChooser<>("AutoChooser", AutoBuilder.buildAutoChooser()); + autoChooser = new LoggedDashboardChooser<>("AutoChooser"); + + final List autoNames = AutoBuilder.getAllAutoNames(); + boolean hasDefault = false; + + for (String autoName : autoNames) { + final PathPlannerAuto autoNonMirrored = new PathPlannerAuto(autoName); + final PathPlannerAuto autoMirrored = new PathPlannerAuto(autoName, true); + + if (!AutonomousConstants.DEFAULT_AUTO_NAME.isEmpty() && AutonomousConstants.DEFAULT_AUTO_NAME.equals(autoName)) { + hasDefault = true; + autoChooser.addDefaultOption(autoNonMirrored.getName(), autoNonMirrored); + autoChooser.addOption(autoMirrored.getName() + "Mirrored", autoMirrored); + } else if (!AutonomousConstants.DEFAULT_AUTO_NAME.isEmpty() && AutonomousConstants.DEFAULT_AUTO_NAME.equals(autoName + "Mirrored")) { + hasDefault = true; + autoChooser.addDefaultOption(autoMirrored.getName() + "Mirrored", autoMirrored); + autoChooser.addOption(autoNonMirrored.getName(), autoNonMirrored); + } else { + autoChooser.addOption(autoNonMirrored.getName(), autoNonMirrored); + autoChooser.addOption(autoMirrored.getName() + "Mirrored", autoMirrored); + } + } + + if (!hasDefault) + autoChooser.addDefaultOption("None", Commands.none()); + else + autoChooser.addOption("None", Commands.none()); + + addCommandsToChooser( + AutonomousCommands.getFloorAutonomousCommand(true), + AutonomousCommands.getFloorAutonomousCommand(false) + ); + } + + private void addCommandsToChooser(Command... commands) { + for (Command command : commands) + autoChooser.addOption(command.getName(), command); } } \ No newline at end of file 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/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java index a57f3472..e1331e62 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -11,8 +11,8 @@ import frc.trigon.robot.commands.commandfactories.GeneralCommands; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; -import org.littletonrobotics.junction.Logger; import lib.hardware.RobotHardwareStats; +import org.littletonrobotics.junction.Logger; import java.util.function.Supplier; @@ -20,7 +20,7 @@ * A command class to assist in the intaking of a game piece. */ public class IntakeAssistCommand extends ParallelCommandGroup { - private static final ProfiledPIDController + static final ProfiledPIDController X_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)), @@ -43,7 +43,7 @@ public IntakeAssistCommand(AssistMode assistMode) { GeneralCommands.getContinuousConditionalCommand( GeneralCommands.getFieldRelativeDriveCommand(), getAssistIntakeCommand(assistMode, () -> distanceFromTrackedGamePiece), - () -> RobotContainer.OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot() == null || distanceFromTrackedGamePiece == null + () -> RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() == null || distanceFromTrackedGamePiece == null ) ); } @@ -53,7 +53,7 @@ public IntakeAssistCommand(AssistMode assistMode) { * This command is for intaking a game piece with a specific robot-relative position. * To create an intake assist command that selects the closest game piece automatically, use {@link IntakeAssistCommand#IntakeAssistCommand(AssistMode)} instead. * - * @param assistMode the type of assistance + * @param assistMode the type of assistance * @param distanceFromTrackedGamePiece the position of the game piece relative to the robot * @return the command */ @@ -70,21 +70,21 @@ public static Command getAssistIntakeCommand(AssistMode assistMode, Supplier { - if (RobotContainer.OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot() != null) + if (RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null) distanceFromTrackedGamePiece = calculateDistanceFromTrackedCGamePiece(); }); } - private static Translation2d calculateDistanceFromTrackedCGamePiece() { + public static Translation2d calculateDistanceFromTrackedCGamePiece() { final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); - final Translation2d trackedObjectPositionOnField = RobotContainer.OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot(); + final Translation2d trackedObjectPositionOnField = RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot(); if (trackedObjectPositionOnField == null) return null; final Translation2d difference = robotPose.getTranslation().minus(trackedObjectPositionOnField); - final Translation2d robotToTrackedGamepieceDistance = difference.rotateBy(robotPose.getRotation().unaryMinus()); - Logger.recordOutput("IntakeAssist/TrackedGamePieceDistance", robotToTrackedGamepieceDistance); - return robotToTrackedGamepieceDistance; + final Translation2d robotToTrackedGamePieceDistance = difference.rotateBy(robotPose.getRotation().unaryMinus()); + Logger.recordOutput("IntakeAssist/TrackedGamePieceDistance", robotToTrackedGamePieceDistance); + return robotToTrackedGamePieceDistance; } private static Translation2d calculateTranslationPower(AssistMode assistMode, Translation2d distanceFromTrackedGamepiece) { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index c6d3f53e..2438ae90 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -2,20 +2,208 @@ import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.RobotContainer; -import org.json.simple.parser.ParseException; +import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand; +import frc.trigon.robot.constants.AutonomousConstants; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.subsystems.arm.ArmCommands; +import frc.trigon.robot.subsystems.arm.ArmConstants; +import frc.trigon.robot.subsystems.elevator.ElevatorCommands; +import frc.trigon.robot.subsystems.elevator.ElevatorConstants; +import frc.trigon.robot.subsystems.intake.IntakeCommands; +import frc.trigon.robot.subsystems.intake.IntakeConstants; +import frc.trigon.robot.subsystems.swerve.SwerveCommands; import lib.utilities.flippable.FlippablePose2d; +import org.json.simple.parser.ParseException; +import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean; import java.io.IOException; import java.util.function.Supplier; +import static frc.trigon.robot.RobotContainer.CORAL_POSE_ESTIMATOR; + /** * A class that contains command factories for preparation commands and commands used during the 15-second autonomous period at the start of each match. */ public class AutonomousCommands { + public static final LoggedNetworkBoolean[] SCORED_L4S = getEmptyL4LoggedNetworkBooleanArray(); + private static FlippablePose2d TARGET_SCORING_POSE = null; + + public static Command getFloorAutonomousCommand(boolean isRight) { + return getCycleCoralCommand(isRight).repeatedly().withName("FloorAutonomous" + (isRight ? "Right" : "Left")); + } + + public static Command getCycleCoralCommand(boolean isRight) { + return new SequentialCommandGroup( + getDriveToReefAndScoreCommand(), + getCollectCoralCommand(isRight) + ); + } + + public static Command getCollectCoralCommand(boolean isRight) { + return new ParallelCommandGroup( + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST), + IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT), + getDriveToCoralCommand(isRight) + ) + .until(RobotContainer.INTAKE::hasCoral) + .unless(() -> RobotContainer.INTAKE.hasCoral() || RobotContainer.ARM.hasGamePiece()); + } + + public static Command getDriveToCoralCommand(boolean isRight) { + return new SequentialCommandGroup( + getFindCoralCommand(isRight).unless(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null).until(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null), + IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedCGamePiece).withTimeout(1.5) + ).repeatedly(); + } + + public static Command getFindCoralCommand(boolean isRight) { + return new SequentialCommandGroup( + SwerveCommands.getDriveToPoseCommand(() -> isRight ? FieldConstants.AUTO_FIND_CORAL_POSE_RIGHT : FieldConstants.AUTO_FIND_CORAL_POSE_LEFT, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS, 2.3), + SwerveCommands.getClosedLoopSelfRelativeDriveCommand( + () -> 0, + () -> 0, + () -> 0.2 + ) + ); + } + + public static Command getDriveToReefAndScoreCommand() { + return new ParallelRaceGroup( + getDriveToReefCommand(), + getCoralSequenceCommand() + ); + } + + public static Command getDriveToReefCommand() { + return new SequentialCommandGroup( + new InstantCommand(() -> TARGET_SCORING_POSE = calculateClosestScoringPose(true)), + new WaitUntilCommand(() -> TARGET_SCORING_POSE != null).raceWith(SwerveCommands.getClosedLoopSelfRelativeDriveCommand(() -> 0, () -> 0, () -> 0)), + SwerveCommands.getDriveToPoseCommand(() -> TARGET_SCORING_POSE, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly() + ); + } + + public static Command getCoralSequenceCommand() { + return new SequentialCommandGroup( +//TODO: when scoring commands implemented CoralPlacementCommands.getLoadCoralCommand(), + new WaitUntilCommand(() -> TARGET_SCORING_POSE != null), + getScoreCommand() + ); + } + + public static Command getScoreCommand() { + return new SequentialCommandGroup( + getPrepareForScoreCommand().until(AutonomousCommands::canFeed), + getFeedCoralCommand() + ).raceWith( + new SequentialCommandGroup( + new WaitUntilCommand(() -> TARGET_SCORING_POSE.get().getTranslation().getDistance(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation()) < 0.15), + IntakeCommands.getPrepareForStateCommand(IntakeConstants.IntakeState.COLLECT) + ) + ); + } + + public static Command getPrepareForScoreCommand() { + return new ParallelCommandGroup( + getOpenElevatorWhenCloseToReefCommand(), + ArmCommands.getPrepareForStateCommand(ArmConstants.ArmState.SCORE_L4) + ); + } + + private static Command getOpenElevatorWhenCloseToReefCommand() { + return GeneralCommands.runWhen( + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_L4), + () -> calculateDistanceToTargetScoringPose() < AutonomousConstants.MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR + ); + } + + private static double calculateDistanceToTargetScoringPose() { + final Translation2d currentTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); + final Translation2d targetTranslation = TARGET_SCORING_POSE.get().getTranslation(); + return currentTranslation.getDistance(targetTranslation); + } + + public static Command getFeedCoralCommand() { + return new ParallelCommandGroup( + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_L4), + ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.SCORE_L4), + getAddCurrentScoringBranchToScoredBranchesCommand() + ).withTimeout(0.25); + } + + public static FlippablePose2d calculateClosestScoringPose(boolean shouldOnlyCheckOpenBranches) { + final boolean[] scoredBranchesAtL4 = getScoredBranchesAtL4(); + final Pose2d currentRobotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + + double closestDistance = Double.POSITIVE_INFINITY; + Pose2d closestScoringPose = null; //TODO: this stuff +// for (FieldConstants.ReefClockPosition currentClockPosition : reefClockPositions) { +// for (FieldConstants.ReefSide currentSide : FieldConstants.ReefSide.values()) { +// if (shouldOnlyCheckOpenBranches && scoredBranchesAtL4[currentClockPosition.ordinal() * 2 + currentSide.ordinal()]) +// continue; +// final Pose2d reefSideScoringPose = CoralPlacingCommands.ScoringLevel.L4.calculateTargetPlacingPosition(currentClockPosition, currentSide).get(); +// final double distance = currentRobotPose.getTranslation().getDistance(reefSideScoringPose.getTranslation()); +// if (distance < closestDistance) { +// closestDistance = distance; +// if (shouldStayBehindAlgae) +// closestScoringPose = reefSideScoringPose.transformBy(new Transform2d(new Translation2d(0.1, 0), new Rotation2d())); +// else +// closestScoringPose = reefSideScoringPose; +// } +// } +// } + + return closestScoringPose == null ? null : new FlippablePose2d(closestScoringPose, false); + } + + private static Command getAddCurrentScoringBranchToScoredBranchesCommand() { + return new InstantCommand( + () -> { + final int branchNumber = getBranchNumberFromScoringPose(TARGET_SCORING_POSE.get()); + SCORED_L4S[branchNumber].set(true); + } + ); + } + + private static int getBranchNumberFromScoringPose(Pose2d scoringPose) { //TODO: this stuff +// for (FieldConstants.ReefClockPosition currentClockPosition : FieldConstants.ReefClockPosition.values()) { +// for (FieldConstants.ReefSide currentSide : FieldConstants.ReefSide.values()) { +// final Pose2d reefSideScoringPose = CoralPlacingCommands.ScoringLevel.L4.calculateTargetPlacingPosition(currentClockPosition, currentSide).get(); +// if (reefSideScoringPose.getTranslation().getDistance(scoringPose.getTranslation()) < 0.01) +// return currentClockPosition.ordinal() * 2 + currentSide.ordinal(); +// } +// } + + return 0; + } + + private static LoggedNetworkBoolean[] getEmptyL4LoggedNetworkBooleanArray() { + final LoggedNetworkBoolean[] array = new LoggedNetworkBoolean[12]; + for (int i = 0; i < array.length; i++) + array[i] = new LoggedNetworkBoolean("ScoredL4s/" + i, false); + return array; + } + + private static boolean[] getScoredBranchesAtL4() { + final boolean[] booleanArray = new boolean[SCORED_L4S.length]; + + for (int i = 0; i < booleanArray.length; i++) + booleanArray[i] = SCORED_L4S[i].get(); + + return booleanArray; + } + + private static boolean canFeed() { + return RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.SCORE_L4) && + RobotContainer.ARM.atState(ArmConstants.ArmState.SCORE_L4) && + TARGET_SCORING_POSE != null && + Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getX()) < 0.085 && + Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getY()) < 0.03; + } + /** * Creates a command that resets the pose estimator's pose to the starting pose of the given autonomous as long as the robot is not enabled. * diff --git a/src/main/java/frc/trigon/robot/constants/PathPlannerConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java similarity index 90% rename from src/main/java/frc/trigon/robot/constants/PathPlannerConstants.java rename to src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index 8b93566e..d773ac9a 100644 --- a/src/main/java/frc/trigon/robot/constants/PathPlannerConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -5,22 +5,26 @@ 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 PathConstraints DRIVE_TO_REEF_CONSTRAINTS = new PathConstraints(2.5, 4, Units.degreesToRadians(450), Units.degreesToRadians(900)); + public static final double MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR = 2.2; public static final String DEFAULT_AUTO_NAME = "DefaultAutoName"; public static final RobotConfig ROBOT_CONFIG = getRobotConfig(); public static final double FEEDFORWARD_SCALAR = 0.5;//TODO: Calibrate diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 9061418c..00673452 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -5,9 +5,11 @@ 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.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import lib.utilities.FilesHandler; +import lib.utilities.flippable.FlippablePose2d; import java.io.IOException; import java.util.HashMap; @@ -26,6 +28,14 @@ public class FieldConstants { private static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0)); public static final HashMap TAG_ID_TO_POSE = fieldLayoutToTagIDToPoseMap(); + private static final double + AUTO_FIND_CORAL_POSE_X = 3.3, + AUTO_FIND_CORAL_POSE_LEFT_Y = 5.5; + private static final Rotation2d AUTO_FIND_CORAL_POSE_LEFT_ROTATION = Rotation2d.fromDegrees(130); + public static final FlippablePose2d + AUTO_FIND_CORAL_POSE_LEFT = new FlippablePose2d(AUTO_FIND_CORAL_POSE_X, AUTO_FIND_CORAL_POSE_LEFT_Y, AUTO_FIND_CORAL_POSE_LEFT_ROTATION, true), + AUTO_FIND_CORAL_POSE_RIGHT = new FlippablePose2d(AUTO_FIND_CORAL_POSE_X, FieldConstants.FIELD_WIDTH_METERS - AUTO_FIND_CORAL_POSE_LEFT_Y, AUTO_FIND_CORAL_POSE_LEFT_ROTATION.unaryMinus(), true); + private static AprilTagFieldLayout createAprilTagFieldLayout() { try { return SHOULD_USE_HOME_TAG_LAYOUT ? 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 2e768d9f..b21cb031 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -83,6 +83,10 @@ public void sysIDDrive(double targetVoltage) { armMasterMotor.setControl(voltageRequest.withOutput(targetVoltage)); } + public boolean hasGamePiece() { + return ArmConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); + } + public boolean isArmAboveSafeAngle() { return getAngle().getDegrees() >= ArmConstants.MAXIMUM_ARM_SAFE_ANGLE.getDegrees(); } 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 7b96ab63..02e4a21c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java @@ -33,6 +33,7 @@ public static Command getGearRatioCalulationCommand() { ); } + public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState) { return getSetTargetStateCommand(targetState, false); } 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 bdbed2b9..ab29c1b6 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -89,6 +89,10 @@ public double getElevatorHeightFromMinimumSafeZone() { return getPositionMeters() - ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; } + public boolean atState(ElevatorConstants.ElevatorState targetState) { + return Math.abs(getPositionMeters() - targetState.targetPositionMeters) > ElevatorConstants.POSITION_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 05758a80..7b09aad6 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -102,6 +102,7 @@ public class ElevatorConstants { */ public static final double MAXIMUM_ELEVATOR_SAFE_ZONE_METERS = MINIMUM_ELEVATOR_SAFE_ZONE_METERS + ArmConstants.ARM_LENGTH_METERS; private static final DoubleSupplier REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER = () -> 0; + static double POSITION_TOLERANCE_METERS = 0.02; static { configureMasterMotor(); diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java index dfb6a22a..c275c86d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/Intake.java @@ -89,6 +89,15 @@ public boolean hasCoral() { return IntakeConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); } + void prepareForState(IntakeConstants.IntakeState targetState) { + this.targetState = targetState; + + setTargetState( + targetState.targetAngle, + 0 + ); + } + void setTargetState(IntakeConstants.IntakeState targetState) { this.targetState = targetState; setTargetState(targetState.targetAngle, targetState.targetVoltage); diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java index 5b1f0c53..9194dbc2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeCommands.java @@ -19,6 +19,14 @@ public static Command getDebuggingCommand() { ); } + public static Command getPrepareForStateCommand(IntakeConstants.IntakeState targetState) { + return new StartEndCommand( + () -> RobotContainer.INTAKE.prepareForState(targetState), + RobotContainer.INTAKE::stop, + RobotContainer.INTAKE + ); + } + public static Command getSetTargetStateCommand(IntakeConstants.IntakeState targetState) { return new StartEndCommand( () -> RobotContainer.INTAKE.setTargetState(targetState), 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 b49b5133ad44e0adbe0cc434cb52bc673fa12b25 Mon Sep 17 00:00:00 2001 From: Nummun14 <142012009+Nummun14@users.noreply.github.com> Date: Fri, 19 Sep 2025 05:42:33 +0300 Subject: [PATCH 02/27] no more craashing --- .../GamePieceAutoDriveCommand.java | 27 +++++++++---------- 1 file changed, 12 insertions(+), 15 deletions(-) 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..f0d8dd7d 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java @@ -6,28 +6,25 @@ 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; public class GamePieceAutoDriveCommand extends ParallelCommandGroup { - private static final ObjectPoseEstimator OBJECT_POSE_ESTIMATOR = RobotContainer.OBJECT_POSE_ESTIMATOR; - private final SimulatedGamePieceConstants.GamePieceType targetGamePieceType; + private static final ObjectPoseEstimator CORAL_POSE_ESTIMATOR = RobotContainer.CORAL_POSE_ESTIMATOR; private Translation2d distanceFromTrackedGamePiece; - public GamePieceAutoDriveCommand(SimulatedGamePieceConstants.GamePieceType targetGamePieceType) { - this.targetGamePieceType = targetGamePieceType; + public GamePieceAutoDriveCommand() { addCommands( getTrackGamePieceCommand(), GeneralCommands.getContinuousConditionalCommand( getDriveToGamePieceCommand(() -> distanceFromTrackedGamePiece), GeneralCommands.getFieldRelativeDriveCommand(), - () -> OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot() != null && shouldMoveTowardsGamePiece(distanceFromTrackedGamePiece) + () -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null && shouldMoveTowardsGamePiece(distanceFromTrackedGamePiece) ) ); } @@ -40,23 +37,23 @@ private Command getTrackGamePieceCommand() { public static Translation2d calculateDistanceFromTrackedGamePiece() { final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); - final Translation2d trackedObjectPositionOnField = OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot(); + final Translation2d trackedObjectPositionOnField = CORAL_POSE_ESTIMATOR.getClosestObjectToRobot(); if (trackedObjectPositionOnField == null) return null; 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,12 +61,12 @@ 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() { final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); - final Translation2d trackedObjectFieldRelativePosition = OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot(); + final Translation2d trackedObjectFieldRelativePosition = CORAL_POSE_ESTIMATOR.getClosestObjectToRobot(); if (trackedObjectFieldRelativePosition == null) return null; From 29d59104c40fd02ebf3b2d3f17d3a88d0636b601 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 19 Sep 2025 12:17:47 +0300 Subject: [PATCH 03/27] Orangize, add transporter, clean states --- .../commandfactories/AutonomousCommands.java | 88 +++++++++---------- .../robot/constants/AutonomousConstants.java | 3 + .../robot/subsystems/arm/ArmConstants.java | 12 +-- .../elevator/ElevatorConstants.java | 6 +- 4 files changed, 55 insertions(+), 54 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index 2438ae90..9b71479b 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -16,6 +16,8 @@ import frc.trigon.robot.subsystems.intake.IntakeCommands; import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; +import frc.trigon.robot.subsystems.transporter.TransporterCommands; +import frc.trigon.robot.subsystems.transporter.TransporterConstants; import lib.utilities.flippable.FlippablePose2d; import org.json.simple.parser.ParseException; import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean; @@ -43,23 +45,6 @@ public static Command getCycleCoralCommand(boolean isRight) { ); } - public static Command getCollectCoralCommand(boolean isRight) { - return new ParallelCommandGroup( - ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST), - IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT), - getDriveToCoralCommand(isRight) - ) - .until(RobotContainer.INTAKE::hasCoral) - .unless(() -> RobotContainer.INTAKE.hasCoral() || RobotContainer.ARM.hasGamePiece()); - } - - public static Command getDriveToCoralCommand(boolean isRight) { - return new SequentialCommandGroup( - getFindCoralCommand(isRight).unless(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null).until(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null), - IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedCGamePiece).withTimeout(1.5) - ).repeatedly(); - } - public static Command getFindCoralCommand(boolean isRight) { return new SequentialCommandGroup( SwerveCommands.getDriveToPoseCommand(() -> isRight ? FieldConstants.AUTO_FIND_CORAL_POSE_RIGHT : FieldConstants.AUTO_FIND_CORAL_POSE_LEFT, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS, 2.3), @@ -78,9 +63,20 @@ public static Command getDriveToReefAndScoreCommand() { ); } + public static Command getCollectCoralCommand(boolean isRight) { + return new ParallelCommandGroup( + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST), + IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT), + TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.COLLECT), + getDriveToCoralCommand(isRight) + ) + .until(RobotContainer.INTAKE::hasCoral) + .unless(() -> RobotContainer.INTAKE.hasCoral() || RobotContainer.ARM.hasGamePiece()); + } + public static Command getDriveToReefCommand() { return new SequentialCommandGroup( - new InstantCommand(() -> TARGET_SCORING_POSE = calculateClosestScoringPose(true)), + new InstantCommand(() -> TARGET_SCORING_POSE = calculateClosestOpenScoringPose()), new WaitUntilCommand(() -> TARGET_SCORING_POSE != null).raceWith(SwerveCommands.getClosedLoopSelfRelativeDriveCommand(() -> 0, () -> 0, () -> 0)), SwerveCommands.getDriveToPoseCommand(() -> TARGET_SCORING_POSE, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly() ); @@ -88,34 +84,52 @@ public static Command getDriveToReefCommand() { public static Command getCoralSequenceCommand() { return new SequentialCommandGroup( -//TODO: when scoring commands implemented CoralPlacementCommands.getLoadCoralCommand(), + //CoralPlacementCommands.getLoadCoralCommand(),TODO new WaitUntilCommand(() -> TARGET_SCORING_POSE != null), getScoreCommand() ); } + public static Command getDriveToCoralCommand(boolean isRight) { + return new SequentialCommandGroup( + getFindCoralCommand(isRight).unless(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null).until(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null), + IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedCGamePiece).withTimeout(1.5) + ).repeatedly(); + } + public static Command getScoreCommand() { return new SequentialCommandGroup( - getPrepareForScoreCommand().until(AutonomousCommands::canFeed), - getFeedCoralCommand() - ).raceWith( - new SequentialCommandGroup( - new WaitUntilCommand(() -> TARGET_SCORING_POSE.get().getTranslation().getDistance(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation()) < 0.15), - IntakeCommands.getPrepareForStateCommand(IntakeConstants.IntakeState.COLLECT) - ) + getPrepareForScoreCommand().until(AutonomousCommands::canScore), + getPlaceCoralCommand() ); } public static Command getPrepareForScoreCommand() { return new ParallelCommandGroup( getOpenElevatorWhenCloseToReefCommand(), - ArmCommands.getPrepareForStateCommand(ArmConstants.ArmState.SCORE_L4) + ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.PREPARE_L4) ); } + private static boolean canScore() { + return RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.PREPARE_L4) && + RobotContainer.ARM.atState(ArmConstants.ArmState.PREPARE_L4) && + TARGET_SCORING_POSE != null && + Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getX()) < AutonomousConstants.REEF_RELATIVE_X_TOLERANCE_METERS && + Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getY()) < AutonomousConstants.REEF_RELATIVE_Y_TOLERANCE_METERS; + } + + public static Command getPlaceCoralCommand() { + return new ParallelCommandGroup( + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_L4), + ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.SCORE_L4), + getAddCurrentScoringBranchToScoredBranchesCommand() + ).withTimeout(0.25); + } + private static Command getOpenElevatorWhenCloseToReefCommand() { return GeneralCommands.runWhen( - ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_L4), + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.PREPARE_L4), () -> calculateDistanceToTargetScoringPose() < AutonomousConstants.MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR ); } @@ -126,15 +140,7 @@ private static double calculateDistanceToTargetScoringPose() { return currentTranslation.getDistance(targetTranslation); } - public static Command getFeedCoralCommand() { - return new ParallelCommandGroup( - ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_L4), - ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.SCORE_L4), - getAddCurrentScoringBranchToScoredBranchesCommand() - ).withTimeout(0.25); - } - - public static FlippablePose2d calculateClosestScoringPose(boolean shouldOnlyCheckOpenBranches) { + public static FlippablePose2d calculateClosestOpenScoringPose() { final boolean[] scoredBranchesAtL4 = getScoredBranchesAtL4(); final Pose2d currentRobotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); @@ -196,14 +202,6 @@ private static boolean[] getScoredBranchesAtL4() { return booleanArray; } - private static boolean canFeed() { - return RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.SCORE_L4) && - RobotContainer.ARM.atState(ArmConstants.ArmState.SCORE_L4) && - TARGET_SCORING_POSE != null && - Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getX()) < 0.085 && - Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getY()) < 0.03; - } - /** * Creates a command that resets the pose estimator's pose to the starting pose of the given autonomous as long as the robot is not enabled. * diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index d773ac9a..e52bcc24 100644 --- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -28,6 +28,9 @@ 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 double + REEF_RELATIVE_X_TOLERANCE_METERS = 0.085, + REEF_RELATIVE_Y_TOLERANCE_METERS = 0.03; private static final PIDConstants AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? 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 ab2c8de8..d7f3b2e3 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -236,17 +236,17 @@ public enum ArmState { REST_FOR_CLIMB(Rotation2d.fromDegrees(0), 0), HOLD_ALGAE(Rotation2d.fromDegrees(90), -4), EJECT(Rotation2d.fromDegrees(60), 4), - PREPARE_SCORE_L1(Rotation2d.fromDegrees(80), 0), SCORE_L1(Rotation2d.fromDegrees(75), 4), - PREPARE_SCORE_L2(Rotation2d.fromDegrees(95), 0), SCORE_L2(Rotation2d.fromDegrees(180), 4), - PREPARE_SCORE_L3(Rotation2d.fromDegrees(95), 0), SCORE_L3(Rotation2d.fromDegrees(90), 4), - PREPARE_SCORE_L4(Rotation2d.fromDegrees(95), 0), SCORE_L4(Rotation2d.fromDegrees(90), 4), - PREPARE_NET_SCORE(Rotation2d.fromDegrees(90), HOLD_ALGAE.targetEndEffectorVoltage), + PREPARE_L1(Rotation2d.fromDegrees(80), 0), + PREPARE_L2(Rotation2d.fromDegrees(95), 0), + PREPARE_L3(Rotation2d.fromDegrees(95), 0), + PREPARE_L4(Rotation2d.fromDegrees(95), 0), + PREPARE_NET(Rotation2d.fromDegrees(90), HOLD_ALGAE.targetEndEffectorVoltage), SCORE_NET(Rotation2d.fromDegrees(160), 4), - PREPARE_PROCESSOR_SCORE(Rotation2d.fromDegrees(90), HOLD_ALGAE.targetEndEffectorVoltage), + PREPARE_PROCESSOR(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); 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 7b09aad6..982c3e3f 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -182,9 +182,9 @@ public enum ElevatorState { PREPARE_L2(0.603, 1), PREPARE_L3(1.003, 1), PREPARE_L4(1.382, 1), - COLLECT_ALGAE_FROM_L2(0.603, 1), - COLLECT_ALGAE_FROM_L3(0.953, 1), - COLLECT_ALGAE_FROM_GROUND(0, 0.7), + COLLECT_ALGAE_L2(0.603, 1), + COLLECT_ALGAE_L3(0.953, 1), + COLLECT_ALGAE_GROUND(0, 0.7), REST_WITH_ALGAE(0.603, 0.3), SCORE_NET(1.382, 0.3); From b7278fdc19670dde8341cf972bc0377e92230ee5 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Fri, 19 Sep 2025 12:19:39 +0300 Subject: [PATCH 04/27] Wow --- .../java/frc/trigon/robot/subsystems/elevator/Elevator.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 ab29c1b6..e0c01efd 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -90,7 +90,7 @@ public double getElevatorHeightFromMinimumSafeZone() { } public boolean atState(ElevatorConstants.ElevatorState targetState) { - return Math.abs(getPositionMeters() - targetState.targetPositionMeters) > ElevatorConstants.POSITION_TOLERANCE_METERS; + return Math.abs(getPositionMeters() - targetState.targetPositionMeters) < ElevatorConstants.POSITION_TOLERANCE_METERS; } void setTargetState(ElevatorConstants.ElevatorState targetState) { From 36cce6f4233323bcd009914bbf4f1bf3fd04c93a Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sat, 20 Sep 2025 22:29:06 +0300 Subject: [PATCH 05/27] Small stuff --- .../GamePieceAutoDriveCommand.java | 77 ------------------- .../commandclasses/IntakeAssistCommand.java | 4 +- .../commandfactories/AutonomousCommands.java | 7 +- .../robot/constants/AutonomousConstants.java | 20 ++++- .../robot/constants/FieldConstants.java | 10 --- .../robot/subsystems/arm/ArmCommands.java | 2 +- .../robot/subsystems/arm/ArmConstants.java | 2 +- .../elevator/ElevatorConstants.java | 2 +- 8 files changed, 25 insertions(+), 99 deletions(-) delete mode 100644 src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java deleted file mode 100644 index f0d8dd7d..00000000 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java +++ /dev/null @@ -1,77 +0,0 @@ -package frc.trigon.robot.commands.commandclasses; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.*; -import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.commands.commandfactories.GeneralCommands; -import frc.trigon.robot.constants.AutonomousConstants; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator; -import frc.trigon.robot.subsystems.swerve.SwerveCommands; -import lib.utilities.flippable.FlippableRotation2d; -import org.littletonrobotics.junction.Logger; - -import java.util.function.Supplier; - -public class GamePieceAutoDriveCommand extends ParallelCommandGroup { - private static final ObjectPoseEstimator CORAL_POSE_ESTIMATOR = RobotContainer.CORAL_POSE_ESTIMATOR; - private Translation2d distanceFromTrackedGamePiece; - - public GamePieceAutoDriveCommand() { - addCommands( - getTrackGamePieceCommand(), - GeneralCommands.getContinuousConditionalCommand( - getDriveToGamePieceCommand(() -> distanceFromTrackedGamePiece), - GeneralCommands.getFieldRelativeDriveCommand(), - () -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null && shouldMoveTowardsGamePiece(distanceFromTrackedGamePiece) - ) - ); - } - - private Command getTrackGamePieceCommand() { - return new RunCommand(() -> { - distanceFromTrackedGamePiece = calculateDistanceFromTrackedGamePiece(); - }); - } - - public static Translation2d calculateDistanceFromTrackedGamePiece() { - final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); - final Translation2d trackedObjectPositionOnField = CORAL_POSE_ESTIMATOR.getClosestObjectToRobot(); - if (trackedObjectPositionOnField == null) - return null; - - final Translation2d robotToGamePiece = robotPose.getTranslation().minus(trackedObjectPositionOnField); - var distanceFromTrackedGamePiece = robotToGamePiece.rotateBy(robotPose.getRotation().unaryMinus()); - Logger.recordOutput("GamePieceAutoDrive/DistanceFromTrackedGamePiece", distanceFromTrackedGamePiece); - 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(() -> AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.get().getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().vxMetersPerSecond)), - SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> 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 - ) - ); - } - - public static boolean shouldMoveTowardsGamePiece(Translation2d distanceFromTrackedGamePiece) { - return distanceFromTrackedGamePiece != null && - (distanceFromTrackedGamePiece.getNorm() > AutonomousConstants.AUTO_COLLECTION_INTAKE_OPEN_CHECK_DISTANCE_METERS);//TODO: If intake is open - } - - public static FlippableRotation2d calculateTargetAngle() { - final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); - final Translation2d trackedObjectFieldRelativePosition = CORAL_POSE_ESTIMATOR.getClosestObjectToRobot(); - if (trackedObjectFieldRelativePosition == null) - return null; - - final Translation2d objectDistanceFromRobot = trackedObjectFieldRelativePosition.minus(robotPose.getTranslation()); - final Rotation2d angularDistanceToObject = new Rotation2d(Math.atan2(objectDistanceFromRobot.getY(), objectDistanceFromRobot.getX())); - return new FlippableRotation2d(angularDistanceToObject, false); - } -} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java index e1331e62..820a57ee 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -71,11 +71,11 @@ public static Command getAssistIntakeCommand(AssistMode assistMode, Supplier { if (RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null) - distanceFromTrackedGamePiece = calculateDistanceFromTrackedCGamePiece(); + distanceFromTrackedGamePiece = calculateDistanceFromTrackedGamePiece(); }); } - public static Translation2d calculateDistanceFromTrackedCGamePiece() { + public static Translation2d calculateDistanceFromTrackedGamePiece() { final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); final Translation2d trackedObjectPositionOnField = RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot(); if (trackedObjectPositionOnField == null) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index 9b71479b..97116dfa 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -8,7 +8,6 @@ import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand; import frc.trigon.robot.constants.AutonomousConstants; -import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.subsystems.arm.ArmCommands; import frc.trigon.robot.subsystems.arm.ArmConstants; import frc.trigon.robot.subsystems.elevator.ElevatorCommands; @@ -47,11 +46,11 @@ public static Command getCycleCoralCommand(boolean isRight) { public static Command getFindCoralCommand(boolean isRight) { return new SequentialCommandGroup( - SwerveCommands.getDriveToPoseCommand(() -> isRight ? FieldConstants.AUTO_FIND_CORAL_POSE_RIGHT : FieldConstants.AUTO_FIND_CORAL_POSE_LEFT, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS, 2.3), + SwerveCommands.getDriveToPoseCommand(() -> isRight ? AutonomousConstants.AUTO_FIND_CORAL_POSE_RIGHT : AutonomousConstants.AUTO_FIND_CORAL_POSE_LEFT, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS, AutonomousConstants.AUTO_FIND_CORAL_END_VELOCITY_METERS_PER_SECOND), SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> 0, () -> 0, - () -> 0.2 + () -> AutonomousConstants.AUTO_FIND_CORAL_ROTATION_POWER ) ); } @@ -93,7 +92,7 @@ public static Command getCoralSequenceCommand() { public static Command getDriveToCoralCommand(boolean isRight) { return new SequentialCommandGroup( getFindCoralCommand(isRight).unless(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null).until(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null), - IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedCGamePiece).withTimeout(1.5) + IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedGamePiece).withTimeout(1.5) ).repeatedly(); } diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index e52bcc24..996d9ff2 100644 --- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -9,12 +9,14 @@ 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.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import frc.trigon.robot.RobotContainer; import lib.hardware.RobotHardwareStats; import lib.utilities.LocalADStarAK; import lib.utilities.flippable.Flippable; +import lib.utilities.flippable.FlippablePose2d; import org.json.simple.parser.ParseException; import java.io.IOException; @@ -23,15 +25,27 @@ * 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 AutonomousConstants { - public static final PathConstraints DRIVE_TO_REEF_CONSTRAINTS = new PathConstraints(2.5, 4, Units.degreesToRadians(450), Units.degreesToRadians(900)); - public static final double MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR = 2.2; - public static final String DEFAULT_AUTO_NAME = "DefaultAutoName"; public static final RobotConfig ROBOT_CONFIG = getRobotConfig(); + public static final PathConstraints DRIVE_TO_REEF_CONSTRAINTS = new PathConstraints(2.5, 4, Units.degreesToRadians(450), Units.degreesToRadians(900)); public static final double FEEDFORWARD_SCALAR = 0.5;//TODO: Calibrate + public static final double MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR = 2.2; public static final double REEF_RELATIVE_X_TOLERANCE_METERS = 0.085, REEF_RELATIVE_Y_TOLERANCE_METERS = 0.03; + public static final String DEFAULT_AUTO_NAME = "DefaultAutoName"; + + private static final double + AUTO_FIND_CORAL_POSE_X = 3.3, + AUTO_FIND_CORAL_POSE_LEFT_Y = 5.5; + private static final Rotation2d AUTO_FIND_CORAL_POSE_LEFT_ROTATION = Rotation2d.fromDegrees(130); + public static final FlippablePose2d + AUTO_FIND_CORAL_POSE_LEFT = new FlippablePose2d(AUTO_FIND_CORAL_POSE_X, AUTO_FIND_CORAL_POSE_LEFT_Y, AUTO_FIND_CORAL_POSE_LEFT_ROTATION, true), + AUTO_FIND_CORAL_POSE_RIGHT = new FlippablePose2d(AUTO_FIND_CORAL_POSE_X, FieldConstants.FIELD_WIDTH_METERS - AUTO_FIND_CORAL_POSE_LEFT_Y, AUTO_FIND_CORAL_POSE_LEFT_ROTATION.unaryMinus(), true); + public static final double + AUTO_FIND_CORAL_END_VELOCITY_METERS_PER_SECOND = 2.5, + AUTO_FIND_CORAL_ROTATION_POWER = 0.2; + private static final PIDConstants AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(0, 0, 0) : diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 00673452..9061418c 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -5,11 +5,9 @@ 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.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import lib.utilities.FilesHandler; -import lib.utilities.flippable.FlippablePose2d; import java.io.IOException; import java.util.HashMap; @@ -28,14 +26,6 @@ public class FieldConstants { private static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0)); public static final HashMap TAG_ID_TO_POSE = fieldLayoutToTagIDToPoseMap(); - private static final double - AUTO_FIND_CORAL_POSE_X = 3.3, - AUTO_FIND_CORAL_POSE_LEFT_Y = 5.5; - private static final Rotation2d AUTO_FIND_CORAL_POSE_LEFT_ROTATION = Rotation2d.fromDegrees(130); - public static final FlippablePose2d - AUTO_FIND_CORAL_POSE_LEFT = new FlippablePose2d(AUTO_FIND_CORAL_POSE_X, AUTO_FIND_CORAL_POSE_LEFT_Y, AUTO_FIND_CORAL_POSE_LEFT_ROTATION, true), - AUTO_FIND_CORAL_POSE_RIGHT = new FlippablePose2d(AUTO_FIND_CORAL_POSE_X, FieldConstants.FIELD_WIDTH_METERS - AUTO_FIND_CORAL_POSE_LEFT_Y, AUTO_FIND_CORAL_POSE_LEFT_ROTATION.unaryMinus(), true); - private static AprilTagFieldLayout createAprilTagFieldLayout() { try { return SHOULD_USE_HOME_TAG_LAYOUT ? 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 02e4a21c..7e4a467e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java @@ -24,7 +24,7 @@ public static Command getDebuggingCommand() { ); } - public static Command getGearRatioCalulationCommand() { + public static Command getGearRatioCalculationCommand() { return new GearRatioCalculationCommand( ArmConstants.ARM_MASTER_MOTOR, ArmConstants.ANGLE_ENCODER, 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 d7f3b2e3..8fef6ca4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -117,7 +117,7 @@ public class ArmConstants { * The highest point of the arms angular zone where the safety logic applies. */ static final Rotation2d MAXIMUM_ARM_SAFE_ANGLE = Rotation2d.fromDegrees(90); - static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(0); + static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(0.5); private static final double COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.2; static final BooleanEvent COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), 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 982c3e3f..b4eac635 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -102,7 +102,7 @@ public class ElevatorConstants { */ public static final double MAXIMUM_ELEVATOR_SAFE_ZONE_METERS = MINIMUM_ELEVATOR_SAFE_ZONE_METERS + ArmConstants.ARM_LENGTH_METERS; private static final DoubleSupplier REVERSE_LIMIT_SENSOR_SIMULATION_SUPPLIER = () -> 0; - static double POSITION_TOLERANCE_METERS = 0.02; + static final double POSITION_TOLERANCE_METERS = 0.02; static { configureMasterMotor(); From fc5d2aa1813064f58cce91ed4c936c052f08e63f Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 21 Sep 2025 10:22:15 +0300 Subject: [PATCH 06/27] Ummmmm --- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 4 ---- 1 file changed, 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 d987e04c..354dd371 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -84,10 +84,6 @@ public void sysIDDrive(double targetVoltage) { armMasterMotor.setControl(voltageRequest.withOutput(targetVoltage)); } - public boolean hasGamePiece() { - return ArmConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); - } - public boolean isArmAboveSafeAngle() { return getAngle().getDegrees() >= ArmConstants.MAXIMUM_ARM_SAFE_ANGLE.getDegrees(); } From d7fb963afad4b4f1a01aef22df9c8b67c82b9e3b Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 21 Sep 2025 10:35:38 +0300 Subject: [PATCH 07/27] Who did this??? --- .../frc/trigon/robot/subsystems/elevator/Elevator.java | 8 ++------ .../robot/subsystems/elevator/ElevatorConstants.java | 1 - 2 files changed, 2 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index c17cf193..2b1ad112 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -83,12 +83,12 @@ public boolean atState(ElevatorConstants.ElevatorState targetState) { public boolean atTargetState() { final double currentToTargetStateDifferenceMeters = Math.abs(targetState.targetPositionMeters - getPositionMeters()); - return currentToTargetStateDifferenceMeters < ElevatorConstants.HEIGHT_TOLERANCE_METERS; + return currentToTargetStateDifferenceMeters < ElevatorConstants.POSITION_TOLERANCE_METERS; } public boolean atPreparedTargetState() { final double currentToTargetStateDifferenceMeters = Math.abs(targetState.prepareStatePositionMeters - getPositionMeters()); - return currentToTargetStateDifferenceMeters < ElevatorConstants.HEIGHT_TOLERANCE_METERS; + return currentToTargetStateDifferenceMeters < ElevatorConstants.POSITION_TOLERANCE_METERS; } public double getPositionMeters() { @@ -103,10 +103,6 @@ public double getElevatorHeightFromMinimumSafeZone() { return getPositionMeters() - ElevatorConstants.MINIMUM_ELEVATOR_SAFE_ZONE_METERS; } - public boolean atState(ElevatorConstants.ElevatorState targetState) { - return Math.abs(getPositionMeters() - targetState.targetPositionMeters) < ElevatorConstants.POSITION_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 d8871cbc..2099eb12 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -84,7 +84,6 @@ public class ElevatorConstants { MINIMUM_ELEVATOR_HEIGHT_METERS, Color.kYellow ); - static final double HEIGHT_TOLERANCE_METERS = 0.01; static final double SECOND_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS = 0.603; static final double DRUM_DIAMETER_METERS = DRUM_RADIUS_METERS * 2; private static final double REVERSE_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS = 0.1; From aef696f7a1f2dca992f50b06635eb810b7d7a54b Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 21 Sep 2025 12:38:49 +0300 Subject: [PATCH 08/27] =?UTF-8?q?@ShmayaR,=20do=20the=20rest=20?= =?UTF-8?q?=F0=9F=99=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../commands/commandfactories/AutonomousCommands.java | 10 +++++----- .../commandfactories/CoralPlacingCommands.java | 4 ++-- src/main/java/frc/trigon/robot/subsystems/arm/Arm.java | 10 +++++----- .../frc/trigon/robot/subsystems/elevator/Elevator.java | 4 ++-- 4 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index 97116dfa..071ed90c 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -83,7 +83,7 @@ public static Command getDriveToReefCommand() { public static Command getCoralSequenceCommand() { return new SequentialCommandGroup( - //CoralPlacementCommands.getLoadCoralCommand(),TODO + CoralCollectionCommands.getLoadCoralCommand(), new WaitUntilCommand(() -> TARGET_SCORING_POSE != null), getScoreCommand() ); @@ -106,13 +106,13 @@ public static Command getScoreCommand() { public static Command getPrepareForScoreCommand() { return new ParallelCommandGroup( getOpenElevatorWhenCloseToReefCommand(), - ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.PREPARE_L4) + ArmCommands.getPrepareForStateCommand(ArmConstants.ArmState.SCORE_L4) ); } private static boolean canScore() { - return RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.PREPARE_L4) && - RobotContainer.ARM.atState(ArmConstants.ArmState.PREPARE_L4) && + return RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.SCORE_L4, true) && + RobotContainer.ARM.atState(ArmConstants.ArmState.SCORE_L4, true) && TARGET_SCORING_POSE != null && Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getX()) < AutonomousConstants.REEF_RELATIVE_X_TOLERANCE_METERS && Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getY()) < AutonomousConstants.REEF_RELATIVE_Y_TOLERANCE_METERS; @@ -128,7 +128,7 @@ public static Command getPlaceCoralCommand() { private static Command getOpenElevatorWhenCloseToReefCommand() { return GeneralCommands.runWhen( - ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.PREPARE_L4), + ElevatorCommands.getPrepareStateCommand(ElevatorConstants.ElevatorState.SCORE_L4), () -> calculateDistanceToTargetScoringPose() < AutonomousConstants.MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR ); } 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..b355fcc9 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -35,7 +35,7 @@ public static Command getScoreInReefCommand(boolean shouldScoreRight) { private static Command getAutonomouslyScoreCommand(boolean shouldScoreRight) { return new SequentialCommandGroup( - getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(() -> isArmAndElevatorAtPrepareState(shouldScoreRight)), + getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(() -> isReadyToScore(shouldScoreRight)), new ParallelCommandGroup( ElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getElevatorState), ArmCommands.getSetTargetStateCommand(REEF_CHOOSER::getArmState, CoralPlacingCommands::shouldReverseScore) @@ -106,7 +106,7 @@ public static FlippablePose2d calculateClosestScoringPose(boolean shouldScoreRig return new FlippablePose2d(closestScoringPose.transformBy(scoringPoseToBranch), false); } - private static boolean isArmAndElevatorAtPrepareState(boolean shouldScoreRight) { + private static boolean isReadyToScore(boolean shouldScoreRight) { return RobotContainer.ELEVATOR.atPreparedTargetState() && RobotContainer.ARM.atPrepareAngle() && RobotContainer.SWERVE.atPose(calculateClosestScoringPose(shouldScoreRight)); diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index 354dd371..e425a77a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -88,14 +88,14 @@ public boolean isArmAboveSafeAngle() { return getAngle().getDegrees() >= ArmConstants.MAXIMUM_ARM_SAFE_ANGLE.getDegrees(); } - public boolean atState(ArmConstants.ArmState targetState, boolean isStateReversed) { + public boolean atState(ArmConstants.ArmState targetState, boolean atPrepareState, boolean isStateReversed) { if (isStateReversed) - return this.targetState == targetState && atAngle(subtractFrom360Degrees(targetState.targetAngle)); - return atState(targetState); + return this.targetState == targetState && atAngle(subtractFrom360Degrees(atPrepareState ? targetState.prepareAngle : targetState.targetAngle)); + return atState(targetState, atPrepareState); } - public boolean atState(ArmConstants.ArmState targetState) { - return this.targetState == targetState && atAngle(targetState.targetAngle); + public boolean atState(ArmConstants.ArmState targetState, boolean atPrepareState) { + return this.targetState == targetState && atAngle(atPrepareState ? targetState.prepareAngle : targetState.targetAngle); } public boolean atTargetAngle() { 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 2b1ad112..803a6b6a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -77,8 +77,8 @@ public void updatePeriodically() { Logger.recordOutput("Elevator/CurrentPositionMeters", getPositionMeters()); } - public boolean atState(ElevatorConstants.ElevatorState targetState) { - return targetState == this.targetState && atTargetState(); + public boolean atState(ElevatorConstants.ElevatorState targetState, boolean atPrepareState) { + return targetState == this.targetState && (atPrepareState ? atPreparedTargetState() : atTargetState()); } public boolean atTargetState() { From a87293ba0a544ad3e329ebcb339940838589bfe6 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 21 Sep 2025 12:41:18 +0300 Subject: [PATCH 09/27] "Theoretically" fine --- .../commandfactories/AutonomousCommands.java | 46 +++++++++---------- 1 file changed, 22 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index 071ed90c..263fcecc 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -8,6 +8,7 @@ import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand; import frc.trigon.robot.constants.AutonomousConstants; +import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.subsystems.arm.ArmCommands; import frc.trigon.robot.subsystems.arm.ArmConstants; import frc.trigon.robot.subsystems.elevator.ElevatorCommands; @@ -144,22 +145,19 @@ public static FlippablePose2d calculateClosestOpenScoringPose() { final Pose2d currentRobotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); double closestDistance = Double.POSITIVE_INFINITY; - Pose2d closestScoringPose = null; //TODO: this stuff -// for (FieldConstants.ReefClockPosition currentClockPosition : reefClockPositions) { -// for (FieldConstants.ReefSide currentSide : FieldConstants.ReefSide.values()) { -// if (shouldOnlyCheckOpenBranches && scoredBranchesAtL4[currentClockPosition.ordinal() * 2 + currentSide.ordinal()]) -// continue; -// final Pose2d reefSideScoringPose = CoralPlacingCommands.ScoringLevel.L4.calculateTargetPlacingPosition(currentClockPosition, currentSide).get(); -// final double distance = currentRobotPose.getTranslation().getDistance(reefSideScoringPose.getTranslation()); -// if (distance < closestDistance) { -// closestDistance = distance; -// if (shouldStayBehindAlgae) -// closestScoringPose = reefSideScoringPose.transformBy(new Transform2d(new Translation2d(0.1, 0), new Rotation2d())); -// else -// closestScoringPose = reefSideScoringPose; -// } -// } -// } + Pose2d closestScoringPose = null; + for (FieldConstants.ReefClockPosition currentClockPosition : FieldConstants.ReefClockPosition.values()) { + for (FieldConstants.ReefSide currentSide : FieldConstants.ReefSide.values()) { + if (scoredBranchesAtL4[currentClockPosition.ordinal() * 2 + currentSide.ordinal()]) + continue; + final Pose2d reefSideScoringPose = CoralPlacingCommands.ScoringLevel.L4.calculateTargetPlacingPosition(currentClockPosition, currentSide).get(); + final double distance = currentRobotPose.getTranslation().getDistance(reefSideScoringPose.getTranslation()); + if (distance < closestDistance) { + closestDistance = distance; + closestScoringPose = reefSideScoringPose; + } + } + } return closestScoringPose == null ? null : new FlippablePose2d(closestScoringPose, false); } @@ -173,14 +171,14 @@ private static Command getAddCurrentScoringBranchToScoredBranchesCommand() { ); } - private static int getBranchNumberFromScoringPose(Pose2d scoringPose) { //TODO: this stuff -// for (FieldConstants.ReefClockPosition currentClockPosition : FieldConstants.ReefClockPosition.values()) { -// for (FieldConstants.ReefSide currentSide : FieldConstants.ReefSide.values()) { -// final Pose2d reefSideScoringPose = CoralPlacingCommands.ScoringLevel.L4.calculateTargetPlacingPosition(currentClockPosition, currentSide).get(); -// if (reefSideScoringPose.getTranslation().getDistance(scoringPose.getTranslation()) < 0.01) -// return currentClockPosition.ordinal() * 2 + currentSide.ordinal(); -// } -// } + private static int getBranchNumberFromScoringPose(Pose2d scoringPose) { + for (FieldConstants.ReefClockPosition currentClockPosition : FieldConstants.ReefClockPosition.values()) { + for (FieldConstants.ReefSide currentSide : FieldConstants.ReefSide.values()) { + final Pose2d reefSideScoringPose = CoralPlacingCommands.ScoringLevel.L4.calculateTargetPlacingPosition(currentClockPosition, currentSide).get(); + if (reefSideScoringPose.getTranslation().getDistance(scoringPose.getTranslation()) < 0.01) + return currentClockPosition.ordinal() * 2 + currentSide.ordinal(); + } + } return 0; } From ce5e02c07de0182d11cf6d8cb3d0d7156f180739 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 1 Oct 2025 01:10:44 +0300 Subject: [PATCH 10/27] no erros Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../commandfactories/AutonomousCommands.java | 22 +++++++++---------- .../CoralPlacingCommands.java | 19 ++++++++++++++++ 2 files changed, 29 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index 263fcecc..e1ed8728 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -9,10 +9,10 @@ import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand; import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.constants.FieldConstants; -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.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; import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; @@ -65,13 +65,12 @@ public static Command getDriveToReefAndScoreCommand() { public static Command getCollectCoralCommand(boolean isRight) { return new ParallelCommandGroup( - ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST), IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT), TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.COLLECT), getDriveToCoralCommand(isRight) ) .until(RobotContainer.INTAKE::hasCoral) - .unless(() -> RobotContainer.INTAKE.hasCoral() || RobotContainer.ARM.hasGamePiece()); + .unless(() -> RobotContainer.TRANSPORTER.hasCoral() || RobotContainer.END_EFFECTOR.hasGamePiece()); } public static Command getDriveToReefCommand() { @@ -107,13 +106,12 @@ public static Command getScoreCommand() { public static Command getPrepareForScoreCommand() { return new ParallelCommandGroup( getOpenElevatorWhenCloseToReefCommand(), - ArmCommands.getPrepareForStateCommand(ArmConstants.ArmState.SCORE_L4) + ArmElevatorCommands.getPrepareForStateCommand(() -> ArmElevatorConstants.ArmElevatorState.SCORE_L4) ); } private static boolean canScore() { - return RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.SCORE_L4, true) && - RobotContainer.ARM.atState(ArmConstants.ArmState.SCORE_L4, true) && + return RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.SCORE_L4, true) && TARGET_SCORING_POSE != null && Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getX()) < AutonomousConstants.REEF_RELATIVE_X_TOLERANCE_METERS && Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getY()) < AutonomousConstants.REEF_RELATIVE_Y_TOLERANCE_METERS; @@ -121,15 +119,15 @@ private static boolean canScore() { public static Command getPlaceCoralCommand() { return new ParallelCommandGroup( - ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_L4), - ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.SCORE_L4), + ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.SCORE_L4), + EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_CORAL), getAddCurrentScoringBranchToScoredBranchesCommand() ).withTimeout(0.25); } private static Command getOpenElevatorWhenCloseToReefCommand() { return GeneralCommands.runWhen( - ElevatorCommands.getPrepareStateCommand(ElevatorConstants.ElevatorState.SCORE_L4), + ArmElevatorCommands.getPrepareForStateCommand(() -> ArmElevatorConstants.ArmElevatorState.SCORE_L4), () -> calculateDistanceToTargetScoringPose() < AutonomousConstants.MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR ); } 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 496874a4..15b6fcb4 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -209,6 +209,25 @@ public enum ScoringLevel { this.armElevatorAlgaeCollectionState = determineArmElevatorAlgaeCollectionState(); } + /** + * Calculates the target placing position using the clock position and the target reef side. + * The reef side transform will be flipped depending on operator input. + * To make it more intuitive for the operator to input the reef side, + * left will always correspond to the physical left side in the driver station, + * as opposed to "reef relative" left. + * + * @param reefClockPosition the desired clock position of the reef + * @param reefSide the desired side of the reef, left or right (as seen from the driver station) + * @return the target placing position + */ + public FlippablePose2d calculateTargetPlacingPosition(FieldConstants.ReefClockPosition reefClockPosition, FieldConstants.ReefSide reefSide) { + final Pose2d reefCenterPose = new Pose2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, reefClockPosition.clockAngle); + final double yTransform = reefSide.shouldFlipYTransform(reefClockPosition) ? -positiveYTransformMeters : positiveYTransformMeters; + final Transform2d transform = new Transform2d(xTransformMeters, yTransform, rotationTransform); + + return new FlippablePose2d(reefCenterPose.plus(transform), true); + } + private ArmElevatorConstants.ArmElevatorState determineArmElevatorState() { return switch (level) { From 269a8aadbe778ea3944ce5063bb50e9f5403660a Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 1 Oct 2025 01:43:07 +0300 Subject: [PATCH 11/27] crashing is dumb Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../commands/commandfactories/AutonomousCommands.java | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index e1ed8728..674a6e7f 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -98,18 +98,11 @@ public static Command getDriveToCoralCommand(boolean isRight) { public static Command getScoreCommand() { return new SequentialCommandGroup( - getPrepareForScoreCommand().until(AutonomousCommands::canScore), + getOpenElevatorWhenCloseToReefCommand().until(AutonomousCommands::canScore), getPlaceCoralCommand() ); } - public static Command getPrepareForScoreCommand() { - return new ParallelCommandGroup( - getOpenElevatorWhenCloseToReefCommand(), - ArmElevatorCommands.getPrepareForStateCommand(() -> ArmElevatorConstants.ArmElevatorState.SCORE_L4) - ); - } - private static boolean canScore() { return RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.SCORE_L4, true) && TARGET_SCORING_POSE != null && From 5c317d050c7e8b683484fb7d8aab45bbf452b6ce Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 1 Oct 2025 04:55:25 +0300 Subject: [PATCH 12/27] progress is made Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../java/frc/trigon/robot/RobotContainer.java | 2 +- .../commandclasses/IntakeAssistCommand.java | 4 ++-- .../commandfactories/AutonomousCommands.java | 3 +-- .../robot/constants/CameraConstants.java | 19 ++++++++++++++++--- 4 files changed, 20 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index a05eeb17..0f2f2182 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -46,7 +46,7 @@ public class RobotContainer { public static final PoseEstimator ROBOT_POSE_ESTIMATOR = new PoseEstimator(); public static final ObjectPoseEstimator CORAL_POSE_ESTIMATOR = new ObjectPoseEstimator( CameraConstants.OBJECT_POSE_ESTIMATOR_DELETION_THRESHOLD_SECONDS, - ObjectPoseEstimator.DistanceCalculationMethod.ROTATION_AND_TRANSLATION, + ObjectPoseEstimator.DistanceCalculationMethod.TRANSLATION, SimulatedGamePieceConstants.GamePieceType.CORAL, CameraConstants.OBJECT_DETECTION_CAMERA ); diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java index 820a57ee..7771eac9 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -99,8 +99,8 @@ private static Translation2d calculateTranslationPower(AssistMode assistMode, Tr return calculateNormalAssistTranslationPower(assistMode, selfRelativeJoystickPower, xPIDOutput, yPIDOutput); } - private static double calculateThetaPower(AssistMode assistMode, Translation2d distanceFromTrackedGamepiece) { - return calculateThetaAssistPower(assistMode, distanceFromTrackedGamepiece.getAngle().plus(Rotation2d.k180deg).unaryMinus()); + private static double calculateThetaPower(AssistMode assistMode, Translation2d distanceFromTrackedGamePiece) { + return calculateThetaAssistPower(assistMode, distanceFromTrackedGamePiece.getAngle().plus(Rotation2d.k180deg).unaryMinus()); } private static Translation2d calculateAlternateAssistTranslationPower(Translation2d joystickValue, double xPIDOutput, double yPIDOutput) { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index 674a6e7f..4d6fbd97 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -90,7 +90,6 @@ public static Command getCoralSequenceCommand() { } public static Command getDriveToCoralCommand(boolean isRight) { - return new SequentialCommandGroup( getFindCoralCommand(isRight).unless(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null).until(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null), IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedGamePiece).withTimeout(1.5) ).repeatedly(); @@ -104,7 +103,7 @@ public static Command getScoreCommand() { } private static boolean canScore() { - return RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.SCORE_L4, true) && + return (RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.SCORE_L4.prepareState, true) || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.SCORE_L4.prepareState)) && TARGET_SCORING_POSE != null && Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getX()) < AutonomousConstants.REEF_RELATIVE_X_TOLERANCE_METERS && Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getY()) < AutonomousConstants.REEF_RELATIVE_Y_TOLERANCE_METERS; diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 1b750328..7c7fdfa8 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -5,16 +5,29 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; +import frc.trigon.robot.poseestimation.poseestimator.StandardDeviations; public class CameraConstants { private static final Transform3d ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA = new Transform3d( - new Translation3d(0, 0, 0), - new Rotation3d(0, Units.degreesToRadians(0), 0) + new Translation3d(0.2015, 195, 715.6), + new Rotation3d(0, Units.degreesToRadians(60), 0) ); - public static final double OBJECT_POSE_ESTIMATOR_DELETION_THRESHOLD_SECONDS = 0.5; + public static final double OBJECT_POSE_ESTIMATOR_DELETION_THRESHOLD_SECONDS = 0; public static final ObjectDetectionCamera OBJECT_DETECTION_CAMERA = new ObjectDetectionCamera( "ObjectDetectionCamera", ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA ); + public static final AprilTagCamera + INTAKE_SIDE_CAMERA = new AprilTagCamera( + AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, + "IntakeSideCamera", + new Transform3d( + new Translation3d(0.2247, 195, 749.8), + new Rotation3d(0, Units.degreesToRadians(51), 0) + ), + new StandardDeviations(0, 0) //TOD0: Measure + ); } \ No newline at end of file From 194694d46ee51bb71470c1b500e61a29e5ce3f29 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 1 Oct 2025 04:57:01 +0300 Subject: [PATCH 13/27] Update AutonomousCommands.java Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../robot/commands/commandfactories/AutonomousCommands.java | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index 4d6fbd97..0a1fd48f 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -90,8 +90,10 @@ public static Command getCoralSequenceCommand() { } public static Command getDriveToCoralCommand(boolean isRight) { - getFindCoralCommand(isRight).unless(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null).until(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null), - IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedGamePiece).withTimeout(1.5) + return new ConditionalCommand( + getFindCoralCommand(isRight).onlyWhile(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() == null), + IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedGamePiece).withTimeout(1.5), + () -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() == null ).repeatedly(); } From 732c68b15cc1c86b70828f838ea9d48b00d15e80 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 1 Oct 2025 05:01:52 +0300 Subject: [PATCH 14/27] why it never see coral Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- src/main/java/frc/trigon/robot/constants/CameraConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 7c7fdfa8..3ecdd23e 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -11,7 +11,7 @@ public class CameraConstants { private static final Transform3d ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA = new Transform3d( - new Translation3d(0.2015, 195, 715.6), + new Translation3d(0.2015, 0.195, 0.7156), new Rotation3d(0, Units.degreesToRadians(60), 0) ); @@ -25,7 +25,7 @@ public class CameraConstants { AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, "IntakeSideCamera", new Transform3d( - new Translation3d(0.2247, 195, 749.8), + new Translation3d(0.2247, 0.195, 0.7498), new Rotation3d(0, Units.degreesToRadians(51), 0) ), new StandardDeviations(0, 0) //TOD0: Measure From be98f79ad5aa40527fde16a8c6c5243f0dabdc10 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 1 Oct 2025 05:36:32 +0300 Subject: [PATCH 15/27] Update CameraConstants.java Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- src/main/java/frc/trigon/robot/constants/CameraConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 3ecdd23e..8f53cae2 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -12,7 +12,7 @@ public class CameraConstants { private static final Transform3d ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA = new Transform3d( new Translation3d(0.2015, 0.195, 0.7156), - new Rotation3d(0, Units.degreesToRadians(60), 0) + new Rotation3d(0, Units.degreesToRadians(30), 0) ); public static final double OBJECT_POSE_ESTIMATOR_DELETION_THRESHOLD_SECONDS = 0; From 29500df3be9caa43635ae119809b19ecb75c337d Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 1 Oct 2025 06:03:05 +0300 Subject: [PATCH 16/27] working on it Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../commands/commandfactories/AutonomousCommands.java | 8 ++------ .../commandfactories/CoralCollectionCommands.java | 2 +- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index 0a1fd48f..e67bfe74 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -13,11 +13,7 @@ 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; -import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; -import frc.trigon.robot.subsystems.transporter.TransporterCommands; -import frc.trigon.robot.subsystems.transporter.TransporterConstants; import lib.utilities.flippable.FlippablePose2d; import org.json.simple.parser.ParseException; import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean; @@ -65,8 +61,8 @@ public static Command getDriveToReefAndScoreCommand() { public static Command getCollectCoralCommand(boolean isRight) { return new ParallelCommandGroup( - IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT), - TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.COLLECT), + CoralCollectionCommands.getIntakeSequenceCommand(), + ArmElevatorCommands.getPrepareForStateCommand(() -> ArmElevatorConstants.ArmElevatorState.LOAD_CORAL), getDriveToCoralCommand(isRight) ) .until(RobotContainer.INTAKE::hasCoral) 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 1981cb19..541f1010 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -44,7 +44,7 @@ public static Command getUnloadCoralCommand() { ).until(() -> !RobotContainer.END_EFFECTOR.hasGamePiece() && RobotContainer.INTAKE.hasCoral()); } - private static Command getIntakeSequenceCommand() { + static Command getIntakeSequenceCommand() { return getInitiateCollectionCommand().until(RobotContainer.TRANSPORTER::hasCoral); } From afeeddb5be83159d8fad16aaa822c8f03dc956f4 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 1 Oct 2025 06:14:50 +0300 Subject: [PATCH 17/27] Update AutonomousCommands.java Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../robot/commands/commandfactories/AutonomousCommands.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index e67bfe74..421c827e 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -88,7 +88,7 @@ public static Command getCoralSequenceCommand() { public static Command getDriveToCoralCommand(boolean isRight) { return new ConditionalCommand( getFindCoralCommand(isRight).onlyWhile(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() == null), - IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedGamePiece).withTimeout(1.5), + IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedGamePiece).until(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() == null).withTimeout(1.5), () -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() == null ).repeatedly(); } From c2afab169f515c5217e089a64dd691b09b2d7bf1 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 1 Oct 2025 06:53:13 +0300 Subject: [PATCH 18/27] WHY DOES IT EQUAL NULL Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../commandclasses/IntakeAssistCommand.java | 43 +++++++++---------- .../commandfactories/AutonomousCommands.java | 7 +-- .../robot/constants/OperatorConstants.java | 2 +- 3 files changed, 26 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java index 7771eac9..d11027b5 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -42,7 +42,7 @@ public IntakeAssistCommand(AssistMode assistMode) { getTrackGamePieceCommand(), GeneralCommands.getContinuousConditionalCommand( GeneralCommands.getFieldRelativeDriveCommand(), - getAssistIntakeCommand(assistMode, () -> distanceFromTrackedGamePiece), + getAssistIntakeCommand(assistMode, () -> distanceFromTrackedGamePiece, OperatorConstants.INTAKE_ASSIST_SCALAR), () -> RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() == null || distanceFromTrackedGamePiece == null ) ); @@ -57,13 +57,13 @@ public IntakeAssistCommand(AssistMode assistMode) { * @param distanceFromTrackedGamePiece the position of the game piece relative to the robot * @return the command */ - public static Command getAssistIntakeCommand(AssistMode assistMode, Supplier distanceFromTrackedGamePiece) { + public static Command getAssistIntakeCommand(AssistMode assistMode, Supplier distanceFromTrackedGamePiece, double intakeAssistScalar) { return new SequentialCommandGroup( new InstantCommand(() -> resetPIDControllers(distanceFromTrackedGamePiece.get())), SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> calculateTranslationPower(assistMode, distanceFromTrackedGamePiece.get()).getX(), - () -> calculateTranslationPower(assistMode, distanceFromTrackedGamePiece.get()).getY(), - () -> calculateThetaPower(assistMode, distanceFromTrackedGamePiece.get()) + () -> calculateTranslationPower(assistMode, distanceFromTrackedGamePiece.get(), intakeAssistScalar).getX(), + () -> calculateTranslationPower(assistMode, distanceFromTrackedGamePiece.get(), intakeAssistScalar).getY(), + () -> calculateThetaPower(assistMode, distanceFromTrackedGamePiece.get(), intakeAssistScalar) ) ); } @@ -87,20 +87,20 @@ public static Translation2d calculateDistanceFromTrackedGamePiece() { return robotToTrackedGamePieceDistance; } - private static Translation2d calculateTranslationPower(AssistMode assistMode, Translation2d distanceFromTrackedGamepiece) { + private static Translation2d calculateTranslationPower(AssistMode assistMode, Translation2d distanceFromTrackedGamePiece, double intakeAssistScalar) { final Translation2d joystickPower = new Translation2d(OperatorConstants.DRIVER_CONTROLLER.getLeftY(), OperatorConstants.DRIVER_CONTROLLER.getLeftX()); final Translation2d selfRelativeJoystickPower = joystickPower.rotateBy(RobotContainer.SWERVE.getDriveRelativeAngle().unaryMinus()); - final double xPIDOutput = clampToOutputRange(X_PID_CONTROLLER.calculate(distanceFromTrackedGamepiece.getX())); - final double yPIDOutput = clampToOutputRange(Y_PID_CONTROLLER.calculate(distanceFromTrackedGamepiece.getY())); + final double xPIDOutput = clampToOutputRange(X_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.getX())); + final double yPIDOutput = clampToOutputRange(Y_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.getY())); if (assistMode.equals(AssistMode.ALTERNATE_ASSIST)) return calculateAlternateAssistTranslationPower(selfRelativeJoystickPower, xPIDOutput, yPIDOutput); - return calculateNormalAssistTranslationPower(assistMode, selfRelativeJoystickPower, xPIDOutput, yPIDOutput); + return calculateNormalAssistTranslationPower(assistMode, selfRelativeJoystickPower, xPIDOutput, yPIDOutput, intakeAssistScalar); } - private static double calculateThetaPower(AssistMode assistMode, Translation2d distanceFromTrackedGamePiece) { - return calculateThetaAssistPower(assistMode, distanceFromTrackedGamePiece.getAngle().plus(Rotation2d.k180deg).unaryMinus()); + private static double calculateThetaPower(AssistMode assistMode, Translation2d distanceFromTrackedGamePiece, double intakeAssistScalar) { + return calculateThetaAssistPower(assistMode, distanceFromTrackedGamePiece.getAngle().plus(Rotation2d.k180deg).unaryMinus(), intakeAssistScalar); } private static Translation2d calculateAlternateAssistTranslationPower(Translation2d joystickValue, double xPIDOutput, double yPIDOutput) { @@ -115,25 +115,25 @@ private static Translation2d calculateAlternateAssistTranslationPower(Translatio return new Translation2d(xPower, yPower); } - private static Translation2d calculateNormalAssistTranslationPower(AssistMode assistMode, Translation2d joystickValue, double xPIDOutput, double yPIDOutput) { + private static Translation2d calculateNormalAssistTranslationPower(AssistMode assistMode, Translation2d joystickValue, double xPIDOutput, double yPIDOutput, double intakeAssistScalar) { final double xJoystickPower = joystickValue.getX(), yJoystickPower = joystickValue.getY(); final double - xPower = assistMode.shouldAssistX ? calculateNormalAssistPower(xPIDOutput, xJoystickPower) : xJoystickPower, - yPower = assistMode.shouldAssistY ? calculateNormalAssistPower(yPIDOutput, yJoystickPower) : yJoystickPower; + xPower = assistMode.shouldAssistX ? calculateNormalAssistPower(xPIDOutput, xJoystickPower, intakeAssistScalar) : xJoystickPower, + yPower = assistMode.shouldAssistY ? calculateNormalAssistPower(yPIDOutput, yJoystickPower, intakeAssistScalar) : yJoystickPower; return new Translation2d(xPower, yPower); } - private static double calculateThetaAssistPower(AssistMode assistMode, Rotation2d thetaOffset) { + private static double calculateThetaAssistPower(AssistMode assistMode, Rotation2d thetaOffset, double intakeAssistScalar) { final double pidOutput = clampToOutputRange(THETA_PID_CONTROLLER.calculate(thetaOffset.getRadians())), joystickValue = OperatorConstants.DRIVER_CONTROLLER.getRightX(); if (assistMode.equals(AssistMode.ALTERNATE_ASSIST)) return calculateAlternateAssistPower(pidOutput, joystickValue, joystickValue); - return calculateNormalAssistPower(pidOutput, joystickValue); + return calculateNormalAssistPower(pidOutput, joystickValue, intakeAssistScalar); } private static double clampToOutputRange(double value) { @@ -144,9 +144,8 @@ private static double calculateAlternateAssistPower(double pidOutput, double pid return pidOutput * (1 - Math.abs(pidScalar)) + joystickPower; } - private static double calculateNormalAssistPower(double pidOutput, double joystickPower) { - final double intakeAssistScalar = OperatorConstants.INTAKE_ASSIST_SCALAR; - return (pidOutput * intakeAssistScalar) + (joystickPower * (1 - intakeAssistScalar)); + private static double calculateNormalAssistPower(double pidOutput, double joystickPower, double scalar) { + return (pidOutput * scalar) + (joystickPower * (1 - scalar)); } private static void resetPIDControllers(Translation2d distanceFromTrackedGamePiece) { @@ -164,15 +163,15 @@ public enum AssistMode { */ ALTERNATE_ASSIST(true, true, true), /** - * Applies pid values to autonomously drive to the game piece, scaled by {@link OperatorConstants#INTAKE_ASSIST_SCALAR} in addition to the driver's inputs + * Applies pid values to autonomously drive to the game piece, scaled by the intake assist scalar in addition to the driver's inputs */ FULL_ASSIST(true, true, true), /** - * Applies pid values to align to the game piece, scaled by {@link OperatorConstants#INTAKE_ASSIST_SCALAR} in addition to the driver's inputs + * Applies pid values to align to the game piece, scaled by the intake assist scalar in addition to the driver's inputs */ ALIGN_ASSIST(false, true, true), /** - * Applies pid values to face the game piece, scaled by {@link OperatorConstants#INTAKE_ASSIST_SCALAR} in addition to the driver's inputs + * Applies pid values to face the game piece, scaled by the intake assist scalar in addition to the driver's inputs */ ASSIST_ROTATION(false, false, true); diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index 421c827e..f1426746 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -9,6 +9,7 @@ import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand; import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands; import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants; import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands; @@ -87,9 +88,9 @@ public static Command getCoralSequenceCommand() { public static Command getDriveToCoralCommand(boolean isRight) { return new ConditionalCommand( - getFindCoralCommand(isRight).onlyWhile(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() == null), - IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedGamePiece).until(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() == null).withTimeout(1.5), - () -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() == null + IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedGamePiece, OperatorConstants.INTAKE_ASSIST_SCALAR).onlyWhile(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null).withTimeout(1.5), + getFindCoralCommand(isRight).until(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null), + () -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null ).repeatedly(); } diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index b9d2c68b..a6ef4925 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -35,7 +35,7 @@ public class OperatorConstants { TRANSLATION_STICK_SPEED_DIVIDER = 1, ROTATION_STICK_SPEED_DIVIDER = 1; - public static final double INTAKE_ASSIST_SCALAR = 0.0; + public static final double INTAKE_ASSIST_SCALAR = 1; public static final IntakeAssistCommand.AssistMode DEFAULT_INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.ALTERNATE_ASSIST; public static final Trigger From e4ca74533853fc72eddda9502a2447ee09ee024a Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Wed, 1 Oct 2025 07:20:19 +0300 Subject: [PATCH 19/27] temp --- .../commands/commandclasses/IntakeAssistCommand.java | 8 ++++++-- .../commands/commandfactories/AutonomousCommands.java | 2 +- .../java/frc/trigon/robot/constants/CameraConstants.java | 2 +- 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java index d11027b5..adbdf552 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -28,7 +28,7 @@ public class IntakeAssistCommand extends ParallelCommandGroup { new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : new ProfiledPIDController(0.3, 0, 0.03, new TrapezoidProfile.Constraints(2.65, 5.5)), THETA_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? - new ProfiledPIDController(0.4, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : + new ProfiledPIDController(0.2, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)); private Translation2d distanceFromTrackedGamePiece; @@ -88,6 +88,8 @@ public static Translation2d calculateDistanceFromTrackedGamePiece() { } private static Translation2d calculateTranslationPower(AssistMode assistMode, Translation2d distanceFromTrackedGamePiece, double intakeAssistScalar) { + if (distanceFromTrackedGamePiece == null) + return new Translation2d(0, 0); final Translation2d joystickPower = new Translation2d(OperatorConstants.DRIVER_CONTROLLER.getLeftY(), OperatorConstants.DRIVER_CONTROLLER.getLeftX()); final Translation2d selfRelativeJoystickPower = joystickPower.rotateBy(RobotContainer.SWERVE.getDriveRelativeAngle().unaryMinus()); @@ -100,7 +102,9 @@ private static Translation2d calculateTranslationPower(AssistMode assistMode, Tr } private static double calculateThetaPower(AssistMode assistMode, Translation2d distanceFromTrackedGamePiece, double intakeAssistScalar) { - return calculateThetaAssistPower(assistMode, distanceFromTrackedGamePiece.getAngle().plus(Rotation2d.k180deg).unaryMinus(), intakeAssistScalar); + if (distanceFromTrackedGamePiece == null || !assistMode.shouldAssistTheta) + return 0; + return calculateThetaAssistPower(assistMode, distanceFromTrackedGamePiece.getAngle().plus(Rotation2d.k180deg), intakeAssistScalar); } private static Translation2d calculateAlternateAssistTranslationPower(Translation2d joystickValue, double xPIDOutput, double yPIDOutput) { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index f1426746..4ae9d033 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -88,7 +88,7 @@ public static Command getCoralSequenceCommand() { public static Command getDriveToCoralCommand(boolean isRight) { return new ConditionalCommand( - IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedGamePiece, OperatorConstants.INTAKE_ASSIST_SCALAR).onlyWhile(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null).withTimeout(1.5), + IntakeAssistCommand.getAssistIntakeCommand(IntakeAssistCommand.AssistMode.FULL_ASSIST, IntakeAssistCommand::calculateDistanceFromTrackedGamePiece, OperatorConstants.INTAKE_ASSIST_SCALAR).onlyWhile(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null).withTimeout(10), getFindCoralCommand(isRight).until(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null), () -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null ).repeatedly(); diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 8f53cae2..3c6ec869 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -15,7 +15,7 @@ public class CameraConstants { new Rotation3d(0, Units.degreesToRadians(30), 0) ); - public static final double OBJECT_POSE_ESTIMATOR_DELETION_THRESHOLD_SECONDS = 0; + public static final double OBJECT_POSE_ESTIMATOR_DELETION_THRESHOLD_SECONDS = 1; public static final ObjectDetectionCamera OBJECT_DETECTION_CAMERA = new ObjectDetectionCamera( "ObjectDetectionCamera", ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA From 2f7bf280ff64e3cc6a6b42f00528caa0de3db4b3 Mon Sep 17 00:00:00 2001 From: Shm Date: Wed, 1 Oct 2025 01:37:40 -0400 Subject: [PATCH 20/27] 12 coral auto :) Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../commandclasses/IntakeAssistCommand.java | 14 +++++++++++--- .../AlgaeManipulationCommands.java | 4 ++-- .../commandfactories/CoralCollectionCommands.java | 8 ++++---- .../trigon/robot/constants/OperatorConstants.java | 2 +- .../armelevator/ArmElevatorConstants.java | 2 +- 5 files changed, 19 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java index adbdf552..ba2d2cec 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -28,7 +28,7 @@ public class IntakeAssistCommand extends ParallelCommandGroup { new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : new ProfiledPIDController(0.3, 0, 0.03, new TrapezoidProfile.Constraints(2.65, 5.5)), THETA_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? - new ProfiledPIDController(0.2, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : + new ProfiledPIDController(0.4, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)); private Translation2d distanceFromTrackedGamePiece; @@ -104,7 +104,15 @@ private static Translation2d calculateTranslationPower(AssistMode assistMode, Tr private static double calculateThetaPower(AssistMode assistMode, Translation2d distanceFromTrackedGamePiece, double intakeAssistScalar) { if (distanceFromTrackedGamePiece == null || !assistMode.shouldAssistTheta) return 0; - return calculateThetaAssistPower(assistMode, distanceFromTrackedGamePiece.getAngle().plus(Rotation2d.k180deg), intakeAssistScalar); + Rotation2d distanceAngle = distanceFromTrackedGamePiece.getAngle().plus(Rotation2d.k180deg).unaryMinus(); + Rotation2d robotAngle = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation(); + Rotation2d diff = robotAngle.minus(distanceAngle); + Logger.recordOutput("IntakeAssist/difference", diff); + Logger.recordOutput("IntakeAssist/robotAngle", robotAngle); + Logger.recordOutput("IntakeAssist/distanceAngle", distanceAngle); + var pow = calculateThetaAssistPower(assistMode, distanceAngle, intakeAssistScalar); + Logger.recordOutput("IntakeAssist/pow", pow); + return pow; } private static Translation2d calculateAlternateAssistTranslationPower(Translation2d joystickValue, double xPIDOutput, double yPIDOutput) { @@ -155,7 +163,7 @@ private static double calculateNormalAssistPower(double pidOutput, double joysti private static void resetPIDControllers(Translation2d distanceFromTrackedGamePiece) { X_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().vxMetersPerSecond); Y_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getY(), RobotContainer.SWERVE.getSelfRelativeVelocity().vyMetersPerSecond); - THETA_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getAngle().getRadians(), RobotContainer.SWERVE.getSelfRelativeVelocity().omegaRadiansPerSecond); + THETA_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.getAngle().plus(Rotation2d.k180deg).unaryMinus().getRadians(), RobotContainer.SWERVE.getSelfRelativeVelocity().omegaRadiansPerSecond); } /** 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 8553061f..ec50c158 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -63,8 +63,8 @@ public static Command getFloorAlgaeCollectionCommand() { public static Command getReefAlgaeCollectionCommand() { return new SequentialCommandGroup( GeneralCommands.getResetFlipArmOverrideCommand(), - new InstantCommand(() -> IS_HOLDING_ALGAE = true), CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.END_EFFECTOR::hasGamePiece), + new InstantCommand(() -> IS_HOLDING_ALGAE = true), getInitiateReefAlgaeCollectionCommand().until(RobotContainer.END_EFFECTOR::hasGamePiece), GeneralCommands.getResetFlipArmOverrideCommand(), getScoreAlgaeCommand().alongWith(getAlgaeCollectionConfirmationCommand()) @@ -86,7 +86,7 @@ private static Command getAlignToReefCommand() { () -> calculateClosestAlgaeCollectionPose().getRotation() ) ).raceWith( - new WaitUntilCommand(RobotContainer.END_EFFECTOR::hasGamePiece), + new WaitUntilCommand(() -> RobotContainer.END_EFFECTOR.hasGamePiece() && IS_HOLDING_ALGAE), new WaitUntilCommand(OperatorConstants.STOP_REEF_ALGAE_ALIGN_TRIGGER) ).onlyIf(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY).asProxy(); } 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 541f1010..809cb63f 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -5,6 +5,7 @@ import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands; import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants; @@ -26,8 +27,7 @@ public static Command getCoralCollectionCommand() { getLoadCoralCommand().schedule(); } ) - ); - // new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE) + ).alongWith(new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE)); } public static Command getLoadCoralCommand() { @@ -40,8 +40,8 @@ public static Command getLoadCoralCommand() { 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()); + GeneralCommands.runWhen(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.UNLOAD_CORAL), () -> RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.UNLOAD_CORAL)) + ).until(() -> !RobotContainer.END_EFFECTOR.hasGamePiece()); } static Command getIntakeSequenceCommand() { diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index a6ef4925..9358f60b 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -36,7 +36,7 @@ public class OperatorConstants { ROTATION_STICK_SPEED_DIVIDER = 1; public static final double INTAKE_ASSIST_SCALAR = 1; - public static final IntakeAssistCommand.AssistMode DEFAULT_INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.ALTERNATE_ASSIST; + public static final IntakeAssistCommand.AssistMode DEFAULT_INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.FULL_ASSIST; public static final Trigger RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.povUp(), 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 b91cd417..c385ba5b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -336,7 +336,7 @@ public enum ArmElevatorState { REST_WITH_ALGAE(Rotation2d.fromDegrees(90), 0.603, null, false, 0.7), REST_FOR_CLIMB(Rotation2d.fromDegrees(180), 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), + UNLOAD_CORAL(Rotation2d.fromDegrees(0), 0.603, null, false, 0.7), EJECT(Rotation2d.fromDegrees(60), 0.603, null, false, 0.7), SCORE_L1(Rotation2d.fromDegrees(70), 0.4, null, false, 1), SCORE_L2(Rotation2d.fromDegrees(90), 0.3, PREPARE_SCORE_L2, false, 1), From b53034b20cd3cafaf36065633095b510e580b9c7 Mon Sep 17 00:00:00 2001 From: Shm Date: Thu, 2 Oct 2025 18:21:53 -0400 Subject: [PATCH 21/27] no collect algae during auto Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../CoralCollectionCommands.java | 1 - .../robot/constants/OperatorConstants.java | 2 +- .../ObjectDetectionCamera.java | 20 +++++++++++++++++++ 3 files changed, 21 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java index 809cb63f..8a88d3b1 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -17,7 +17,6 @@ import frc.trigon.robot.subsystems.transporter.TransporterConstants; public class CoralCollectionCommands { - public static Command getCoralCollectionCommand() { return new SequentialCommandGroup( getIntakeSequenceCommand(), diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 9358f60b..a6ef4925 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -36,7 +36,7 @@ public class OperatorConstants { ROTATION_STICK_SPEED_DIVIDER = 1; public static final double INTAKE_ASSIST_SCALAR = 1; - public static final IntakeAssistCommand.AssistMode DEFAULT_INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.FULL_ASSIST; + public static final IntakeAssistCommand.AssistMode DEFAULT_INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.ALTERNATE_ASSIST; public static final Trigger RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.povUp(), diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java index 51c211af..13498248 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java @@ -3,12 +3,15 @@ import edu.wpi.first.math.geometry.*; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.commandfactories.CoralCollectionCommands; import frc.trigon.robot.misc.objectdetectioncamera.io.PhotonObjectDetectionCameraIO; import frc.trigon.robot.misc.objectdetectioncamera.io.SimulationObjectDetectionCameraIO; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import org.littletonrobotics.junction.Logger; import lib.hardware.RobotHardwareStats; +import java.util.ArrayList; + /** * An object detection camera is a class that represents a camera that detects objects other than apriltags, most likely game pieces. */ @@ -71,6 +74,14 @@ public Translation2d[] getObjectPositionsOnField(SimulatedGamePieceConstants.Gam } public Rotation3d[] getTargetObjectsRotations(SimulatedGamePieceConstants.GamePieceType targetGamePiece) { + if (targetGamePiece == SimulatedGamePieceConstants.GamePieceType.CORAL) { + ArrayList rotations = new ArrayList<>(); + for (Rotation3d rotation : objectDetectionCameraInputs.visibleObjectRotations[targetGamePiece.id]) { + if (!isLollipop(rotation.toRotation2d())) + rotations.add(rotation); + } + return rotations.toArray(new Rotation3d[0]); + } return objectDetectionCameraInputs.visibleObjectRotations[targetGamePiece.id]; } @@ -98,6 +109,15 @@ private Translation2d calculateObjectPositionFromRotation(Rotation3d objectRotat return objectRotationStart.transformBy(objectRotationStartToGround).getTranslation().toTranslation2d(); } + private boolean isLollipop(Rotation2d objectYaw) { + for (Rotation3d algaeYaw : getTargetObjectsRotations(SimulatedGamePieceConstants.GamePieceType.ALGAE)) { + final double difference = Math.abs(algaeYaw.toRotation2d().minus(objectYaw).getDegrees()); + if (difference < ObjectDetectionCameraConstants.LOLLIPOP_TOLERANCE.getDegrees()) + return true; + } + return false; + } + private ObjectDetectionCameraIO generateIO(String hostname, Transform3d robotCenterToCamera) { if (RobotHardwareStats.isReplay()) return new ObjectDetectionCameraIO(); From 819293ee36a6d176667d76617a2a40da0ec04036 Mon Sep 17 00:00:00 2001 From: Shm Date: Thu, 2 Oct 2025 18:47:52 -0400 Subject: [PATCH 22/27] no hitting reef Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../commands/commandfactories/AutonomousCommands.java | 2 +- .../misc/simulatedfield/SimulatedGamePieceConstants.java | 7 +++---- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index 4ae9d033..2ce1dfa1 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -63,7 +63,7 @@ public static Command getDriveToReefAndScoreCommand() { public static Command getCollectCoralCommand(boolean isRight) { return new ParallelCommandGroup( CoralCollectionCommands.getIntakeSequenceCommand(), - ArmElevatorCommands.getPrepareForStateCommand(() -> ArmElevatorConstants.ArmElevatorState.LOAD_CORAL), + new WaitCommand(0.5).andThen(ArmElevatorCommands.getPrepareForStateCommand(() -> ArmElevatorConstants.ArmElevatorState.LOAD_CORAL)), getDriveToCoralCommand(isRight) ) .until(RobotContainer.INTAKE::hasCoral) 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 93a90793..ceaf583d 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java @@ -71,14 +71,13 @@ 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(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); + NET_MINIMUM_X_LOCATION = new FlippablePose3d(FlippablePose3d.flipPose(new Pose3d(FIELD_LENGTH_METERS - 10.724 + 0.82, 4.730, 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); static final FlippablePose3d - RIGHT_CORAL_SPAWN_POSE = new FlippablePose3d(new Pose3d(1.5, 1.5, 0, new Rotation3d()), true), - LEFT_CORAL_SPAWN_POSE = new FlippablePose3d(new Pose3d(1.5, 6.5, 0, new Rotation3d()), true); + RIGHT_CORAL_SPAWN_POSE = new FlippablePose3d(new Pose3d(1.25, 1.25, 0, new Rotation3d()), true), + LEFT_CORAL_SPAWN_POSE = new FlippablePose3d(new Pose3d(1.25, 6.75, 0, new Rotation3d()), true); static { ALGAE_ON_FIELD.addAll(createAlgaeOnReef()); From 85fdcb5443fd9bc19564fcb8dec7e24b52f71cab Mon Sep 17 00:00:00 2001 From: Shm Date: Thu, 2 Oct 2025 19:58:55 -0400 Subject: [PATCH 23/27] made first coral different Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../commandfactories/AutonomousCommands.java | 13 +++++++++++-- .../trigon/robot/constants/AutonomousConstants.java | 11 ++++++++--- 2 files changed, 19 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index 2ce1dfa1..9ddfa448 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -30,6 +30,7 @@ public class AutonomousCommands { public static final LoggedNetworkBoolean[] SCORED_L4S = getEmptyL4LoggedNetworkBooleanArray(); private static FlippablePose2d TARGET_SCORING_POSE = null; + private static boolean IS_FIRST_CORAL = true; public static Command getFloorAutonomousCommand(boolean isRight) { return getCycleCoralCommand(isRight).repeatedly().withName("FloorAutonomous" + (isRight ? "Right" : "Left")); @@ -44,7 +45,7 @@ public static Command getCycleCoralCommand(boolean isRight) { public static Command getFindCoralCommand(boolean isRight) { return new SequentialCommandGroup( - SwerveCommands.getDriveToPoseCommand(() -> isRight ? AutonomousConstants.AUTO_FIND_CORAL_POSE_RIGHT : AutonomousConstants.AUTO_FIND_CORAL_POSE_LEFT, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS, AutonomousConstants.AUTO_FIND_CORAL_END_VELOCITY_METERS_PER_SECOND), + getDriveToFindCoralPoseCommand(isRight), SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> 0, () -> 0, @@ -53,6 +54,14 @@ public static Command getFindCoralCommand(boolean isRight) { ); } + private static Command getDriveToFindCoralPoseCommand(boolean isRight) { + return new ConditionalCommand( + SwerveCommands.getDriveToPoseCommand(() -> isRight ? AutonomousConstants.AUTO_FIND_CORAL_POSE_RIGHT : AutonomousConstants.AUTO_FIND_CORAL_POSE_LEFT, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS, AutonomousConstants.AUTO_FIND_CORAL_END_VELOCITY_METERS_PER_SECOND), + SwerveCommands.getDriveToPoseCommand(() -> isRight ? AutonomousConstants.AUTO_FIND_FIRST_CORAL_POSE_RIGHT : AutonomousConstants.AUTO_FIND_FIRST_CORAL_POSE_LEFT, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS, AutonomousConstants.AUTO_FIND_CORAL_END_VELOCITY_METERS_PER_SECOND).alongWith(new InstantCommand(() -> IS_FIRST_CORAL = false)), + () -> !IS_FIRST_CORAL + ); + } + public static Command getDriveToReefAndScoreCommand() { return new ParallelRaceGroup( getDriveToReefCommand(), @@ -63,7 +72,7 @@ public static Command getDriveToReefAndScoreCommand() { public static Command getCollectCoralCommand(boolean isRight) { return new ParallelCommandGroup( CoralCollectionCommands.getIntakeSequenceCommand(), - new WaitCommand(0.5).andThen(ArmElevatorCommands.getPrepareForStateCommand(() -> ArmElevatorConstants.ArmElevatorState.LOAD_CORAL)), + ArmElevatorCommands.getPrepareForStateCommand(() -> ArmElevatorConstants.ArmElevatorState.LOAD_CORAL), getDriveToCoralCommand(isRight) ) .until(RobotContainer.INTAKE::hasCoral) diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index 424d8fd4..48ac2de2 100644 --- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -36,11 +36,16 @@ public class AutonomousConstants { private static final double AUTO_FIND_CORAL_POSE_X = 3.3, - AUTO_FIND_CORAL_POSE_LEFT_Y = 5.5; - private static final Rotation2d AUTO_FIND_CORAL_POSE_LEFT_ROTATION = Rotation2d.fromDegrees(130); + AUTO_FIND_CORAL_POSE_LEFT_Y = 5.5, + AUTO_FIND_FIRST_CORAL_POST_LEFT_Y = 6.5; + private static final Rotation2d + AUTO_FIND_CORAL_POSE_LEFT_ROTATION = Rotation2d.fromDegrees(130), + AUTO_FIND_FIRST_CORAL_POSE_LEFT_ROTATION = Rotation2d.fromDegrees(170); public static final FlippablePose2d AUTO_FIND_CORAL_POSE_LEFT = new FlippablePose2d(AUTO_FIND_CORAL_POSE_X, AUTO_FIND_CORAL_POSE_LEFT_Y, AUTO_FIND_CORAL_POSE_LEFT_ROTATION, true), - AUTO_FIND_CORAL_POSE_RIGHT = new FlippablePose2d(AUTO_FIND_CORAL_POSE_X, FieldConstants.FIELD_WIDTH_METERS - AUTO_FIND_CORAL_POSE_LEFT_Y, AUTO_FIND_CORAL_POSE_LEFT_ROTATION.unaryMinus(), true); + AUTO_FIND_CORAL_POSE_RIGHT = new FlippablePose2d(AUTO_FIND_CORAL_POSE_X, FieldConstants.FIELD_WIDTH_METERS - AUTO_FIND_CORAL_POSE_LEFT_Y, AUTO_FIND_CORAL_POSE_LEFT_ROTATION.unaryMinus(), true), + AUTO_FIND_FIRST_CORAL_POSE_LEFT = new FlippablePose2d(AUTO_FIND_CORAL_POSE_X, AUTO_FIND_FIRST_CORAL_POST_LEFT_Y, AUTO_FIND_FIRST_CORAL_POSE_LEFT_ROTATION, true), + AUTO_FIND_FIRST_CORAL_POSE_RIGHT = new FlippablePose2d(AUTO_FIND_CORAL_POSE_X, FieldConstants.FIELD_WIDTH_METERS - AUTO_FIND_FIRST_CORAL_POST_LEFT_Y, AUTO_FIND_FIRST_CORAL_POSE_LEFT_ROTATION.unaryMinus(), true); public static final double AUTO_FIND_CORAL_END_VELOCITY_METERS_PER_SECOND = 2.5, AUTO_FIND_CORAL_ROTATION_POWER = 0.2; From f00fa2faa2b7f3ed6dfeac16d40310e8ea962dcf Mon Sep 17 00:00:00 2001 From: Shm Date: Thu, 2 Oct 2025 22:03:55 -0400 Subject: [PATCH 24/27] stuff works better Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../AlgaeManipulationCommands.java | 6 +++--- .../commandfactories/CoralCollectionCommands.java | 8 ++++++-- .../commandfactories/CoralPlacingCommands.java | 15 ++++++--------- .../trigon/robot/constants/FieldConstants.java | 2 +- .../robot/subsystems/armelevator/ArmElevator.java | 3 +++ .../robot/subsystems/endeffector/EndEffector.java | 4 ++++ 6 files changed, 23 insertions(+), 15 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 ec50c158..41268ba7 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -48,12 +48,12 @@ public static void toggleLollipopCollection() { public static Command getFloorAlgaeCollectionCommand() { return new SequentialCommandGroup( GeneralCommands.getResetFlipArmOverrideCommand(), + CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.END_EFFECTOR::hasGamePiece), + getInitiateFloorAlgaeCollectionCommand().until(RobotContainer.END_EFFECTOR::hasGamePiece), new InstantCommand(() -> { IS_HOLDING_ALGAE = true; SHOULD_COLLECT_FROM_LOLLIPOP = false; }), - CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.END_EFFECTOR::hasGamePiece), - getInitiateFloorAlgaeCollectionCommand().until(RobotContainer.END_EFFECTOR::hasGamePiece), GeneralCommands.getResetFlipArmOverrideCommand(), getScoreAlgaeCommand().alongWith(getAlgaeCollectionConfirmationCommand()) .until(() -> !RobotContainer.END_EFFECTOR.hasGamePiece() && !isScoreAlgaeButtonPressed()) @@ -64,8 +64,8 @@ public static Command getReefAlgaeCollectionCommand() { return new SequentialCommandGroup( GeneralCommands.getResetFlipArmOverrideCommand(), CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.END_EFFECTOR::hasGamePiece), - new InstantCommand(() -> IS_HOLDING_ALGAE = true), getInitiateReefAlgaeCollectionCommand().until(RobotContainer.END_EFFECTOR::hasGamePiece), + new InstantCommand(() -> IS_HOLDING_ALGAE = true), GeneralCommands.getResetFlipArmOverrideCommand(), getScoreAlgaeCommand().alongWith(getAlgaeCollectionConfirmationCommand()) .until(() -> !RobotContainer.END_EFFECTOR.hasGamePiece() && !isScoreAlgaeButtonPressed()) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java index 8a88d3b1..14f12b00 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -26,14 +26,14 @@ public static Command getCoralCollectionCommand() { getLoadCoralCommand().schedule(); } ) - ).alongWith(new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE)); + ).alongWith(new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE)).unless(CoralCollectionCommands::hasCoral); } public static Command getLoadCoralCommand() { return new ParallelCommandGroup( ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.LOAD_CORAL), EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.LOAD_CORAL) - ).until(RobotContainer.END_EFFECTOR::hasGamePiece).onlyWhile(() -> !RobotContainer.END_EFFECTOR.hasGamePiece()); + ).until(RobotContainer.END_EFFECTOR::hasGamePiece); } public static Command getUnloadCoralCommand() { @@ -64,4 +64,8 @@ private static Command getAlignCoralCommand() { private static Command getCollectionConfirmationCommand() { return new InstantCommand(() -> OperatorConstants.DRIVER_CONTROLLER.rumble(OperatorConstants.RUMBLE_DURATION_SECONDS, OperatorConstants.RUMBLE_POWER)); } + + static boolean hasCoral() { + return (!AlgaeManipulationCommands.isHoldingAlgae() && RobotContainer.END_EFFECTOR.hasGamePiece()) || RobotContainer.TRANSPORTER.hasCoral(); + } } \ No newline at end of file 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 15b6fcb4..a7367cf0 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -4,10 +4,7 @@ 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.Command; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; -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.AutonomousConstants; import frc.trigon.robot.constants.FieldConstants; @@ -32,7 +29,7 @@ public static Command getScoreInReefCommand(boolean shouldScoreRight) { getScoreCommand(shouldScoreRight), () -> SHOULD_SCORE_AUTONOMOUSLY && REEF_CHOOSER.getScoringLevel() != ScoringLevel.L1 ) - ).onlyIf(() -> RobotContainer.END_EFFECTOR.hasGamePiece() || RobotContainer.TRANSPORTER.hasCoral()); + ).onlyIf(CoralCollectionCommands::hasCoral); } private static Command getAutonomouslyScoreCommand(boolean shouldScoreRight) { @@ -59,7 +56,8 @@ private static Command getAutonomouslyPrepareScoreCommand(boolean shouldScoreRig return new ParallelCommandGroup( getPrepareArmElevatorIfWontHitReef(shouldScoreRight), new SequentialCommandGroup( - getAutonomousDriveToNoHitReefPose(shouldScoreRight).asProxy().onlyWhile(() -> !isPrepareArmAngleAboveCurrentArmAngle()), + getAutonomousDriveToNoHitReefPose(shouldScoreRight).asProxy().until(CoralPlacingCommands::isPrepareArmAngleAboveCurrentArmAngle), + new WaitUntilCommand(CoralPlacingCommands::isPrepareArmAngleAboveCurrentArmAngle), getAutonomousDriveToReef(shouldScoreRight).asProxy() ).unless(() -> REEF_CHOOSER.getScoringLevel() == ScoringLevel.L1) ); @@ -132,7 +130,6 @@ private static FlippablePose2d calculateClosestNoHitReefPose(boolean shouldScore for (final Rotation2d targetRotation : reefClockAngles) { final Pose2d reefCenterAtTargetRotation = new Pose2d(reefCenterPosition, targetRotation); - final Pose2d currentScoringPose = reefCenterAtTargetRotation.transformBy(reefCenterToScoringPose); final double distanceFromCurrentScoringPoseMeters = currentScoringPose.getTranslation().getDistance(robotPositionOnField); if (distanceFromCurrentScoringPoseMeters < distanceFromClosestScoringPoseMeters) { @@ -150,8 +147,8 @@ private static FlippablePose2d calculateClosestNoHitReefPose(boolean shouldScore return new FlippablePose2d(closestScoringPose.transformBy(scoringPoseToBranch), false); } - private static boolean isPrepareArmAngleAboveCurrentArmAngle() { - ArmElevatorConstants.ArmElevatorState targetState = REEF_CHOOSER.getArmElevatorState(); + public static boolean isPrepareArmAngleAboveCurrentArmAngle() { + final ArmElevatorConstants.ArmElevatorState targetState = REEF_CHOOSER.getArmElevatorState(); final Rotation2d targetAngle = targetState.prepareState == null ? targetState.targetAngle : targetState.prepareState.targetAngle; diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 70f2ee0f..8c71b297 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -32,7 +32,7 @@ public class FieldConstants { REEF_CENTER_TO_TARGET_SCORING_POSITION_Y_TRANSFORM_METERS = 0.17, REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS = 1.6, 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; + public static final double SAFE_DISTANCE_FROM_SCORING_POSE_METERS = 0.25; private static final boolean SHOULD_USE_HOME_TAG_LAYOUT = false; public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = createAprilTagFieldLayout(); 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 567e26e9..8d0a3fe2 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevator.java @@ -8,6 +8,8 @@ import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.commandfactories.CoralCollectionCommands; +import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands; import frc.trigon.robot.subsystems.MotorSubsystem; import lib.hardware.phoenix6.cancoder.CANcoderEncoder; import lib.hardware.phoenix6.cancoder.CANcoderSignal; @@ -94,6 +96,7 @@ public void updatePeriodically() { elevatorMasterMotor.update(); Logger.recordOutput("Elevator/CurrentPositionMeters", getCurrentElevatorPositionMeters()); Logger.recordOutput("Arm/CurrentPositionDegrees", getCurrentArmAngle().getDegrees()); + Logger.recordOutput("Arm/isPrepareArmAngleAboveCurrentArmAngle", CoralPlacingCommands.isPrepareArmAngleAboveCurrentArmAngle()); } @Override 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 8d4ecfe5..ff915dd4 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java @@ -3,11 +3,13 @@ import com.ctre.phoenix6.controls.VoltageOut; import edu.wpi.first.math.geometry.Translation3d; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.commandfactories.AlgaeManipulationCommands; import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; import frc.trigon.robot.subsystems.MotorSubsystem; import lib.hardware.phoenix6.talonfx.TalonFXMotor; import lib.hardware.phoenix6.talonfx.TalonFXSignal; import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; public class EndEffector extends MotorSubsystem { private final TalonFXMotor endEffectorMotor = EndEffectorConstants.END_EFFECTOR_MOTOR; @@ -27,6 +29,8 @@ public void updateMechanism() { public void updatePeriodically() { endEffectorMotor.update(); EndEffectorConstants.DISTANCE_SENSOR.updateSensor(); + + Logger.recordOutput("EndEffector/isHoldingAlgae", AlgaeManipulationCommands.isHoldingAlgae()); } @AutoLogOutput(key = "EndEffector/HasCoral") From cb9f89cab1f02149989ee6c9b89d0ce4303faf8d Mon Sep 17 00:00:00 2001 From: Shm Date: Thu, 2 Oct 2025 23:06:59 -0400 Subject: [PATCH 25/27] to drives Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../commandfactories/AutonomousCommands.java | 23 +++++++++++++++++-- .../robot/constants/AutonomousConstants.java | 13 +++++++---- 2 files changed, 30 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index 9ddfa448..bef54960 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -56,12 +56,31 @@ public static Command getFindCoralCommand(boolean isRight) { private static Command getDriveToFindCoralPoseCommand(boolean isRight) { return new ConditionalCommand( - SwerveCommands.getDriveToPoseCommand(() -> isRight ? AutonomousConstants.AUTO_FIND_CORAL_POSE_RIGHT : AutonomousConstants.AUTO_FIND_CORAL_POSE_LEFT, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS, AutonomousConstants.AUTO_FIND_CORAL_END_VELOCITY_METERS_PER_SECOND), - SwerveCommands.getDriveToPoseCommand(() -> isRight ? AutonomousConstants.AUTO_FIND_FIRST_CORAL_POSE_RIGHT : AutonomousConstants.AUTO_FIND_FIRST_CORAL_POSE_LEFT, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS, AutonomousConstants.AUTO_FIND_CORAL_END_VELOCITY_METERS_PER_SECOND).alongWith(new InstantCommand(() -> IS_FIRST_CORAL = false)), + SwerveCommands.getDriveToPoseCommand( + () -> isRight ? AutonomousConstants.AUTO_FIND_CORAL_POSE_RIGHT : AutonomousConstants.AUTO_FIND_CORAL_POSE_LEFT, + AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS, + AutonomousConstants.AUTO_FIND_CORAL_END_VELOCITY_METERS_PER_SECOND + ), + getDriveToFirstCoralPoseCommand(isRight), () -> !IS_FIRST_CORAL ); } + private static Command getDriveToFirstCoralPoseCommand(boolean isRight) { + return new SequentialCommandGroup( + SwerveCommands.getDriveToPoseCommand( + () -> isRight ? AutonomousConstants.AUTO_FIND_FIRST_CORAL_FIRST_POSE_RIGHT : AutonomousConstants.AUTO_FIND_FIRST_CORAL_FIRST_POSE_LEFT, + AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS, + AutonomousConstants.AUTO_FIND_CORAL_END_VELOCITY_METERS_PER_SECOND + ), + SwerveCommands.getDriveToPoseCommand( + () -> isRight ? AutonomousConstants.AUTO_FIND_FIRST_CORAL_SECOND_POSE_RIGHT : AutonomousConstants.AUTO_FIND_FIRST_CORAL_SECOND_POSE_LEFT, + AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS, + AutonomousConstants.AUTO_FIND_CORAL_END_VELOCITY_METERS_PER_SECOND + ) + ).alongWith(new InstantCommand(() -> IS_FIRST_CORAL = false)); + } + public static Command getDriveToReefAndScoreCommand() { return new ParallelRaceGroup( getDriveToReefCommand(), diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index 48ac2de2..a13abb42 100644 --- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -37,17 +37,22 @@ public class AutonomousConstants { private static final double AUTO_FIND_CORAL_POSE_X = 3.3, AUTO_FIND_CORAL_POSE_LEFT_Y = 5.5, - AUTO_FIND_FIRST_CORAL_POST_LEFT_Y = 6.5; + AUTO_FIND_FIRST_CORAL_FIRST_POSE_X = 4.6, + AUTO_FIND_FIRST_CORAL_FIRST_POSE_LEFT_Y = 6.1, + AUTO_FIND_FIRST_CORAL_SECOND_POSE_X = 2.75, + AUTO_FIND_FIRST_CORAL_SECOND_POSE_LEFT_Y = 6.5; private static final Rotation2d AUTO_FIND_CORAL_POSE_LEFT_ROTATION = Rotation2d.fromDegrees(130), AUTO_FIND_FIRST_CORAL_POSE_LEFT_ROTATION = Rotation2d.fromDegrees(170); public static final FlippablePose2d AUTO_FIND_CORAL_POSE_LEFT = new FlippablePose2d(AUTO_FIND_CORAL_POSE_X, AUTO_FIND_CORAL_POSE_LEFT_Y, AUTO_FIND_CORAL_POSE_LEFT_ROTATION, true), AUTO_FIND_CORAL_POSE_RIGHT = new FlippablePose2d(AUTO_FIND_CORAL_POSE_X, FieldConstants.FIELD_WIDTH_METERS - AUTO_FIND_CORAL_POSE_LEFT_Y, AUTO_FIND_CORAL_POSE_LEFT_ROTATION.unaryMinus(), true), - AUTO_FIND_FIRST_CORAL_POSE_LEFT = new FlippablePose2d(AUTO_FIND_CORAL_POSE_X, AUTO_FIND_FIRST_CORAL_POST_LEFT_Y, AUTO_FIND_FIRST_CORAL_POSE_LEFT_ROTATION, true), - AUTO_FIND_FIRST_CORAL_POSE_RIGHT = new FlippablePose2d(AUTO_FIND_CORAL_POSE_X, FieldConstants.FIELD_WIDTH_METERS - AUTO_FIND_FIRST_CORAL_POST_LEFT_Y, AUTO_FIND_FIRST_CORAL_POSE_LEFT_ROTATION.unaryMinus(), true); + AUTO_FIND_FIRST_CORAL_FIRST_POSE_LEFT = new FlippablePose2d(AUTO_FIND_FIRST_CORAL_FIRST_POSE_X, AUTO_FIND_FIRST_CORAL_FIRST_POSE_LEFT_Y, AUTO_FIND_FIRST_CORAL_POSE_LEFT_ROTATION, true), + AUTO_FIND_FIRST_CORAL_FIRST_POSE_RIGHT = new FlippablePose2d(AUTO_FIND_FIRST_CORAL_FIRST_POSE_X, FieldConstants.FIELD_WIDTH_METERS - AUTO_FIND_FIRST_CORAL_FIRST_POSE_LEFT_Y, AUTO_FIND_FIRST_CORAL_POSE_LEFT_ROTATION.unaryMinus(), true), + AUTO_FIND_FIRST_CORAL_SECOND_POSE_LEFT = new FlippablePose2d(AUTO_FIND_FIRST_CORAL_SECOND_POSE_X, AUTO_FIND_FIRST_CORAL_SECOND_POSE_LEFT_Y, AUTO_FIND_FIRST_CORAL_POSE_LEFT_ROTATION, true), + AUTO_FIND_FIRST_CORAL_SECOND_POSE_RIGHT = new FlippablePose2d(AUTO_FIND_FIRST_CORAL_SECOND_POSE_X, FieldConstants.FIELD_WIDTH_METERS - AUTO_FIND_FIRST_CORAL_SECOND_POSE_LEFT_Y, AUTO_FIND_FIRST_CORAL_POSE_LEFT_ROTATION.unaryMinus(), true); public static final double - AUTO_FIND_CORAL_END_VELOCITY_METERS_PER_SECOND = 2.5, + AUTO_FIND_CORAL_END_VELOCITY_METERS_PER_SECOND = 3.5, AUTO_FIND_CORAL_ROTATION_POWER = 0.2; private static final PIDConstants From 1ebfe5b07402c0d71c3da5e7ec50327407a35876 Mon Sep 17 00:00:00 2001 From: Shm Date: Thu, 2 Oct 2025 23:45:01 -0400 Subject: [PATCH 26/27] added camera coordinates Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../java/frc/trigon/robot/RobotContainer.java | 6 ++- .../robot/constants/CameraConstants.java | 45 +++++++++++++++---- 2 files changed, 41 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 0f2f2182..361bc594 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -43,7 +43,11 @@ import java.util.List; public class RobotContainer { - public static final PoseEstimator ROBOT_POSE_ESTIMATOR = new PoseEstimator(); + public static final PoseEstimator ROBOT_POSE_ESTIMATOR = new PoseEstimator( + CameraConstants.INTAKE_SIDE_REEF_TAG_CAMERA, + CameraConstants.LEFT_REEF_TAG_CAMERA, + CameraConstants.RIGHT_REEF_TAG_CAMERA + ); public static final ObjectPoseEstimator CORAL_POSE_ESTIMATOR = new ObjectPoseEstimator( CameraConstants.OBJECT_POSE_ESTIMATOR_DELETION_THRESHOLD_SECONDS, ObjectPoseEstimator.DistanceCalculationMethod.TRANSLATION, diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 3c6ec869..418efb00 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -10,10 +10,28 @@ import frc.trigon.robot.poseestimation.poseestimator.StandardDeviations; public class CameraConstants { - private static final Transform3d ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA = new Transform3d( + private static final StandardDeviations + REEF_TAG_CAMERA_STANDARD_DEVIATIONS = new StandardDeviations( + 0.015, + 0.01 + ); + private static final Transform3d + ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA = new Transform3d( new Translation3d(0.2015, 0.195, 0.7156), new Rotation3d(0, Units.degreesToRadians(30), 0) - ); + ), + ROBOT_CENTER_TO_INTAKE_REEF_TAG_CAMERA = new Transform3d( + new Translation3d(0.2247, 0.195, 0.7498), + new Rotation3d(0, Units.degreesToRadians(90 - 51), 0) + ), + ROBOT_CENTER_TO_LEFT_REEF_TAG_CAMERA = new Transform3d( + new Translation3d(-0.129, 0.2032, 0.1258), + new Rotation3d(0, Units.degreesToRadians(60), 0) + ), + ROBOT_CENTER_TO_RIGHT_REEF_TAG_CAMERA = new Transform3d( + new Translation3d(-0.129, -0.2032, 0.1258), + new Rotation3d(0, Units.degreesToRadians(60), 0) + ); public static final double OBJECT_POSE_ESTIMATOR_DELETION_THRESHOLD_SECONDS = 1; public static final ObjectDetectionCamera OBJECT_DETECTION_CAMERA = new ObjectDetectionCamera( @@ -21,13 +39,22 @@ public class CameraConstants { ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA ); public static final AprilTagCamera - INTAKE_SIDE_CAMERA = new AprilTagCamera( + INTAKE_SIDE_REEF_TAG_CAMERA = new AprilTagCamera( AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, - "IntakeSideCamera", - new Transform3d( - new Translation3d(0.2247, 0.195, 0.7498), - new Rotation3d(0, Units.degreesToRadians(51), 0) + "IntakeSideReefTagCameraCamera", + ROBOT_CENTER_TO_INTAKE_REEF_TAG_CAMERA, + REEF_TAG_CAMERA_STANDARD_DEVIATIONS + ), + LEFT_REEF_TAG_CAMERA = new AprilTagCamera( + AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, + "LeftReefTagCamera", + ROBOT_CENTER_TO_LEFT_REEF_TAG_CAMERA, + REEF_TAG_CAMERA_STANDARD_DEVIATIONS ), - new StandardDeviations(0, 0) //TOD0: Measure - ); + RIGHT_REEF_TAG_CAMERA = new AprilTagCamera( + AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, + "RightReefTagCamera", + ROBOT_CENTER_TO_RIGHT_REEF_TAG_CAMERA, + REEF_TAG_CAMERA_STANDARD_DEVIATIONS + ); } \ No newline at end of file From 1668e3f05d42ad7294fe73f541238bee42300506 Mon Sep 17 00:00:00 2001 From: Shm Date: Thu, 2 Oct 2025 23:50:37 -0400 Subject: [PATCH 27/27] oops Co-Authored-By: Nummun14 <142012009+Nummun14@users.noreply.github.com> --- .../armelevator/ArmElevatorConstants.java | 42 +++++++++---------- 1 file changed, 21 insertions(+), 21 deletions(-) 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 6dc98222..eb4e9a9a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -327,27 +327,27 @@ private static void configureReverseLimitSensor() { } public enum ArmElevatorState { - PREPARE_SCORE_L1(Rotation2d.fromDegrees(110), 0.3, null, false, 1), - PREPARE_SCORE_L2(Rotation2d.fromDegrees(100), 0.3, null, false, 1), - PREPARE_SCORE_L3(Rotation2d.fromDegrees(100), 0.7, null, false, 1), - PREPARE_SCORE_L4(Rotation2d.fromDegrees(120), 1.2, null, false, 1), - REST(Rotation2d.fromDegrees(0), 0.603, null, false, 0.7), - REST_WITH_CORAL(Rotation2d.fromDegrees(180), 0.603, null, false, 0.7), - REST_WITH_ALGAE(Rotation2d.fromDegrees(90), 0.603, null, false, 0.7), - REST_FOR_CLIMB(Rotation2d.fromDegrees(180), 0.603, null, false, 0.7), - LOAD_CORAL(Rotation2d.fromDegrees(0), 0.5519, REST, true, 0.7), - UNLOAD_CORAL(Rotation2d.fromDegrees(0), 0.603, null, false, 0.7), - EJECT(Rotation2d.fromDegrees(60), 0.603, null, false, 0.7), - SCORE_L1(Rotation2d.fromDegrees(70), 0.4, null, false, 1), - SCORE_L2(Rotation2d.fromDegrees(90), 0.3, PREPARE_SCORE_L2, false, 1), - 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.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), - COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(70), 0.1, null, true, 1); + PREPARE_SCORE_L1(Rotation2d.fromDegrees(110), 0.29, null, false, 1), + PREPARE_SCORE_L2(Rotation2d.fromDegrees(100), 0.29, null, false, 1), + PREPARE_SCORE_L3(Rotation2d.fromDegrees(100), 0.69, null, false, 1), + PREPARE_SCORE_L4(Rotation2d.fromDegrees(120), 1.19, null, false, 1), + REST(Rotation2d.fromDegrees(0), 0.593, null, false, 0.7), + REST_WITH_CORAL(Rotation2d.fromDegrees(180), 0.593, null, false, 0.7), + REST_WITH_ALGAE(Rotation2d.fromDegrees(90), 0.593, null, false, 0.7), + REST_FOR_CLIMB(Rotation2d.fromDegrees(180), 0.593, null, false, 0.7), + LOAD_CORAL(Rotation2d.fromDegrees(0), 0.5419, REST, true, 0.7), + UNLOAD_CORAL(Rotation2d.fromDegrees(0), 0.5419, null, false, 0.7), + EJECT(Rotation2d.fromDegrees(60), 0.593, null, false, 0.7), + SCORE_L1(Rotation2d.fromDegrees(70), 0.39, null, false, 1), + SCORE_L2(Rotation2d.fromDegrees(90), 0.29, PREPARE_SCORE_L2, false, 1), + SCORE_L3(Rotation2d.fromDegrees(90), 0.69, PREPARE_SCORE_L3, false, 1), + SCORE_L4(Rotation2d.fromDegrees(100), 1.19, PREPARE_SCORE_L4, false, 1), + SCORE_NET(Rotation2d.fromDegrees(160), 1.372, null, false, 0.3), + SCORE_PROCESSOR(Rotation2d.fromDegrees(90), 0.09, null, false, 0.7), + COLLECT_ALGAE_L2(Rotation2d.fromDegrees(90), 0.593, null, false, 1), + COLLECT_ALGAE_L3(Rotation2d.fromDegrees(90), 0.943, null, false, 1), + COLLECT_ALGAE_LOLLIPOP(Rotation2d.fromDegrees(70), 0.29, null, false, 1), + COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(70), 0.09, null, true, 1); public final Rotation2d targetAngle; public final double targetPositionMeters;