diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 2f7bb6fc..0915353b 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -6,17 +6,16 @@ package frc.trigon.robot; import com.pathplanner.lib.auto.AutoBuilder; -import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.commands.commandfactories.*; +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.misc.simulatedfield.SimulationFieldHandler; @@ -92,21 +91,25 @@ private void bindDefaultCommands() { private void initializeGeneralSystems() { Flippable.init(); LEDConstants.init(); - PathPlannerConstants.init(); + AutonomousConstants.init(); } private void bindControllerCommands() { OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND); - OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); +// OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); - OperatorConstants.SPAWN_CORAL_TRIGGER.onTrue(new InstantCommand(() -> SimulationFieldHandler.updateCoralSpawning(new Pose3d(ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose())))); - OperatorConstants.CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getCoralCollectionCommand()); + OperatorConstants.FLOOR_ALGAE_COLLECTION_TRIGGER.toggleOnTrue(AlgaeManipulationCommands.getFloorAlgaeCollectionCommand()); + OperatorConstants.REEF_ALGAE_COLLECTION_TRIGGER.toggleOnTrue(AlgaeManipulationCommands.getReefAlgaeCollectionCommand()); - OperatorConstants.EJECT_CORAL_TRIGGER.whileTrue(CoralEjectionCommands.getCoralEjectionCommand()); + OperatorConstants.CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getCoralCollectionCommand()); OperatorConstants.SCORE_CORAL_LEFT_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(false)); OperatorConstants.SCORE_CORAL_RIGHT_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(true)); + OperatorConstants.EJECT_CORAL_TRIGGER.whileTrue(CoralEjectionCommands.getCoralEjectionCommand()); + OperatorConstants.SPAWN_CORAL_IN_SIMULATION_TRIGGER.onTrue(new InstantCommand(() -> SimulationFieldHandler.updateCoralSpawning(ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose()))); + OperatorConstants.FLIP_ARM_TRIGGER.onTrue(new InstantCommand(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE = !OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE)); + OperatorConstants.LOLLIPOP_ALGAE_TOGGLE_TRIGGER.onTrue(new InstantCommand(AlgaeManipulationCommands::toggleLollipopCollection)); OperatorConstants.CLIMB_TRIGGER.toggleOnTrue(ClimbCommands.getClimbCommand()); } 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/CoralAutoDriveCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/CoralAutoDriveCommand.java index 026e0680..1182ebfb 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/CoralAutoDriveCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/CoralAutoDriveCommand.java @@ -6,7 +6,7 @@ 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.subsystems.swerve.SwerveCommands; import lib.utilities.flippable.FlippableRotation2d; @@ -44,16 +44,16 @@ public static Translation2d calculateDistanceFromTrackedGamePiece() { final Translation2d robotToGamePiece = robotPose.getTranslation().minus(trackedObjectPositionOnField); var distanceFromTrackedGamePiece = robotToGamePiece.rotateBy(robotPose.getRotation().unaryMinus()); Logger.recordOutput("GamePieceAutoDrive/DistanceFromTrackedGamePiece", distanceFromTrackedGamePiece); - Logger.recordOutput("GamePieceAutoDrive/XDistanceFromTrackedGamePiece", PathPlannerConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.getSetpoint().position); + Logger.recordOutput("GamePieceAutoDrive/XDistanceFromTrackedGamePiece", AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.getSetpoint().position); return distanceFromTrackedGamePiece; } public static Command getDriveToGamePieceCommand(Supplier distanceFromTrackedGamePiece) { return new SequentialCommandGroup( - new InstantCommand(() -> PathPlannerConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.get().getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().vxMetersPerSecond)), + new InstantCommand(() -> AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.reset(distanceFromTrackedGamePiece.get().getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().vxMetersPerSecond)), SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> PathPlannerConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.get().getX()), - () -> PathPlannerConstants.GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.get().getY()), + () -> AutonomousConstants.GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.get().getX()), + () -> AutonomousConstants.GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER.calculate(distanceFromTrackedGamePiece.get().getY()), CoralAutoDriveCommand::calculateTargetAngle ) ); @@ -61,7 +61,7 @@ public static Command getDriveToGamePieceCommand(Supplier distanc public static boolean shouldMoveTowardsGamePiece(Translation2d distanceFromTrackedGamePiece) { return distanceFromTrackedGamePiece != null && - (distanceFromTrackedGamePiece.getNorm() > PathPlannerConstants.AUTO_COLLECTION_INTAKE_OPEN_CHECK_DISTANCE_METERS);//TODO: If intake is open + (distanceFromTrackedGamePiece.getNorm() > AutonomousConstants.AUTO_COLLECTION_INTAKE_OPEN_CHECK_DISTANCE_METERS);//TODO: If intake is open } public static FlippableRotation2d calculateTargetAngle() { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java new file mode 100644 index 00000000..8553061f --- /dev/null +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -0,0 +1,260 @@ +package frc.trigon.robot.commands.commandfactories; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.*; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.CommandConstants; +import frc.trigon.robot.commands.commandclasses.WaitUntilChangeCommand; +import frc.trigon.robot.constants.AutonomousConstants; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.constants.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.intake.IntakeCommands; +import frc.trigon.robot.subsystems.intake.IntakeConstants; +import frc.trigon.robot.subsystems.swerve.SwerveCommands; +import lib.hardware.RobotHardwareStats; +import lib.utilities.flippable.Flippable; +import lib.utilities.flippable.FlippablePose2d; +import lib.utilities.flippable.FlippableRotation2d; +import lib.utilities.flippable.FlippableTranslation2d; + +import java.util.List; +import java.util.Map; + +public class AlgaeManipulationCommands { + private static boolean + IS_HOLDING_ALGAE = false, + SHOULD_COLLECT_FROM_LOLLIPOP = false; + private static final PIDController SMALL_POSITION_ALIGN_PID_CONTROLLER = + RobotHardwareStats.isSimulation() ? + new PIDController(3, 0, 0) : + new PIDController(3, 0, 0); + + public static boolean isHoldingAlgae() { + return IS_HOLDING_ALGAE; + } + + public static void toggleLollipopCollection() { + SHOULD_COLLECT_FROM_LOLLIPOP = !SHOULD_COLLECT_FROM_LOLLIPOP; + } + + public static Command getFloorAlgaeCollectionCommand() { + return new SequentialCommandGroup( + GeneralCommands.getResetFlipArmOverrideCommand(), + new InstantCommand(() -> { + IS_HOLDING_ALGAE = true; + 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()) + ).finallyDo(() -> IS_HOLDING_ALGAE = false); + } + + 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), + GeneralCommands.getResetFlipArmOverrideCommand(), + getScoreAlgaeCommand().alongWith(getAlgaeCollectionConfirmationCommand()) + .until(() -> !RobotContainer.END_EFFECTOR.hasGamePiece() && !isScoreAlgaeButtonPressed()) + ) + .alongWith(getAlignToReefCommand()) + .finallyDo(() -> IS_HOLDING_ALGAE = false); + } + + private static Command getAlignToReefCommand() { + return new SequentialCommandGroup( + SwerveCommands.getDriveToPoseCommand( + AlgaeManipulationCommands::calculateClosestAlgaeCollectionPose, + AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS + ), + SwerveCommands.getClosedLoopSelfRelativeDriveCommand( + () -> fieldRelativePowersToSelfRelativeXPower(OperatorConstants.DRIVER_CONTROLLER.getLeftY(), OperatorConstants.DRIVER_CONTROLLER.getLeftX()), + () -> SMALL_POSITION_ALIGN_PID_CONTROLLER.calculate(calculateReefRelativeYOffset(), 0), + () -> calculateClosestAlgaeCollectionPose().getRotation() + ) + ).raceWith( + new WaitUntilCommand(RobotContainer.END_EFFECTOR::hasGamePiece), + new WaitUntilCommand(OperatorConstants.STOP_REEF_ALGAE_ALIGN_TRIGGER) + ).onlyIf(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY).asProxy(); + } + + private static Command getScoreAlgaeCommand() { + return new SelectCommand<>( + Map.of( + 0, getHoldAlgaeCommand(), + 1, getScoreInNetCommand(), + 2, getScoreInProcessorCommand() + ), + AlgaeManipulationCommands::getAlgaeScoreMethodSelector + ).raceWith(new WaitUntilChangeCommand<>(AlgaeManipulationCommands::isScoreAlgaeButtonPressed)).repeatedly(); + } + + private static Command getHoldAlgaeCommand() { + return new ParallelCommandGroup( + ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.REST_WITH_ALGAE), + EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.HOLD_ALGAE) + ); + } + + private static Command getScoreInNetCommand() { + return new ParallelRaceGroup( + GeneralCommands.getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState.SCORE_NET, false, AlgaeManipulationCommands::shouldReverseNetScore), + GeneralCommands.runWhen(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_ALGAE), OperatorConstants.CONTINUE_TRIGGER), + getDriveToNetCommand() + ); + } + + private static Command getScoreInProcessorCommand() { + return new ParallelCommandGroup( + GeneralCommands.getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState.SCORE_PROCESSOR, false), + GeneralCommands.runWhen(EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_ALGAE), OperatorConstants.CONTINUE_TRIGGER), + getDriveToProcessorCommand() + ).finallyDo(GeneralCommands.getFieldRelativeDriveCommand()::schedule); + } + + private static Command getDriveToNetCommand() { + return new SequentialCommandGroup( + SwerveCommands.getDriveToPoseCommand( + AlgaeManipulationCommands::calculateClosestNetScoringPose, + AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS + ).until(() -> RobotContainer.SWERVE.atPose(calculateClosestNetScoringPose())), + SwerveCommands.getClosedLoopFieldRelativeDriveCommand( + () -> -SMALL_POSITION_ALIGN_PID_CONTROLLER.calculate(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getX(), calculateClosestNetScoringPose().get().getX()), + () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), + () -> new FlippableRotation2d(shouldReverseNetScore() ? Rotation2d.kZero : Rotation2d.k180deg, true) + ) + ).asProxy().onlyWhile(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY); + } + + private static Command getDriveToProcessorCommand() { + return SwerveCommands.getDriveToPoseCommand( + () -> FieldConstants.FLIPPABLE_PROCESSOR_SCORE_POSE, + AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS + ).asProxy() + .until(() -> RobotContainer.SWERVE.atPose(FieldConstants.FLIPPABLE_PROCESSOR_SCORE_POSE)) + .onlyWhile(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY); + } + + private static int getAlgaeScoreMethodSelector() { + if (OperatorConstants.SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean()) + return 1; + if (OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean()) + return 2; + return 0; + } + + private static boolean shouldReverseNetScore() { + final Rotation2d swerveAngle = RobotContainer.SWERVE.getDriveRelativeAngle(); + return swerveAngle.getDegrees() < 90 && swerveAngle.getDegrees() > -90; + } + + private static Command getInitiateFloorAlgaeCollectionCommand() { + return new ConditionalCommand( + getCollectAlgaeFromLollipopSequenceCommand(), + getCollectAlgaeFromFloorSequenceCommand(), + () -> SHOULD_COLLECT_FROM_LOLLIPOP + ).raceWith( + new WaitUntilChangeCommand<>(() -> SHOULD_COLLECT_FROM_LOLLIPOP) + ).repeatedly(); + } + + private static Command getCollectAlgaeFromLollipopSequenceCommand() { + return new ParallelCommandGroup( + new SequentialCommandGroup( + GeneralCommands.getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_LOLLIPOP, false), + EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.COLLECT_ALGAE) + ), + getIntakeCoralFromLollipopCommand() + .onlyIf(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE) + .until(() -> !OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE) + .repeatedly() + ); + } + + private static Command getCollectAlgaeFromFloorSequenceCommand() { + return new ParallelCommandGroup( + ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_FLOOR), + EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.COLLECT_ALGAE) + ); + } + + private static Command getIntakeCoralFromLollipopCommand() { + return GeneralCommands.getContinuousConditionalCommand( + IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.OPEN_REST), + CoralCollectionCommands.getCoralCollectionCommand(), + RobotContainer.TRANSPORTER::hasCoral + ).asProxy(); + } + + private static Command getInitiateReefAlgaeCollectionCommand() { + return new ParallelCommandGroup( + GeneralCommands.getFlippableOverridableArmCommand(OperatorConstants.REEF_CHOOSER::getArmElevatorAlgaeCollectionState, false, CoralPlacingCommands::shouldReverseScore), + EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.COLLECT_ALGAE) + ).raceWith( + new WaitUntilChangeCommand<>(OperatorConstants.REEF_CHOOSER::getArmElevatorAlgaeCollectionState) + ).repeatedly(); + } + + private static Command getAlgaeCollectionConfirmationCommand() { + return new InstantCommand(() -> OperatorConstants.DRIVER_CONTROLLER.rumble(OperatorConstants.RUMBLE_DURATION_SECONDS, OperatorConstants.RUMBLE_POWER)); + } + + private static FlippablePose2d calculateClosestNetScoringPose() { + final Translation2d robotTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); + if (Flippable.isRedAlliance() ? robotTranslation.getY() < FieldConstants.FLIPPABLE_NET_SCORE_POSE.get().getY() : robotTranslation.getY() > FieldConstants.FLIPPABLE_NET_SCORE_POSE.get().getY()) + return new FlippablePose2d(new Pose2d( + FieldConstants.FLIPPABLE_NET_SCORE_POSE.get().getX(), + robotTranslation.getY(), + shouldReverseNetScore() ? Rotation2d.k180deg : Rotation2d.kZero + ), false); + return FieldConstants.FLIPPABLE_NET_SCORE_POSE; + } + + private static FlippablePose2d calculateClosestAlgaeCollectionPose() { + final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + final Translation2d reefCenterPosition = new FlippableTranslation2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, true).get(); + final Rotation2d[] reefClockAngles = FieldConstants.REEF_CLOCK_ANGLES; + final Transform2d reefCenterToBranchScoringPose = new Transform2d(FieldConstants.REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS, 0, new Rotation2d()); + final List scoringPoses = new java.util.ArrayList<>(); + + for (Rotation2d reefClockAngle : reefClockAngles) { + final Pose2d reefCenterAtTargetRotation = new Pose2d(reefCenterPosition, reefClockAngle); + scoringPoses.add(reefCenterAtTargetRotation.transformBy(reefCenterToBranchScoringPose)); + } + + final Pose2d closestScoringPose = robotPose.nearest(scoringPoses); + return new FlippablePose2d(closestScoringPose.getTranslation(), closestScoringPose.getRotation().plus(CoralPlacingCommands.shouldReverseScore() ? Rotation2d.k180deg : new Rotation2d()).getRadians(), false); + } + + private static boolean isScoreAlgaeButtonPressed() { + return OperatorConstants.SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean() || + OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean(); + } + + private static double fieldRelativePowersToSelfRelativeXPower(double xPower, double yPower) { + final Rotation2d robotHeading = RobotContainer.SWERVE.getDriveRelativeAngle(); + final double xValue = CommandConstants.calculateDriveStickAxisValue(xPower); + final double yValue = CommandConstants.calculateDriveStickAxisValue(yPower); + + return (xValue * robotHeading.getCos()) + (yValue * robotHeading.getSin()); + } + + private static double calculateReefRelativeYOffset() { + final Pose2d target = calculateClosestAlgaeCollectionPose().get(); + final Pose2d robot = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); + return robot.relativeTo(target).getY(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java index d8a2f92a..1981cb19 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -21,7 +21,10 @@ public static Command getCoralCollectionCommand() { return new SequentialCommandGroup( getIntakeSequenceCommand(), new InstantCommand( - () -> getLoadCoralCommand().schedule() + () -> { + if (!AlgaeManipulationCommands.isHoldingAlgae()) + getLoadCoralCommand().schedule(); + } ) ); // new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE) @@ -34,11 +37,15 @@ public static Command getLoadCoralCommand() { ).until(RobotContainer.END_EFFECTOR::hasGamePiece).onlyWhile(() -> !RobotContainer.END_EFFECTOR.hasGamePiece()); } + public static Command getUnloadCoralCommand() { + return new ParallelCommandGroup( + ArmElevatorCommands.getSetTargetStateCommand(ArmElevatorConstants.ArmElevatorState.UNLOAD_CORAL), + EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.UNLOAD_CORAL) + ).until(() -> !RobotContainer.END_EFFECTOR.hasGamePiece() && RobotContainer.INTAKE.hasCoral()); + } + private static Command getIntakeSequenceCommand() { - return new SequentialCommandGroup( - getInitiateCollectionCommand().until(RobotContainer.INTAKE::hasCoral), - new InstantCommand(() -> getAlignCoralCommand().schedule()).alongWith(getCollectionConfirmationCommand()) - ).until(RobotContainer.INTAKE::hasCoral); + return getInitiateCollectionCommand().until(RobotContainer.TRANSPORTER::hasCoral); } private static Command getInitiateCollectionCommand() { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java index 583c52e5..496874a4 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -13,7 +13,6 @@ import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.misc.ReefChooser; -import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands; import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants; import frc.trigon.robot.subsystems.endeffector.EndEffectorCommands; import frc.trigon.robot.subsystems.endeffector.EndEffectorConstants; @@ -23,8 +22,7 @@ public class CoralPlacingCommands { public static boolean SHOULD_SCORE_AUTONOMOUSLY = true; - private static final ReefChooser REEF_CHOOSER = OperatorConstants.REEF_CHOOSER; - private static final double SAFE_DISTANCE_FROM_SCORING_POSE_METERS = 0.2; + static final ReefChooser REEF_CHOOSER = OperatorConstants.REEF_CHOOSER; public static Command getScoreInReefCommand(boolean shouldScoreRight) { return new SequentialCommandGroup( @@ -34,14 +32,14 @@ public static Command getScoreInReefCommand(boolean shouldScoreRight) { getScoreCommand(shouldScoreRight), () -> SHOULD_SCORE_AUTONOMOUSLY && REEF_CHOOSER.getScoringLevel() != ScoringLevel.L1 ) - ); + ).onlyIf(() -> RobotContainer.END_EFFECTOR.hasGamePiece() || RobotContainer.TRANSPORTER.hasCoral()); } private static Command getAutonomouslyScoreCommand(boolean shouldScoreRight) { return new SequentialCommandGroup( getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(() -> isReadyToScore(shouldScoreRight)), new ParallelCommandGroup( - ArmElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getArmElevatorState, CoralPlacingCommands::shouldReverseScore), + GeneralCommands.getFlippableOverridableArmCommand(REEF_CHOOSER::getArmElevatorState, false, CoralPlacingCommands::shouldReverseScore), EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_CORAL) ) ); @@ -51,7 +49,7 @@ private static Command getScoreCommand(boolean shouldScoreRight) { return new SequentialCommandGroup( getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(OperatorConstants.CONTINUE_TRIGGER), new ParallelCommandGroup( - ArmElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getArmElevatorState, CoralPlacingCommands::shouldReverseScore), + GeneralCommands.getFlippableOverridableArmCommand(REEF_CHOOSER::getArmElevatorState, false, CoralPlacingCommands::shouldReverseScore), EndEffectorCommands.getSetTargetStateCommand(EndEffectorConstants.EndEffectorState.SCORE_CORAL) ) ); @@ -59,7 +57,7 @@ private static Command getScoreCommand(boolean shouldScoreRight) { private static Command getAutonomouslyPrepareScoreCommand(boolean shouldScoreRight) { return new ParallelCommandGroup( - getOpenArmElevatorIfWontHitReef(shouldScoreRight), + getPrepareArmElevatorIfWontHitReef(shouldScoreRight), new SequentialCommandGroup( getAutonomousDriveToNoHitReefPose(shouldScoreRight).asProxy().onlyWhile(() -> !isPrepareArmAngleAboveCurrentArmAngle()), getAutonomousDriveToReef(shouldScoreRight).asProxy() @@ -81,10 +79,10 @@ private static Command getAutonomousDriveToNoHitReefPose(boolean shouldScoreRigh ); } - private static Command getOpenArmElevatorIfWontHitReef(boolean shouldScoreRight) { - return new GeneralCommands().runWhen( - ArmElevatorCommands.getPrepareForStateCommand(REEF_CHOOSER::getArmElevatorState, CoralPlacingCommands::shouldReverseScore), - () -> CoralPlacingCommands.isPrepareArmAngleAboveCurrentArmAngle() || calculateDistanceToTargetScoringPose(shouldScoreRight) > SAFE_DISTANCE_FROM_SCORING_POSE_METERS + private static Command getPrepareArmElevatorIfWontHitReef(boolean shouldScoreRight) { + return GeneralCommands.runWhen( + GeneralCommands.getFlippableOverridableArmCommand(REEF_CHOOSER::getArmElevatorState, true, CoralPlacingCommands::shouldReverseScore), + () -> CoralPlacingCommands.isPrepareArmAngleAboveCurrentArmAngle() || calculateDistanceToTargetScoringPose(shouldScoreRight) > FieldConstants.SAFE_DISTANCE_FROM_SCORING_POSE_METERS ); } @@ -165,9 +163,7 @@ private static boolean isReadyToScore(boolean shouldScoreRight) { && RobotContainer.SWERVE.atPose(calculateClosestScoringPose(shouldScoreRight)); } - private static boolean shouldReverseScore() { - if (REEF_CHOOSER.getScoringLevel() == ScoringLevel.L1) - return false; + static boolean shouldReverseScore() { final Rotation2d robotRotation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation(); final Translation2d robotTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); final Translation2d reefCenterTranslation = FieldConstants.FLIPPABLE_REEF_CENTER_TRANSLATION.get(); @@ -189,7 +185,9 @@ public enum ScoringLevel { L3(L2.xTransformMeters, L2.positiveYTransformMeters, Rotation2d.fromDegrees(0)), L4(L2.xTransformMeters, L2.positiveYTransformMeters, Rotation2d.fromDegrees(0)); - public final ArmElevatorConstants.ArmElevatorState armElevatorState; + public final ArmElevatorConstants.ArmElevatorState + armElevatorState, + armElevatorAlgaeCollectionState; public final int level = calculateLevel(); final double xTransformMeters, positiveYTransformMeters; final Rotation2d rotationTransform; @@ -208,6 +206,7 @@ public enum ScoringLevel { this.positiveYTransformMeters = positiveYTransformMeters; this.rotationTransform = rotationTransform; this.armElevatorState = determineArmElevatorState(); + this.armElevatorAlgaeCollectionState = determineArmElevatorAlgaeCollectionState(); } @@ -221,6 +220,14 @@ private ArmElevatorConstants.ArmElevatorState determineArmElevatorState() { }; } + private ArmElevatorConstants.ArmElevatorState determineArmElevatorAlgaeCollectionState() { + return switch (level) { + case 1, 2 -> ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_L2; + case 3, 4 -> ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_L3; + default -> throw new IllegalStateException("Unexpected value: " + ordinal()); + }; + } + private int calculateLevel() { return ordinal() + 1; } diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java index 5fbedf61..2eb31504 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/GeneralCommands.java @@ -1,12 +1,16 @@ package frc.trigon.robot.commands.commandfactories; +import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.commands.CommandConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.MotorSubsystem; +import frc.trigon.robot.subsystems.armelevator.ArmElevatorCommands; +import frc.trigon.robot.subsystems.armelevator.ArmElevatorConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; import java.util.function.BooleanSupplier; +import java.util.function.Supplier; /** * A class that contains the general commands of the robot, such as commands that alter a command or commands that affect all subsystems. @@ -52,6 +56,7 @@ public static Command runWhen(Command command, BooleanSupplier condition) { } /** + * ---- UNTESTED ----
* A command that only runs when a condition is met for a certain amount of time. * * @param command the command to run @@ -60,6 +65,32 @@ public static Command runWhen(Command command, BooleanSupplier condition) { * @return the command */ public static Command runWhen(Command command, BooleanSupplier condition, double debounceTimeSeconds) { - return runWhen(new WaitCommand(debounceTimeSeconds).andThen(command.onlyIf(condition)), condition); + final Debouncer debouncer = new Debouncer(0, Debouncer.DebounceType.kRising); + return new SequentialCommandGroup( + new InstantCommand(() -> debouncer.setDebounceTime(debounceTimeSeconds)), + runWhen(command, () -> debouncer.calculate(condition.getAsBoolean())) + ); + } + + public static Command getResetFlipArmOverrideCommand() { + return new InstantCommand(() -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE = false); + } + + public static Command getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState targetState, boolean isPrepareState, BooleanSupplier shouldStartFlipped) { + return isPrepareState ? + ArmElevatorCommands.getPrepareForStateCommand(() -> targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped.getAsBoolean()) : + ArmElevatorCommands.getSetTargetStateCommand(() -> targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped.getAsBoolean()); + } + + public static Command getFlippableOverridableArmCommand(Supplier targetState, boolean isPrepareState, BooleanSupplier shouldStartFlipped) { + return isPrepareState ? + ArmElevatorCommands.getPrepareForStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped.getAsBoolean()) : + ArmElevatorCommands.getSetTargetStateCommand(targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE ^ shouldStartFlipped.getAsBoolean()); + } + + public static Command getFlippableOverridableArmCommand(ArmElevatorConstants.ArmElevatorState targetState, boolean isPrepareState) { + return isPrepareState ? + ArmElevatorCommands.getPrepareForStateCommand(() -> targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE) : + ArmElevatorCommands.getSetTargetStateCommand(() -> targetState, () -> OperatorConstants.SHOULD_FLIP_ARM_OVERRIDE); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index 996d9ff2..424d8fd4 100644 --- a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -25,16 +25,15 @@ * A class that contains the constants and configurations for everything related to the 15-second autonomous period at the start of the match. */ public class AutonomousConstants { + public static final String DEFAULT_AUTO_NAME = "DefaultAutoName"; public static final RobotConfig ROBOT_CONFIG = getRobotConfig(); - public static final PathConstraints DRIVE_TO_REEF_CONSTRAINTS = new PathConstraints(2.5, 4, Units.degreesToRadians(450), Units.degreesToRadians(900)); public static final double FEEDFORWARD_SCALAR = 0.5;//TODO: Calibrate + public static final 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 double REEF_RELATIVE_X_TOLERANCE_METERS = 0.085, REEF_RELATIVE_Y_TOLERANCE_METERS = 0.03; - - public static final String DEFAULT_AUTO_NAME = "DefaultAutoName"; - + private static final double AUTO_FIND_CORAL_POSE_X = 3.3, AUTO_FIND_CORAL_POSE_LEFT_Y = 5.5; diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 1918a305..70f2ee0f 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -5,6 +5,7 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.*; +import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import lib.utilities.Conversions; import lib.utilities.FilesHandler; import lib.utilities.flippable.FlippablePose2d; @@ -22,6 +23,17 @@ public class FieldConstants { 1, 2, 3, 4, 5, 12, 13, 14, 15, 16 ); + public static final int REEF_CLOCK_POSITIONS = 6; + public static final Rotation2d REEF_CLOCK_POSITION_DIFFERENCE = Rotation2d.fromDegrees(Conversions.DEGREES_PER_ROTATIONS / REEF_CLOCK_POSITIONS); + public static final Rotation2d[] REEF_CLOCK_ANGLES = ReefClockPosition.getClockAngles(); + public static final Translation2d BLUE_REEF_CENTER_TRANSLATION = new Translation2d(4.48945, FIELD_WIDTH_METERS / 2); + public static final double + REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS = 1.3, + REEF_CENTER_TO_TARGET_SCORING_POSITION_Y_TRANSFORM_METERS = 0.17, + REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS = 1.6, + REEF_CENTER_TO_TARGET_NO_HIT_REEF_POSITION_X_TRANSFORM_METERS = REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS + 0.3; + public static final double SAFE_DISTANCE_FROM_SCORING_POSE_METERS = 2.2; + private static final boolean SHOULD_USE_HOME_TAG_LAYOUT = false; public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = createAprilTagFieldLayout(); private static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0)); @@ -36,18 +48,10 @@ public class FieldConstants { AUTO_FIND_CORAL_POSE_RIGHT = new FlippablePose2d(AUTO_FIND_CORAL_POSE_X, FieldConstants.FIELD_WIDTH_METERS - AUTO_FIND_CORAL_POSE_LEFT_Y, AUTO_FIND_CORAL_POSE_LEFT_ROTATION.unaryMinus(), true); public static final Rotation2d LEFT_FEEDER_ANGLE = Rotation2d.fromDegrees(54); - - public static final int REEF_CLOCK_POSITIONS = 6; - public static final Rotation2d REEF_CLOCK_POSITION_DIFFERENCE = Rotation2d.fromDegrees(Conversions.DEGREES_PER_ROTATIONS / REEF_CLOCK_POSITIONS); - public static final Rotation2d[] REEF_CLOCK_ANGLES = ReefClockPosition.getClockAngles(); - public static final Translation2d BLUE_REEF_CENTER_TRANSLATION = new Translation2d(4.48945, FIELD_WIDTH_METERS / 2); public static final FlippableTranslation2d FLIPPABLE_REEF_CENTER_TRANSLATION = new FlippableTranslation2d(BLUE_REEF_CENTER_TRANSLATION, true); - public static final double - REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS = 1.3, - REEF_CENTER_TO_TARGET_SCORING_POSITION_Y_TRANSFORM_METERS = 0.17, - REEF_CENTER_TO_TARGET_NO_HIT_REEF_POSITION_X_TRANSFORM_METERS = REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS + 0.3, - - REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS = 1.6; + public static final FlippablePose2d + FLIPPABLE_PROCESSOR_SCORE_POSE = new FlippablePose2d(SimulatedGamePieceConstants.PROCESSOR_LOCATION.get().toPose2d(), true), + FLIPPABLE_NET_SCORE_POSE = new FlippablePose2d(SimulatedGamePieceConstants.NET_MINIMUM_X_LOCATION.get().toPose2d(), true); private static AprilTagFieldLayout createAprilTagFieldLayout() { try { diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index d1303510..b9d2c68b 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -1,8 +1,10 @@ package frc.trigon.robot.constants; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand; +import frc.trigon.robot.commands.commandfactories.AlgaeManipulationCommands; import frc.trigon.robot.misc.ReefChooser; import lib.hardware.misc.KeyboardController; import lib.hardware.misc.XboxController; @@ -23,6 +25,10 @@ public class OperatorConstants { ); public static final KeyboardController OPERATOR_CONTROLLER = new KeyboardController(); public static final ReefChooser REEF_CHOOSER = new ReefChooser(REEF_CHOOSER_PORT); + public static boolean SHOULD_FLIP_ARM_OVERRIDE = false;//TODO: Implement everywhere + private static boolean + IS_LEFT_SCORE_BUTTON_PRESSED = false, + IS_RIGHT_SCORE_BUTTON_PRESSED = false; public static final double POV_DIVIDER = 2, @@ -33,26 +39,33 @@ public class OperatorConstants { public static final IntakeAssistCommand.AssistMode DEFAULT_INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.ALTERNATE_ASSIST; public static final Trigger - RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.rightStick(), + RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.povUp(), DRIVE_FROM_DPAD_TRIGGER = new Trigger(() -> DRIVER_CONTROLLER.getPov() != -1), TOGGLE_BRAKE_TRIGGER = OPERATOR_CONTROLLER.g().or(RobotController::getUserButton), DEBUGGING_TRIGGER = OPERATOR_CONTROLLER.f2(), - CONTINUE_TRIGGER = DRIVER_CONTROLLER.rightTrigger().or(OPERATOR_CONTROLLER.k()), + CONTINUE_TRIGGER = DRIVER_CONTROLLER.leftStick().and(DRIVER_CONTROLLER.rightStick()).or(OPERATOR_CONTROLLER.k()), FORWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.right(), BACKWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.left(), FORWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.up(), BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.down(); - public static final Trigger + FLOOR_ALGAE_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftBumper(),//TODO: Add operator control + REEF_ALGAE_COLLECTION_TRIGGER = DRIVER_CONTROLLER.rightBumper().or(OPERATOR_CONTROLLER.a()), + STOP_REEF_ALGAE_ALIGN_TRIGGER = DRIVER_CONTROLLER.povLeft(), + SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.n().or(createScoreTrigger(true, true)), + SCORE_ALGAE_IN_PROCESSOR_TRIGGER = OPERATOR_CONTROLLER.j().or(createScoreTrigger(false, true)), CORAL_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftTrigger().or(OPERATOR_CONTROLLER.c()), - SPAWN_CORAL_TRIGGER = OPERATOR_CONTROLLER.equals(), - SCORE_CORAL_LEFT_TRIGGER = DRIVER_CONTROLLER.leftBumper(), - SCORE_CORAL_RIGHT_TRIGGER = DRIVER_CONTROLLER.rightBumper(), - EJECT_CORAL_TRIGGER = DRIVER_CONTROLLER.back().or(OPERATOR_CONTROLLER.e()), + SCORE_CORAL_RIGHT_TRIGGER = createScoreTrigger(true, false), + SCORE_CORAL_LEFT_TRIGGER = createScoreTrigger(false, false), + EJECT_CORAL_TRIGGER = OPERATOR_CONTROLLER.e(); + public static final Trigger + SPAWN_CORAL_IN_SIMULATION_TRIGGER = OPERATOR_CONTROLLER.equals(), + FLIP_ARM_TRIGGER = DRIVER_CONTROLLER.start(), + LOLLIPOP_ALGAE_TOGGLE_TRIGGER = DRIVER_CONTROLLER.a(), CLIMB_TRIGGER = DRIVER_CONTROLLER.back().or(OPERATOR_CONTROLLER.c()); public static final Trigger - SET_TARGET_SCORING_REEF_LEVEL_L1_TRIGGER = OPERATOR_CONTROLLER.numpad0().or(DRIVER_CONTROLLER.a()), + SET_TARGET_SCORING_REEF_LEVEL_L1_TRIGGER = OPERATOR_CONTROLLER.numpad0().or(DRIVER_CONTROLLER.a().and(() -> !AlgaeManipulationCommands.isHoldingAlgae())), SET_TARGET_SCORING_REEF_LEVEL_L2_TRIGGER = OPERATOR_CONTROLLER.numpad1().or(DRIVER_CONTROLLER.b()), SET_TARGET_SCORING_REEF_LEVEL_L3_TRIGGER = OPERATOR_CONTROLLER.numpad2().or(DRIVER_CONTROLLER.x()), SET_TARGET_SCORING_REEF_LEVEL_L4_TRIGGER = OPERATOR_CONTROLLER.numpad3().or(DRIVER_CONTROLLER.y()), @@ -64,4 +77,23 @@ public class OperatorConstants { SET_TARGET_SCORING_REEF_CLOCK_POSITION_12_OCLOCK_TRIGGER = OPERATOR_CONTROLLER.numpad8(), SET_TARGET_REEF_SCORING_SIDE_LEFT_TRIGGER = OPERATOR_CONTROLLER.left(), SET_TARGET_REEF_SCORING_SIDE_RIGHT_TRIGGER = OPERATOR_CONTROLLER.right(); + + private static Trigger createScoreTrigger(boolean isRight, boolean isAlgaeCommand) { + final Trigger scoreTrigger; + + if (isRight) + scoreTrigger = DRIVER_CONTROLLER.rightStick() + .and(() -> !IS_LEFT_SCORE_BUTTON_PRESSED) + .onTrue(new InstantCommand(() -> IS_RIGHT_SCORE_BUTTON_PRESSED = true)) + .onFalse(new InstantCommand(() -> IS_RIGHT_SCORE_BUTTON_PRESSED = false)); + else + scoreTrigger = DRIVER_CONTROLLER.leftStick() + .and(() -> !IS_RIGHT_SCORE_BUTTON_PRESSED) + .onTrue(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = true)) + .onFalse(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = false)); + + if (isAlgaeCommand) + return scoreTrigger.and(AlgaeManipulationCommands::isHoldingAlgae); + return scoreTrigger.and(() -> !AlgaeManipulationCommands.isHoldingAlgae()); + } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/PathPlannerConstants.java b/src/main/java/frc/trigon/robot/constants/PathPlannerConstants.java deleted file mode 100644 index 8b93566e..00000000 --- a/src/main/java/frc/trigon/robot/constants/PathPlannerConstants.java +++ /dev/null @@ -1,84 +0,0 @@ -package frc.trigon.robot.constants; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.commands.PathfindingCommand; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; -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 frc.trigon.robot.RobotContainer; -import org.json.simple.parser.ParseException; -import lib.hardware.RobotHardwareStats; -import lib.utilities.LocalADStarAK; -import lib.utilities.flippable.Flippable; - -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 static final String DEFAULT_AUTO_NAME = "DefaultAutoName"; - public static final RobotConfig ROBOT_CONFIG = getRobotConfig(); - public static final double FEEDFORWARD_SCALAR = 0.5;//TODO: Calibrate - - private static final PIDConstants - AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? - new PIDConstants(0, 0, 0) : - new PIDConstants(0, 0, 0), - AUTO_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? - new PIDConstants(0, 0, 0) : - new PIDConstants(0, 0, 0); - - - public static final PIDController GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? - new PIDController(0.5, 0, 0) : - new PIDController(0.3, 0, 0.03); - public static final ProfiledPIDController GAME_PIECE_AUTO_DRIVE_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)); - public static final double AUTO_COLLECTION_INTAKE_OPEN_CHECK_DISTANCE_METERS = 2; - - private static final PPHolonomicDriveController AUTO_PATH_FOLLOWING_CONTROLLER = new PPHolonomicDriveController( - AUTO_TRANSLATION_PID_CONSTANTS, - AUTO_ROTATION_PID_CONSTANTS - ); - - /** - * Initializes PathPlanner. This needs to be called before any PathPlanner function can be used. - */ - public static void init() { - Pathfinding.setPathfinder(new LocalADStarAK()); - PathfindingCommand.warmupCommand().schedule(); - configureAutoBuilder(); - registerCommands(); - } - - private static void configureAutoBuilder() { - AutoBuilder.configure( - RobotContainer.ROBOT_POSE_ESTIMATOR::getEstimatedRobotPose, - RobotContainer.ROBOT_POSE_ESTIMATOR::resetPose, - RobotContainer.SWERVE::getSelfRelativeVelocity, - (chassisSpeeds -> RobotContainer.SWERVE.drivePathPlanner(chassisSpeeds, true)), - AUTO_PATH_FOLLOWING_CONTROLLER, - ROBOT_CONFIG, - Flippable::isRedAlliance, - RobotContainer.SWERVE - ); - } - - private static RobotConfig getRobotConfig() { - try { - return RobotConfig.fromGUISettings(); - } catch (IOException | ParseException e) { - throw new RuntimeException(e); - } - } - - private static void registerCommands() { - //TODO: Implement - } -} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/misc/ReefChooser.java b/src/main/java/frc/trigon/robot/misc/ReefChooser.java index aebe0f54..87d1d644 100644 --- a/src/main/java/frc/trigon/robot/misc/ReefChooser.java +++ b/src/main/java/frc/trigon/robot/misc/ReefChooser.java @@ -27,6 +27,10 @@ public ArmElevatorConstants.ArmElevatorState getArmElevatorState() { return scoringLevel.armElevatorState; } + public ArmElevatorConstants.ArmElevatorState getArmElevatorAlgaeCollectionState() { + return scoringLevel.armElevatorAlgaeCollectionState; + } + private void configureBindings() { configureScoringLevelBindings(); configureFallbackBindings(); diff --git a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java index 214357de..93a90793 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.util.Units; import frc.trigon.robot.constants.FieldConstants; +import lib.utilities.flippable.Flippable; import lib.utilities.flippable.FlippablePose3d; import lib.utilities.flippable.FlippableTranslation2d; @@ -34,8 +35,8 @@ public class SimulatedGamePieceConstants { createNewCoral(new Pose3d(FIELD_LENGTH_METERS - 1.22, FIELD_WIDTH_METERS / 2, 0.15, CORAL_TO_VERTICAL_POSITION_ROTATION)), createNewCoral(new Pose3d(FIELD_LENGTH_METERS - 1.22, FIELD_WIDTH_METERS / 2 - 1.83, 0.15, CORAL_TO_VERTICAL_POSITION_ROTATION)), createNewCoral(new Pose3d(FIELD_LENGTH_METERS - 1.22, FIELD_WIDTH_METERS / 2 + 1.83, 0.15, CORAL_TO_VERTICAL_POSITION_ROTATION)) - )), - ALGAE_ON_FIELD = new ArrayList<>(List.of( + )); + public static final ArrayList ALGAE_ON_FIELD = new ArrayList<>(List.of( createNewAlgae(new Pose3d(1.22, FIELD_WIDTH_METERS / 2, 0.5, new Rotation3d())), createNewAlgae(new Pose3d(1.22, FIELD_WIDTH_METERS / 2 - 1.83, 0.5, new Rotation3d())), createNewAlgae(new Pose3d(1.22, FIELD_WIDTH_METERS / 2 + 1.83, 0.5, new Rotation3d())), @@ -49,6 +50,10 @@ public class SimulatedGamePieceConstants { REEF_CENTER_TO_L2_VECTOR = new Translation3d(0.652, 0.1643, 0.6983), REEF_CENTER_TO_L3_VECTOR = new Translation3d(0.652, 0.1643, 1.1101), REEF_CENTER_TO_L4_VECTOR = new Translation3d(0.7796, 0.1643, 1.7345); + private static final double ALGAE_RADIUS_METERS = 0.2032; + private static final Translation3d + REEF_CENTER_TO_L2_ALGAE_VECTOR = new Translation3d(0.652, 0.1643, 0.70612 + ALGAE_RADIUS_METERS), + REEF_CENTER_TO_L3_ALGAE_VECTOR = new Translation3d(0.652, 0.1643, 1.01196 + ALGAE_RADIUS_METERS); private static final Rotation3d REEF_TO_2_OCLOCK_ROTATION = new Rotation3d(0, 0, Math.PI / 3), REEF_TO_4_OCLOCK_ROTATION = new Rotation3d(0, 0, Math.PI / 3 * 2), @@ -61,9 +66,13 @@ public class SimulatedGamePieceConstants { CORAL_TO_L2_AND_L3_ALIGNMENT = new Transform3d(new Translation3d(0, 0, 0), new Rotation3d(0, -Units.degreesToRadians(35), 0)), CORAL_TO_L4_ALIGNMENT = new Transform3d(new Translation3d(0, 0, 0), CORAL_TO_VERTICAL_POSITION_ROTATION); private static final Transform3d - RIGHT_BRANCH_TO_LEFT_BRANCH = new Transform3d(new Translation3d(0, -0.33, 0), new Rotation3d()); + RIGHT_BRANCH_TO_LEFT_BRANCH = new Transform3d(new Translation3d(0, -0.33, 0), new Rotation3d()), + RIGHT_BRANCH_TO_BETWEEN_BRANCHES = new Transform3d(new Translation3d(0, -0.165, 0), new Rotation3d()); public static final ArrayList CORAL_SCORING_LOCATIONS = calculatedCoralScoringLocations(); - public static final Pose3d PROCESSOR_LOCATION = new Pose3d(0, 0, 0, new Rotation3d()); + public static final 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); public static final FlippableTranslation2d LEFT_FEEDER_POSITION = new FlippableTranslation2d(0.923, 7.370, true), RIGHT_FEEDER_POSITION = new FlippableTranslation2d(0.923, 0.668, true); @@ -71,6 +80,10 @@ public class SimulatedGamePieceConstants { RIGHT_CORAL_SPAWN_POSE = new FlippablePose3d(new Pose3d(1.5, 1.5, 0, new Rotation3d()), true), LEFT_CORAL_SPAWN_POSE = new FlippablePose3d(new Pose3d(1.5, 6.5, 0, new Rotation3d()), true); + static { + ALGAE_ON_FIELD.addAll(createAlgaeOnReef()); + } + private static SimulatedGamePiece createNewCoral(Pose3d startingPose) { return new SimulatedGamePiece(startingPose, GamePieceType.CORAL); } @@ -90,9 +103,26 @@ private static ArrayList calculatedCoralScoringLocations() { return coralScoringPoses; } + private static ArrayList createAlgaeOnReef() { + final ArrayList algaeStartingPoses = new ArrayList<>(); + for (int clockFace = 2; clockFace <= 12; clockFace += 2) { + final Pose3d pose = calculateAlgaeStartingPose(clockFace); + algaeStartingPoses.add(new SimulatedGamePiece(pose, GamePieceType.ALGAE)); + algaeStartingPoses.add(new SimulatedGamePiece(FlippablePose3d.flipPose(pose), GamePieceType.ALGAE)); + } + return algaeStartingPoses; + } + + private static Pose3d calculateAlgaeStartingPose(int clockFace) { + final int level = clockFace % 4 == 0 ? 2 : 3; + final Translation3d reefCenterToLevelVector = level == 2 ? REEF_CENTER_TO_L2_ALGAE_VECTOR : REEF_CENTER_TO_L3_ALGAE_VECTOR; + final Rotation3d reefToClockFaceRotation = getReefClockFaceRotation(clockFace); + return new Pose3d(new Pose2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, new Rotation2d())).transformBy(new Transform3d(reefCenterToLevelVector.rotateBy(reefToClockFaceRotation), reefToClockFaceRotation)).transformBy(RIGHT_BRANCH_TO_BETWEEN_BRANCHES); + } + private static FlippablePose3d calculateCoralScorePose(int level, int clockFace, boolean isLeftBranch) { + final Rotation3d reefToClockFaceRotation = getReefClockFaceRotation(clockFace); final Translation3d reefCenterToLevelVector; - final Rotation3d reefToClockFaceRotation; final Transform3d coralAlignment; switch (level) { case 1 -> reefCenterToLevelVector = REEF_CENTER_TO_L1_VECTOR; @@ -101,15 +131,6 @@ private static FlippablePose3d calculateCoralScorePose(int level, int clockFace, case 4 -> reefCenterToLevelVector = REEF_CENTER_TO_L4_VECTOR; default -> reefCenterToLevelVector = new Translation3d(); } - switch (clockFace) { - case 2 -> reefToClockFaceRotation = REEF_TO_2_OCLOCK_ROTATION; - case 4 -> reefToClockFaceRotation = REEF_TO_4_OCLOCK_ROTATION; - case 6 -> reefToClockFaceRotation = REEF_TO_6_OCLOCK_ROTATION; - case 8 -> reefToClockFaceRotation = REEF_TO_8_OCLOCK_ROTATION; - case 10 -> reefToClockFaceRotation = REEF_TO_10_OCLOCK_ROTATION; - case 12 -> reefToClockFaceRotation = REEF_TO_12_OCLOCK_ROTATION; - default -> reefToClockFaceRotation = new Rotation3d(); - } switch (level) { case 1 -> coralAlignment = CORAL_TO_L1_ALIGNMENT; case 2, 3 -> coralAlignment = CORAL_TO_L2_AND_L3_ALIGNMENT; @@ -119,6 +140,18 @@ private static FlippablePose3d calculateCoralScorePose(int level, int clockFace, return new FlippablePose3d(new Pose3d(new Pose2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, new Rotation2d())).transformBy(new Transform3d(reefCenterToLevelVector.rotateBy(reefToClockFaceRotation), reefToClockFaceRotation)).transformBy(isLeftBranch ? RIGHT_BRANCH_TO_LEFT_BRANCH : new Transform3d()).transformBy(coralAlignment), true); } + private static Rotation3d getReefClockFaceRotation(int clockFace) { + return switch (clockFace) { + case 2 -> REEF_TO_2_OCLOCK_ROTATION; + case 4 -> REEF_TO_4_OCLOCK_ROTATION; + case 6 -> REEF_TO_6_OCLOCK_ROTATION; + case 8 -> REEF_TO_8_OCLOCK_ROTATION; + case 10 -> REEF_TO_10_OCLOCK_ROTATION; + case 12 -> REEF_TO_12_OCLOCK_ROTATION; + default -> new Rotation3d(); + }; + } + public enum GamePieceType { ALGAE(0.2, 0), CORAL(0.06, 1); diff --git a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java index a07b61b6..103fad9d 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -1,5 +1,6 @@ package frc.trigon.robot.misc.simulatedfield; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform3d; import frc.trigon.robot.RobotContainer; @@ -89,10 +90,10 @@ private static void updateCollection() { HELD_ALGAE_INDEX = getIndexOfCollectedGamePiece(algaeCollectionPose, ALGAE_ON_FIELD, SimulatedGamePieceConstants.ALGAE_INTAKE_TOLERANCE_METERS); } - public static void updateCoralSpawning(Pose3d robotPose) { + public static void updateCoralSpawning(Pose2d robotPose) { final double - distanceFromLeftFeeder = robotPose.toPose2d().getTranslation().getDistance(SimulatedGamePieceConstants.LEFT_FEEDER_POSITION.get()), - distanceFromRightFeeder = robotPose.toPose2d().getTranslation().getDistance(SimulatedGamePieceConstants.RIGHT_FEEDER_POSITION.get()); + distanceFromLeftFeeder = robotPose.getTranslation().getDistance(SimulatedGamePieceConstants.LEFT_FEEDER_POSITION.get()), + distanceFromRightFeeder = robotPose.getTranslation().getDistance(SimulatedGamePieceConstants.RIGHT_FEEDER_POSITION.get()); final FlippablePose3d coralSpawnPose = distanceFromLeftFeeder < distanceFromRightFeeder ? SimulatedGamePieceConstants.LEFT_CORAL_SPAWN_POSE : SimulatedGamePieceConstants.RIGHT_CORAL_SPAWN_POSE; @@ -124,7 +125,14 @@ private static boolean isCollectingCoral() { } private static boolean isCollectingAlgae() { - return RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_L2) || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_L3); + return RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_L2) + || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_L2, true) + || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_L3) + || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_L3, true) + || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_FLOOR) + || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_FLOOR, true) + || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_LOLLIPOP) + || RobotContainer.ARM_ELEVATOR.atState(ArmElevatorConstants.ArmElevatorState.COLLECT_ALGAE_LOLLIPOP, true); } private static boolean isCoralLoading() { @@ -135,6 +143,7 @@ private static void updateEjection() { if (HELD_CORAL_INDEX != null) updateCoralEjection(); + if (HELD_ALGAE_INDEX != null && RobotContainer.END_EFFECTOR.isEjecting()) { final SimulatedGamePiece heldAlgae = ALGAE_ON_FIELD.get(HELD_ALGAE_INDEX); heldAlgae.release(RobotContainer.END_EFFECTOR.calculateLinearEndEffectorVelocity(), RobotContainer.SWERVE.getFieldRelativeVelocity3d()); diff --git a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationScoringHandler.java b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationScoringHandler.java index f4891f11..adb39e15 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationScoringHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationScoringHandler.java @@ -27,7 +27,7 @@ private static void checkCoralScored(SimulatedGamePiece coral) { } private static void checkAlgaeScored(SimulatedGamePiece algae) { - if (!isGamePieceScored(algae, SimulatedGamePieceConstants.PROCESSOR_LOCATION, SimulatedGamePieceConstants.ALGAE_SCORING_TOLERANCE_METERS)) + if (!isGamePieceScored(algae, SimulatedGamePieceConstants.PROCESSOR_LOCATION.get(), SimulatedGamePieceConstants.ALGAE_SCORING_TOLERANCE_METERS)) return; algae.isScored = true; diff --git a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java index 8a9efe3d..b91cd417 100644 --- a/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/armelevator/ArmElevatorConstants.java @@ -336,17 +336,18 @@ public enum ArmElevatorState { REST_WITH_ALGAE(Rotation2d.fromDegrees(90), 0.603, null, false, 0.7), REST_FOR_CLIMB(Rotation2d.fromDegrees(180), 0.603, null, false, 0.7), LOAD_CORAL(Rotation2d.fromDegrees(0), 0.5519, REST, true, 0.7), + UNLOAD_CORAL(Rotation2d.fromDegrees(0), 0.5519, null, false, 0.7), EJECT(Rotation2d.fromDegrees(60), 0.603, null, false, 0.7), SCORE_L1(Rotation2d.fromDegrees(70), 0.4, null, false, 1), SCORE_L2(Rotation2d.fromDegrees(90), 0.3, PREPARE_SCORE_L2, false, 1), SCORE_L3(Rotation2d.fromDegrees(90), 0.7, PREPARE_SCORE_L3, false, 1), SCORE_L4(Rotation2d.fromDegrees(100), 1.2, PREPARE_SCORE_L4, false, 1), SCORE_NET(Rotation2d.fromDegrees(160), 1.382, null, false, 0.3), - SCORE_PROCESSOR(Rotation2d.fromDegrees(90), 0.603, null, false, 0.7), + SCORE_PROCESSOR(Rotation2d.fromDegrees(90), 0.1, null, false, 0.7), COLLECT_ALGAE_L2(Rotation2d.fromDegrees(90), 0.603, null, false, 1), COLLECT_ALGAE_L3(Rotation2d.fromDegrees(90), 0.953, null, false, 1), - PREPARE_COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(60), 0.2, null, false, 1), - COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(50), 0.2, PREPARE_COLLECT_ALGAE_FLOOR, true, 1); + COLLECT_ALGAE_LOLLIPOP(Rotation2d.fromDegrees(70), 0.3, null, false, 1), + COLLECT_ALGAE_FLOOR(Rotation2d.fromDegrees(70), 0.1, null, true, 1); public final Rotation2d targetAngle; public final double targetPositionMeters; diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java index 1b669e41..8d4ecfe5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffector.java @@ -50,6 +50,7 @@ public Translation3d calculateLinearEndEffectorVelocity() { ); } + @AutoLogOutput(key = "EndEffector/IsEjecting") public boolean isEjecting() { return endEffectorMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE) > 2; } diff --git a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java index e0c3afbe..d9634fe5 100644 --- a/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/endeffector/EndEffectorConstants.java @@ -87,6 +87,7 @@ public enum EndEffectorState { REST(0), EJECT(4), LOAD_CORAL(-4), + UNLOAD_CORAL(4), SCORE_CORAL(4), COLLECT_ALGAE(-4), SCORE_ALGAE(4), diff --git a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java index b7d1209d..ab672f55 100644 --- a/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/intake/IntakeConstants.java @@ -218,6 +218,7 @@ private static void configureDistanceSensor() { public enum IntakeState { REST(0, MINIMUM_ANGLE), + OPEN_REST(0, MAXIMUM_ANGLE), REST_FOR_CLIMB(0, MAXIMUM_ANGLE), COLLECT(5, MAXIMUM_ANGLE), EJECT(-5, MAXIMUM_ANGLE); 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 f0b3496c..84aff59b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -14,7 +14,7 @@ import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.constants.PathPlannerConstants; +import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants; import frc.trigon.robot.subsystems.MotorSubsystem; import frc.trigon.robot.subsystems.swerve.swervemodule.SwerveModule; @@ -33,10 +33,10 @@ public class Swerve extends MotorSubsystem { private final Pigeon2Gyro gyro = SwerveConstants.GYRO; private final SwerveModule[] swerveModules = SwerveConstants.SWERVE_MODULES; private final Phoenix6SignalThread phoenix6SignalThread = Phoenix6SignalThread.getInstance(); - private final SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator(PathPlannerConstants.ROBOT_CONFIG, SwerveModuleConstants.MAXIMUM_MODULE_ROTATIONAL_SPEED_RADIANS_PER_SECOND); + private final SwerveSetpointGenerator setpointGenerator = new SwerveSetpointGenerator(AutonomousConstants.ROBOT_CONFIG, SwerveModuleConstants.MAXIMUM_MODULE_ROTATIONAL_SPEED_RADIANS_PER_SECOND); public Pose2d targetPathPlannerPose = new Pose2d(); public boolean isPathPlannerDriving = false; - private SwerveSetpoint previousSetpoint = new SwerveSetpoint(getSelfRelativeVelocity(), getModuleStates(), DriveFeedforwards.zeros(PathPlannerConstants.ROBOT_CONFIG.numModules)); + private SwerveSetpoint previousSetpoint = new SwerveSetpoint(getSelfRelativeVelocity(), getModuleStates(), DriveFeedforwards.zeros(AutonomousConstants.ROBOT_CONFIG.numModules)); public Swerve() { setName("Swerve"); @@ -166,7 +166,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); } @@ -200,7 +200,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; diff --git a/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java b/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java index 1c02c7c7..6dcf7d90 100644 --- a/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/transporter/TransporterConstants.java @@ -9,6 +9,7 @@ import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.event.BooleanEvent; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; import lib.hardware.misc.simplesensor.SimpleSensor; import lib.hardware.phoenix6.talonfx.TalonFXMotor; import lib.hardware.phoenix6.talonfx.TalonFXSignal; @@ -47,7 +48,7 @@ public class TransporterConstants { GEAR_RATIO, MOMENT_OF_INERTIA ); - private static final DoubleSupplier BEAM_BREAK_SIMULATION_SUPPLIER = () -> 0; //TODO: implement + private static final DoubleSupplier BEAM_BREAK_SIMULATION_SUPPLIER = () -> SimulationFieldHandler.isHoldingCoral() && !SimulationFieldHandler.isCoralInEndEffector() ? 1 : 0; private static final double MAXIMUM_DISPLAYABLE_VELOCITY = 12; static final SpeedMechanism2d diff --git a/src/main/java/lib/utilities/flippable/FlippablePose3d.java b/src/main/java/lib/utilities/flippable/FlippablePose3d.java index 0ac5b46c..6b8ac37f 100644 --- a/src/main/java/lib/utilities/flippable/FlippablePose3d.java +++ b/src/main/java/lib/utilities/flippable/FlippablePose3d.java @@ -58,4 +58,18 @@ protected Pose3d flip(Pose3d pose) { ) ); } + + public static Pose3d flipPose(Pose3d pose) { + final Pose2d flippedPose2d = FlippingUtil.flipFieldPose(pose.toPose2d()); + return new Pose3d( + flippedPose2d.getTranslation().getX(), + flippedPose2d.getTranslation().getY(), + pose.getZ(), + new Rotation3d( + pose.getRotation().getX(), + pose.getRotation().getY(), + flippedPose2d.getRotation().getRadians() + ) + ); + } } \ No newline at end of file