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/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; 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;