diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 00d92645..b54374ce 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -6,6 +6,7 @@ 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.InstantCommand; @@ -40,11 +41,17 @@ 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 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.ROTATION_AND_TRANSLATION, + ObjectPoseEstimator.DistanceCalculationMethod.TRANSLATION, SimulatedGamePieceConstants.GamePieceType.CORAL, CameraConstants.OBJECT_DETECTION_CAMERA ); @@ -122,7 +129,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/commandclasses/CoralAutoDriveCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/CoralAutoDriveCommand.java deleted file mode 100644 index 1182ebfb..00000000 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/CoralAutoDriveCommand.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 CoralAutoDriveCommand extends ParallelCommandGroup { - private static final ObjectPoseEstimator CORAL_POSE_ESTIMATOR = RobotContainer.CORAL_POSE_ESTIMATOR; - private Translation2d distanceFromTrackedGamePiece; - - public CoralAutoDriveCommand() { - 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()), - CoralAutoDriveCommand::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 9fdae10e..ba2d2cec 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -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)), @@ -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) ) ); } @@ -71,36 +71,48 @@ public static Command getAssistIntakeCommand(AssistMode assistMode, Supplier { if (RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null) - distanceFromTrackedGamePiece = calculateDistanceFromTrackedCGamePiece(); + distanceFromTrackedGamePiece = calculateDistanceFromTrackedGamePiece(); }); } - private 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) 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) { + 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()); - 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) { + if (distanceFromTrackedGamePiece == null || !assistMode.shouldAssistTheta) + return 0; + 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) { @@ -115,25 +127,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,15 +156,14 @@ 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) { 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); } /** @@ -164,15 +175,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/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java index 8553061f..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()) @@ -63,9 +63,9 @@ 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), getInitiateReefAlgaeCollectionCommand().until(RobotContainer.END_EFFECTOR::hasGamePiece), + new InstantCommand(() -> IS_HOLDING_ALGAE = true), GeneralCommands.getResetFlipArmOverrideCommand(), getScoreAlgaeCommand().alongWith(getAlgaeCollectionConfirmationCommand()) .until(() -> !RobotContainer.END_EFFECTOR.hasGamePiece() && !isScoreAlgaeButtonPressed()) @@ -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/AutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java index c6d3f53e..bef54960 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,220 @@ 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.constants.OperatorConstants; +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.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; + private static boolean IS_FIRST_CORAL = true; + + 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 getFindCoralCommand(boolean isRight) { + return new SequentialCommandGroup( + getDriveToFindCoralPoseCommand(isRight), + SwerveCommands.getClosedLoopSelfRelativeDriveCommand( + () -> 0, + () -> 0, + () -> AutonomousConstants.AUTO_FIND_CORAL_ROTATION_POWER + ) + ); + } + + 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 + ), + 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(), + getCoralSequenceCommand() + ); + } + + public static Command getCollectCoralCommand(boolean isRight) { + return new ParallelCommandGroup( + CoralCollectionCommands.getIntakeSequenceCommand(), + ArmElevatorCommands.getPrepareForStateCommand(() -> ArmElevatorConstants.ArmElevatorState.LOAD_CORAL), + getDriveToCoralCommand(isRight) + ) + .until(RobotContainer.INTAKE::hasCoral) + .unless(() -> RobotContainer.TRANSPORTER.hasCoral() || RobotContainer.END_EFFECTOR.hasGamePiece()); + } + + public static Command getDriveToReefCommand() { + return new SequentialCommandGroup( + 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() + ); + } + + public static Command getCoralSequenceCommand() { + return new SequentialCommandGroup( + CoralCollectionCommands.getLoadCoralCommand(), + new WaitUntilCommand(() -> TARGET_SCORING_POSE != null), + getScoreCommand() + ); + } + + 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(10), + getFindCoralCommand(isRight).until(() -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null), + () -> CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null + ).repeatedly(); + } + + public static Command getScoreCommand() { + return new SequentialCommandGroup( + getOpenElevatorWhenCloseToReefCommand().until(AutonomousCommands::canScore), + getPlaceCoralCommand() + ); + } + + private static boolean canScore() { + 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; + } + + public static Command getPlaceCoralCommand() { + return new ParallelCommandGroup( + ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.SCORE_L4), + EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_CORAL), + getAddCurrentScoringBranchToScoredBranchesCommand() + ).withTimeout(0.25); + } + + private static Command getOpenElevatorWhenCloseToReefCommand() { + return GeneralCommands.runWhen( + ArmElevatorCommands.getPrepareForStateCommand(() -> ArmElevatorConstants.ArmElevatorState.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 FlippablePose2d calculateClosestOpenScoringPose() { + final boolean[] scoredBranchesAtL4 = getScoredBranchesAtL4(); + final Pose2d currentRobotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + + double closestDistance = Double.POSITIVE_INFINITY; + 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); + } + + 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) { + 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; + } + /** * 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/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java index 1981cb19..14f12b00 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; @@ -16,7 +17,6 @@ import frc.trigon.robot.subsystems.transporter.TransporterConstants; public class CoralCollectionCommands { - public static Command getCoralCollectionCommand() { return new SequentialCommandGroup( getIntakeSequenceCommand(), @@ -26,25 +26,24 @@ public static Command getCoralCollectionCommand() { getLoadCoralCommand().schedule(); } ) - ); - // 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() { 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()); } - private static Command getIntakeSequenceCommand() { + static Command getIntakeSequenceCommand() { return getInitiateCollectionCommand().until(RobotContainer.TRANSPORTER::hasCoral); } @@ -65,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 496874a4..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; @@ -209,6 +206,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) { diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index 424d8fd4..a13abb42 100644 --- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -36,13 +36,23 @@ 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_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_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_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 diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 1b750328..418efb00 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -5,16 +5,56 @@ 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) + 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 = 0.5; + 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 ); + public static final AprilTagCamera + INTAKE_SIDE_REEF_TAG_CAMERA = new AprilTagCamera( + AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, + "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 + ), + 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 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/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 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 ac9fbe27..bb2c395a 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(); 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()); 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/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index 7c5f3835..eb4e9a9a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -349,7 +349,6 @@ public enum ArmElevatorState { 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; public final ArmElevatorState prepareState; 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") 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 389592f2..90505deb 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, + targetState.targetVoltage + ); + } + public Translation3d calculateLinearIntakeVelocity() { double velocityMetersPerSecond = intakeMotor.getSignal(TalonFXSignal.VELOCITY) * 2 * Math.PI * IntakeConstants.WHEEL_RADIUS_METERS; return new Translation3d( 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),