diff --git a/build.gradle b/build.gradle index 0273d82c..c233d28c 100644 --- a/build.gradle +++ b/build.gradle @@ -107,7 +107,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() implementation 'com.google.code.gson:gson:2.10.1' - implementation 'com.github.Programming-TRIGON:TRIGONLib:v2025.3.0' + implementation 'com.github.Programming-TRIGON:TRIGONLib:v2025.3.1' def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" diff --git a/src/main/java/frc/trigon/robot/Robot.java b/src/main/java/frc/trigon/robot/Robot.java index fc72e30d..4bc59416 100644 --- a/src/main/java/frc/trigon/robot/Robot.java +++ b/src/main/java/frc/trigon/robot/Robot.java @@ -65,7 +65,7 @@ public void testInit() { @Override public void simulationPeriodic() { - if (RobotHardwareStats.isSimulation()) + if (RobotHardwareStats.isReplay()) AprilTagCameraConstants.VISION_SIMULATION.update(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedOdometryPose()); SimulationFieldHandler.update(); } diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 260616f5..e7057038 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -13,14 +13,12 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.commands.CommandConstants; -import frc.trigon.robot.commands.commandclasses.CoralAlignmentCommand; -import frc.trigon.robot.commands.commandclasses.CoralAutoDriveCommand; import frc.trigon.robot.commands.commandclasses.LEDAutoSetupCommand; import frc.trigon.robot.commands.commandfactories.*; +import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.constants.CameraConstants; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.constants.OperatorConstants; -import frc.trigon.robot.constants.PathPlannerConstants; import frc.trigon.robot.misc.objectdetectioncamera.ObjectPoseEstimator; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.poseestimation.poseestimator.RobotPoseEstimator; @@ -29,7 +27,6 @@ import frc.trigon.robot.subsystems.algaemanipulator.AlgaeManipulatorCommands; import frc.trigon.robot.subsystems.coralintake.CoralIntake; import frc.trigon.robot.subsystems.coralintake.CoralIntakeCommands; -import frc.trigon.robot.subsystems.coralintake.CoralIntakeConstants; import frc.trigon.robot.subsystems.elevator.Elevator; import frc.trigon.robot.subsystems.elevator.ElevatorCommands; import frc.trigon.robot.subsystems.elevator.ElevatorConstants; @@ -83,7 +80,7 @@ public Command getAutonomousCommand() { */ private void initializeGeneralSystems() { Flippable.init(); - PathPlannerConstants.init(); + AutonomousConstants.init(); } private void configureBindings() { @@ -95,7 +92,7 @@ private void configureBindings() { private void bindDefaultCommands() { SWERVE.setDefaultCommand(GeneralCommands.getFieldRelativeDriveCommand()); ALGAE_MANIPULATOR.setDefaultCommand(AlgaeManipulatorCommands.getDefaultCommand()); - CORAL_INTAKE.setDefaultCommand(CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.REST)); + CORAL_INTAKE.setDefaultCommand(CoralIntakeCommands.getCoralIntakeDefaultCommand()); ELEVATOR.setDefaultCommand(ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST)); GRIPPER.setDefaultCommand(GripperCommands.getGripperDefaultCommand()); } @@ -103,30 +100,28 @@ private void bindDefaultCommands() { private void bindControllerCommands() { OperatorConstants.LED_AUTO_SETUP_TRIGGER.onTrue(new LEDAutoSetupCommand(() -> autoChooser.get().getName())); OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND); -// OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); -// OperatorConstants.TOGGLE_ROTATION_MODE_TRIGGER.onTrue(GeneralCommands.getToggleRotationModeCommand()); OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); + OperatorConstants.DEBUGGING_TRIGGER.whileTrue(CommandConstants.WHEEL_RADIUS_CHARACTERIZATION_COMMAND); + OperatorConstants.RESET_AMP_ALIGNER_TRIGGER.whileTrue(AlgaeManipulationCommands.getResetAmpAlignerCommand()); - OperatorConstants.FLOOR_CORAL_COLLECTION_TRIGGER.and(OperatorConstants.LEFT_MULTIFUNCTION_TRIGGER).whileTrue(new CoralAutoDriveCommand()); - OperatorConstants.OPERATOR_CONTROLLER.f3().whileTrue(new CoralAlignmentCommand()); OperatorConstants.FLOOR_CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getFloorCoralCollectionCommand()); - OperatorConstants.FEEDER_CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getFeederCoralCollectionCommand()); - OperatorConstants.SCORE_CORAL_IN_REEF_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand()); - - OperatorConstants.DEBUGGING_TRIGGER.whileTrue(CommandConstants.WHEEL_RADIUS_CHARACTERIZATION_COMMAND); + OperatorConstants.FEEDER_CORAL_COLLECTION_TRIGGER.whileTrue(CoralCollectionCommands.getFeederCoralCollectionFromIntakeCommand()); + OperatorConstants.RIGHT_SCORE_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(true)); + OperatorConstants.LEFT_SCORE_TRIGGER.whileTrue(CoralPlacingCommands.getScoreInReefCommand(false)); OperatorConstants.EJECT_CORAL_TRIGGER.whileTrue(EjectionCommands.getEjectCoralCommand()); OperatorConstants.UNLOAD_CORAL_TRIGGER.whileTrue(CoralCollectionCommands.getUnloadCoralCommand()); + OperatorConstants.COLLECT_ALGAE_FROM_REEF_TRIGGER.toggleOnTrue(AlgaeManipulationCommands.getCollectAlgaeFromReefCommand()); OperatorConstants.COLLECT_ALGAE_FROM_LOLLIPOP_TRIGGER.toggleOnTrue(AlgaeManipulationCommands.getCollectAlgaeFromLollipopCommand()); - OperatorConstants.FEEDER_CORAL_COLLECTION_WITH_GRIPPER.whileTrue(CoralCollectionCommands.getFeederCoralCollectionFromGripperCommand()); } private void bindSetters() { - OperatorConstants.ENABLE_IGNORE_LOLLIPOP_CORAL_TRIGGER.onTrue(CommandConstants.ENABLE_IGNORE_LOLLIPOP_CORAL_COMMAND); - OperatorConstants.DISABLE_IGNORE_LOLLIPOP_CORAL_TRIGGER.onTrue(CommandConstants.DISABLE_IGNORE_LOLLIPOP_CORAL_COMMAND); + OperatorConstants.ENABLE_INTAKE_ASSIST_TRIGGER.onTrue(CommandConstants.ENABLE_INTAKE_ASSIST_COMMAND); + OperatorConstants.DISABLE_INTAKE_ASSIST_TRIGGER.onTrue(CommandConstants.DISABLE_INTAKE_ASSIST_COMMAND); OperatorConstants.ENABLE_AUTONOMOUS_REEF_SCORING_TRIGGER.onTrue(CommandConstants.ENABLE_AUTONOMOUS_REEF_SCORING_COMMAND); OperatorConstants.DISABLE_AUTONOMOUS_REEF_SCORING_TRIGGER.onTrue(CommandConstants.DISABLE_AUTONOMOUS_REEF_SCORING_COMMAND); + OperatorConstants.TOGGLE_SHOULD_KEEP_INTAKE_OPEN_TRIGGER.onTrue(CommandConstants.TOGGLE_SHOULD_KEEP_INTAKE_OPEN_COMMAND); } private void configureSysIdBindings(MotorSubsystem subsystem) { @@ -148,11 +143,11 @@ private void buildAutoChooser() { final PathPlannerAuto autoNonMirrored = new PathPlannerAuto(autoName); final PathPlannerAuto autoMirrored = new PathPlannerAuto(autoName, true); - if (!PathPlannerConstants.DEFAULT_AUTO_NAME.isEmpty() && PathPlannerConstants.DEFAULT_AUTO_NAME.equals(autoName)) { + 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 (!PathPlannerConstants.DEFAULT_AUTO_NAME.isEmpty() && PathPlannerConstants.DEFAULT_AUTO_NAME.equals(autoName + "Mirrored")) { + } 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); diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index 29f7e5af..d43c052f 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -9,9 +9,9 @@ import frc.trigon.robot.commands.commandfactories.CoralCollectionCommands; import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands; import frc.trigon.robot.commands.commandfactories.GeneralCommands; +import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.constants.OperatorConstants; -import frc.trigon.robot.constants.PathPlannerConstants; import frc.trigon.robot.subsystems.swerve.SwerveCommands; import org.trigon.commands.CameraPositionCalculationCommand; import org.trigon.commands.WheelRadiusCharacterizationCommand; @@ -27,14 +27,14 @@ public class CommandConstants { private static final XboxController DRIVER_CONTROLLER = OperatorConstants.DRIVER_CONTROLLER; private static final double MINIMUM_TRANSLATION_SHIFT_POWER = 0.30, - MINIMUM_ROTATION_SHIFT_POWER = 0.4; + MINIMUM_ROTATION_SHIFT_POWER = 0.4;//TODO: Calibrate with real robot private static final double JOYSTICK_ORIENTED_ROTATION_DEADBAND = 0.07; public static final Command - FIELD_RELATIVE_DRIVE_WITH_JOYSTICK_ORIENTED_ROTATION_TO_REEF_SECTIONS_COMMAND = SwerveCommands.getClosedLoopFieldRelativeDriveCommand( + FIELD_RELATIVE_DRIVE_WITH_JOYSTICK_ORIENTED_ROTATION_COMMAND = SwerveCommands.getClosedLoopFieldRelativeDriveCommand( () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftY()), () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftX()), - CommandConstants::calculateJoystickOrientedToReefSectionsTargetAngle + CommandConstants::calculateTargetHeadingFromJoystickAngle ), SELF_RELATIVE_DRIVE_COMMAND = SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftY()), @@ -42,14 +42,13 @@ public class CommandConstants { () -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX()) ), RESET_HEADING_COMMAND = new InstantCommand(RobotContainer.ROBOT_POSE_ESTIMATOR::resetHeading), - // RESET_HEADING_COMMAND = new InstantCommand(() -> RobotContainer.ROBOT_POSE_ESTIMATOR.resetPose(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose())), - SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND = SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> getXPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), - () -> getYPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), - () -> 0 - ), + SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND = SwerveCommands.getClosedLoopSelfRelativeDriveCommand( + () -> getXPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), + () -> getYPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), + () -> 0 + ), 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), @@ -65,8 +64,9 @@ public class CommandConstants { public static final Command ENABLE_AUTONOMOUS_REEF_SCORING_COMMAND = new InstantCommand(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY = true).ignoringDisable(true), DISABLE_AUTONOMOUS_REEF_SCORING_COMMAND = new InstantCommand(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY = false).ignoringDisable(true), - ENABLE_IGNORE_LOLLIPOP_CORAL_COMMAND = new InstantCommand(() -> CoralCollectionCommands.SHOULD_IGNORE_LOLLIPOP_CORAL = true).ignoringDisable(true), - DISABLE_IGNORE_LOLLIPOP_CORAL_COMMAND = new InstantCommand(() -> CoralCollectionCommands.SHOULD_IGNORE_LOLLIPOP_CORAL = false).ignoringDisable(true); + ENABLE_INTAKE_ASSIST_COMMAND = new InstantCommand(() -> CoralCollectionCommands.SHOULD_ASSIST_INTAKE = true).ignoringDisable(true), + DISABLE_INTAKE_ASSIST_COMMAND = new InstantCommand(() -> CoralCollectionCommands.SHOULD_ASSIST_INTAKE = false).ignoringDisable(true), + TOGGLE_SHOULD_KEEP_INTAKE_OPEN_COMMAND = new InstantCommand(() -> CoralCollectionCommands.SHOULD_KEEP_INTAKE_OPEN = !CoralCollectionCommands.SHOULD_KEEP_INTAKE_OPEN); /** * Calculates the target drive power from an axis value by dividing it by the shift mode value. @@ -106,9 +106,9 @@ public static double calculateShiftModeValue(double minimumPower) { * Calculates the target rotation value from the joystick's angle. Used for joystick oriented rotation. * Joystick oriented rotation is when the robot rotates directly to the joystick's angle. * - * @return the rotation value + * @return the target heading */ - public static FlippableRotation2d calculateJoystickOrientedToReefSectionsTargetAngle() { + public static FlippableRotation2d calculateTargetHeadingFromJoystickAngle() { final double xPower = DRIVER_CONTROLLER.getRightX(), yPower = DRIVER_CONTROLLER.getRightY(); diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/CoralAlignmentCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/CoralAlignmentCommand.java deleted file mode 100644 index a6d21e7a..00000000 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/CoralAlignmentCommand.java +++ /dev/null @@ -1,40 +0,0 @@ -package frc.trigon.robot.commands.commandclasses; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.constants.CameraConstants; -import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; -import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; -import frc.trigon.robot.subsystems.coralintake.CoralIntakeCommands; -import frc.trigon.robot.subsystems.coralintake.CoralIntakeConstants; -import frc.trigon.robot.subsystems.swerve.SwerveCommands; -import org.trigon.utilities.flippable.FlippableRotation2d; - -public class CoralAlignmentCommand extends ParallelCommandGroup { - private static final ObjectDetectionCamera CAMERA = CameraConstants.OBJECT_DETECTION_CAMERA; - - public CoralAlignmentCommand() { - addCommands( - getDriveWhileAligningToNoteCommand(), - CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.COLLECT_FROM_FLOOR) - ); - } - - private Command getDriveWhileAligningToNoteCommand() { - return SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> 0, - () -> 0, - () -> { - var a = CAMERA.getTargetObjectsRotations(SimulatedGamePieceConstants.GamePieceType.CORAL); - if (a.length == 0) - return null; - return new FlippableRotation2d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation().plus(a[0].toRotation2d()), false); - } - ); - } - - private boolean shouldAlignToNote() { - return CAMERA.hasTargets(SimulatedGamePieceConstants.GamePieceType.CORAL) && !RobotContainer.GRIPPER.hasGamePiece(); - } -} \ 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 index c0ffb331..d8983063 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/CoralAutoDriveCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/CoralAutoDriveCommand.java @@ -66,9 +66,9 @@ public static Command getDriveToCoralCommand(Supplier distanceFro // () -> Y_PID_CONTROLLER.calculate(distanceFromTrackedCoral.get().getY()), // CoralAutoDriveCommand::calculateTargetAngle // ).until(() -> shouldMoveTowardsCoral(distanceFromTrackedCoral.get())), - new InstantCommand(() -> X_PID_CONTROLLER.reset(distanceFromTrackedCoral.get().getX())), + new InstantCommand(() -> X_PID_CONTROLLER.reset(distanceFromTrackedCoral.get().getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().vxMetersPerSecond)), SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> X_PID_CONTROLLER.calculate(distanceFromTrackedCoral.get().getX(), -0.65), + () -> X_PID_CONTROLLER.calculate(distanceFromTrackedCoral.get().getX(), -0.76), () -> Y_PID_CONTROLLER.calculate(distanceFromTrackedCoral.get().getY()), CoralAutoDriveCommand::calculateTargetAngle ) diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java new file mode 100644 index 00000000..e96b2523 --- /dev/null +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -0,0 +1,190 @@ +package frc.trigon.robot.commands.commandclasses; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ProfiledPIDController; +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.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.*; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.commandfactories.GeneralCommands; +import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.subsystems.swerve.SwerveCommands; +import org.littletonrobotics.junction.Logger; +import org.trigon.hardware.RobotHardwareStats; + +import java.util.function.Supplier; + +/** + * A command class to assist in the intaking of a game piece. + */ +public class IntakeAssistCommand extends ParallelCommandGroup { + private 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)), + Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? + new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : + new ProfiledPIDController(0.3, 0, 0.03, new TrapezoidProfile.Constraints(2.65, 5.5)), + THETA_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? + new ProfiledPIDController(0.4, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : + new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)); + private Translation2d distanceFromTrackedCoral; + + /** + * Creates a new intake assist command of the given assist type. + * + * @param assistMode the type of assistance + */ + public IntakeAssistCommand(AssistMode assistMode) { + addCommands( + getTrackCoralCommand(), + GeneralCommands.getContinuousConditionalCommand( + GeneralCommands.getFieldRelativeDriveCommand(), + getAssistIntakeCommand(assistMode, () -> distanceFromTrackedCoral), + () -> RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() == null || distanceFromTrackedCoral == null + ) + ); + } + + /** + * Returns a command that assists the intake of a game piece at the given location. + * This command is for intaking a game piece with a specific robot-relative position. + * To create an intake assist command that selects the closest game piece automatically, use {@link IntakeAssistCommand#IntakeAssistCommand(AssistMode)} instead. + * + * @param assistMode the type of assistance + * @param distanceFromTrackedCoral the position of the game piece relative to the robot + * @return the command + */ + public static Command getAssistIntakeCommand(AssistMode assistMode, Supplier distanceFromTrackedCoral) { + return new SequentialCommandGroup( + new InstantCommand(() -> resetPIDControllers(distanceFromTrackedCoral.get())), + SwerveCommands.getClosedLoopSelfRelativeDriveCommand( + () -> calculateTranslationPower(assistMode, distanceFromTrackedCoral.get()).getX(), + () -> calculateTranslationPower(assistMode, distanceFromTrackedCoral.get()).getY(), + () -> calculateThetaPower(assistMode, distanceFromTrackedCoral.get()) + ) + ); + } + + private Command getTrackCoralCommand() { + return new RunCommand(() -> { + if (RobotContainer.CORAL_POSE_ESTIMATOR.getClosestObjectToRobot() != null) + distanceFromTrackedCoral = calculateDistanceFromTrackedCoral(); + }); + } + + private static Translation2d calculateDistanceFromTrackedCoral() { + 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 robotToTrackedCoralDistance = difference.rotateBy(robotPose.getRotation().unaryMinus()); + Logger.recordOutput("IntakeAssist/TrackedCoralDistance", robotToTrackedCoralDistance); + return robotToTrackedCoralDistance; + } + + private static Translation2d calculateTranslationPower(AssistMode assistMode, Translation2d distanceFromTrackedCoral) { + 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(distanceFromTrackedCoral.getX())); + final double yPIDOutput = clampToOutputRange(Y_PID_CONTROLLER.calculate(distanceFromTrackedCoral.getY())); + + if (assistMode.equals(AssistMode.ALTERNATE_ASSIST)) + return calculateAlternateAssistTranslationPower(selfRelativeJoystickPower, xPIDOutput, yPIDOutput); + return calculateNormalAssistTranslationPower(assistMode, selfRelativeJoystickPower, xPIDOutput, yPIDOutput); + } + + private static double calculateThetaPower(AssistMode assistMode, Translation2d distanceFromTrackedCoral) { + return calculateThetaAssistPower(assistMode, distanceFromTrackedCoral.getAngle().plus(Rotation2d.k180deg).unaryMinus()); + } + + private static Translation2d calculateAlternateAssistTranslationPower(Translation2d joystickValue, double xPIDOutput, double yPIDOutput) { + final double pidScalar = Math.cbrt(joystickValue.getNorm()); + final double + xJoystickPower = Math.cbrt(joystickValue.getX()), + yJoystickPower = Math.cbrt(joystickValue.getY()); + final double + xPower = calculateAlternateAssistPower(xPIDOutput, pidScalar, xJoystickPower), + yPower = calculateAlternateAssistPower(yPIDOutput, pidScalar, yJoystickPower); + + return new Translation2d(xPower, yPower); + } + + private static Translation2d calculateNormalAssistTranslationPower(AssistMode assistMode, Translation2d joystickValue, double xPIDOutput, double yPIDOutput) { + final double + xJoystickPower = joystickValue.getX(), + yJoystickPower = joystickValue.getY(); + final double + xPower = assistMode.shouldAssistX ? calculateNormalAssistPower(xPIDOutput, xJoystickPower) : xJoystickPower, + yPower = assistMode.shouldAssistY ? calculateNormalAssistPower(yPIDOutput, yJoystickPower) : yJoystickPower; + + return new Translation2d(xPower, yPower); + } + + private static double calculateThetaAssistPower(AssistMode assistMode, Rotation2d thetaOffset) { + 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); + } + + private static double clampToOutputRange(double value) { + return MathUtil.clamp(value, -1, 1); + } + + private static double calculateAlternateAssistPower(double pidOutput, double pidScalar, double joystickPower) { + 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 void resetPIDControllers(Translation2d distanceFromTrackedCoral) { + X_PID_CONTROLLER.reset(distanceFromTrackedCoral.getX(), RobotContainer.SWERVE.getSelfRelativeVelocity().vxMetersPerSecond); + Y_PID_CONTROLLER.reset(distanceFromTrackedCoral.getY(), RobotContainer.SWERVE.getSelfRelativeVelocity().vyMetersPerSecond); + THETA_PID_CONTROLLER.reset(distanceFromTrackedCoral.getAngle().getRadians(), RobotContainer.SWERVE.getSelfRelativeVelocity().omegaRadiansPerSecond); + } + + /** + * An enum containing different modes in which the command assists the intake of the game piece. + */ + public enum AssistMode { + /** + * An alternate method for assisting the intake where the pid output is scaled down the more input the driver gives. + */ + 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 + */ + 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 + */ + 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 + */ + ASSIST_ROTATION(false, false, true); + + final boolean + shouldAssistX, + shouldAssistY, + shouldAssistTheta; + + AssistMode(boolean shouldAssistX, boolean shouldAssistY, boolean shouldAssistTheta) { + this.shouldAssistX = shouldAssistX; + this.shouldAssistY = shouldAssistY; + this.shouldAssistTheta = shouldAssistTheta; + } + } +} \ No newline at end of file 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 0ecd3163..81c55a00 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AlgaeManipulationCommands.java @@ -7,9 +7,10 @@ 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.constants.PathPlannerConstants; import frc.trigon.robot.misc.ReefChooser; import frc.trigon.robot.subsystems.algaemanipulator.AlgaeManipulatorCommands; import frc.trigon.robot.subsystems.elevator.ElevatorCommands; @@ -21,24 +22,36 @@ import org.trigon.utilities.flippable.FlippableRotation2d; import org.trigon.utilities.flippable.FlippableTranslation2d; +import java.util.Map; + public class AlgaeManipulationCommands { private static final ReefChooser REEF_CHOOSER = OperatorConstants.REEF_CHOOSER; + public static boolean IS_HOLDING_ALGAE = false; public static Command getCollectAlgaeFromLollipopCommand() { return new SequentialCommandGroup( + new InstantCommand(() -> IS_HOLDING_ALGAE = true), CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.GRIPPER::hasGamePiece).asProxy(), - getGripAlgaeCommand(GripperConstants.GripperState.COLLECT_ALGAE_FROM_LOLLIPOP).asProxy().until(() -> OperatorConstants.SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean() || OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean()), + getGripAlgaeCommand(GripperConstants.GripperState.COLLECT_ALGAE_FROM_LOLLIPOP).asProxy() + .until(AlgaeManipulationCommands::isScoreAlgaeButtonPressed), getScoreAlgaeCommand().asProxy() - ).raceWith(getScoreNetEndingConditionCommand()).andThen(AlgaeManipulationCommands::reloadAfterScore); + ) + .raceWith(getScoreNetEndingConditionCommand(), getScoreProcessorEndingConditionCommand()) + .andThen(AlgaeManipulationCommands::reloadAfterScore) + .finallyDo(AlgaeManipulationCommands::disableIsHoldingAlgae); } public static Command getCollectAlgaeFromReefCommand() { return new SequentialCommandGroup( + new InstantCommand(() -> IS_HOLDING_ALGAE = true), CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.GRIPPER::hasGamePiece).asProxy(), - getCollectAlgaeFromReefManuallyCommand().asProxy() + getCollectAlgaeFromReefManuallyCommand().asProxy(), + getScoreAlgaeCommand().asProxy() ) - .alongWith(getAlignToReefCommand().onlyIf(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY && !OperatorConstants.RIGHT_MULTIFUNCTION_TRIGGER.getAsBoolean()).asProxy()) - .raceWith(getScoreNetEndingConditionCommand()).andThen(AlgaeManipulationCommands::reloadAfterScore); + .alongWith(getAlignToReefCommand().onlyIf(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY).asProxy()) + .raceWith(getScoreNetEndingConditionCommand(), getScoreProcessorEndingConditionCommand()) + .andThen(AlgaeManipulationCommands::reloadAfterScore) + .finallyDo(AlgaeManipulationCommands::disableIsHoldingAlgae); } public static Command getResetAmpAlignerCommand() { @@ -52,23 +65,35 @@ private static void reloadAfterScore() { new WaitUntilCommand(() -> RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.REST)).andThen(CoralCollectionCommands.getLoadCoralCommand().asProxy()).onlyIf(() -> RobotContainer.CORAL_INTAKE.hasGamePiece() && REEF_CHOOSER.getScoringLevel() != CoralPlacingCommands.ScoringLevel.L1_CORAL_INTAKE).schedule(); } + private static void disableIsHoldingAlgae() { + new WaitUntilCommand(() -> RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.REST)).andThen(() -> IS_HOLDING_ALGAE = false).schedule(); + } + private static Command getScoreNetEndingConditionCommand() { - return new WaitUntilCommand(() -> OperatorConstants.CONTINUE_TRIGGER.getAsBoolean() && RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.SCORE_NET)).andThen(new WaitCommand(0.5)); + return new WaitUntilCommand(() -> RobotContainer.GRIPPER.atState(GripperConstants.GripperState.SCORE_ALGAE_IN_NET)).andThen(new WaitCommand(0.1)); + } + + private static Command getScoreProcessorEndingConditionCommand() { + return new WaitUntilCommand(() -> RobotContainer.GRIPPER.atState(GripperConstants.GripperState.SCORE_ALGAE_IN_PROCESSOR)).andThen(new WaitCommand(0.1)).andThen(new WaitUntilCommand(() -> !OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean())); } private static Command getCollectAlgaeFromReefManuallyCommand() { return new ParallelCommandGroup( getGripAlgaeCommand(GripperConstants.GripperState.COLLECT_ALGAE_FROM_REEF), getOpenElevatorForAlgaeCommand() - ).until(() -> OperatorConstants.SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean() || OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean()).andThen( - getScoreAlgaeCommand() + ).raceWith( + new SequentialCommandGroup( + new WaitCommand(1), + new WaitUntilCommand(RobotContainer.GRIPPER::isMovingSlowly), + new WaitUntilCommand(() -> !OperatorConstants.COLLECT_ALGAE_FROM_L3_OVERRIDE_TRIGGER.getAsBoolean()) + ) ); } private static Command getGripAlgaeCommand(GripperConstants.GripperState targetGripState) { return new SequentialCommandGroup( - GripperCommands.getSetTargetStateWithCurrentCommand(targetGripState).raceWith(new WaitCommand(1).andThen(new WaitUntilCommand(RobotContainer.GRIPPER::isMovingSlowly))), - GripperCommands.getSetTargetStateWithCurrentCommand(GripperConstants.GripperState.HOLD_ALGAE) + GripperCommands.getSetTargetStateWhileHoldingAlgaeCommand(targetGripState).raceWith(new WaitCommand(1).andThen(new WaitUntilCommand(RobotContainer.GRIPPER::isMovingSlowly))), + GripperCommands.getSetTargetStateWhileHoldingAlgaeCommand(GripperConstants.GripperState.HOLD_ALGAE) ).alongWith( AlgaeManipulatorCommands.getOpenCommand() ); @@ -78,70 +103,88 @@ private static Command getOpenElevatorForAlgaeCommand() { return GeneralCommands.getContinuousConditionalCommand( ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.COLLECT_ALGAE_FROM_L3), ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST_WITH_ALGAE), - OperatorConstants.LEFT_MULTIFUNCTION_TRIGGER + OperatorConstants.COLLECT_ALGAE_FROM_L3_OVERRIDE_TRIGGER ); } private static Command getScoreAlgaeCommand() { - return new ConditionalCommand( - getScoreInNetCommand(), - getScoreInProcessorCommand(), - OperatorConstants.SCORE_ALGAE_IN_NET_TRIGGER + return new SelectCommand<>( + Map.of( + 0, getHoldAlgaeCommand(), + 1, getScoreInNetCommand(), + 2, getScoreInProcessorCommand() + ), + AlgaeManipulationCommands::getAlgaeScoreMethodSelector + ).raceWith(new WaitUntilChangeCommand<>(AlgaeManipulationCommands::isScoreAlgaeButtonPressed)).repeatedly(); + } + + private static int getAlgaeScoreMethodSelector() { + if (OperatorConstants.SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean()) + return 1; + else if (OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean()) + return 2; + return 0; + } + + private static Command getHoldAlgaeCommand() { + return new ParallelCommandGroup( + GripperCommands.getSetTargetStateWhileHoldingAlgaeCommand(GripperConstants.GripperState.HOLD_ALGAE), + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST_WITH_ALGAE), + AlgaeManipulatorCommands.getOpenCommand() ); } private static Command getScoreInNetCommand() { - return new ParallelCommandGroup( + return new ParallelRaceGroup( ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_NET), AlgaeManipulatorCommands.getOpenCommand(), new SequentialCommandGroup( - GripperCommands.getSetTargetStateWithCurrentCommand(GripperConstants.GripperState.PREPARE_FOR_SCORING_ALGAE_IN_NET).until(OperatorConstants.CONTINUE_TRIGGER), - GripperCommands.getSetTargetStateCommand(GripperConstants.GripperState.SCORE_ALGAE_IN_NET) + GripperCommands.getSetTargetStateWhileHoldingAlgaeCommand(GripperConstants.GripperState.PREPARE_FOR_SCORING_ALGAE_IN_NET).until(OperatorConstants.CONTINUE_TRIGGER), + GripperCommands.getSetTargetStateCommand(GripperConstants.GripperState.SCORE_ALGAE_IN_NET).withTimeout(0.5) ), SwerveCommands.getClosedLoopFieldRelativeDriveCommand( () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), () -> new FlippableRotation2d(Rotation2d.k180deg, true) - ).asProxy() + ).asProxy().onlyWhile(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY) ); } private static Command getScoreInProcessorCommand() { return new ParallelCommandGroup( - AlgaeManipulatorCommands.getOpenCommand().until(OperatorConstants.CONTINUE_TRIGGER), + AlgaeManipulatorCommands.getOpenCommand(), new SequentialCommandGroup( - GripperCommands.getSetTargetStateWithCurrentCommand(GripperConstants.GripperState.PREPARE_FOR_SCORING_ALGAE_IN_PROCESSOR).until(OperatorConstants.CONTINUE_TRIGGER), + GripperCommands.getSetTargetStateWhileHoldingAlgaeCommand(GripperConstants.GripperState.PREPARE_FOR_SCORING_ALGAE_IN_PROCESSOR).until(OperatorConstants.CONTINUE_TRIGGER), GripperCommands.getSetTargetStateCommand(GripperConstants.GripperState.SCORE_ALGAE_IN_PROCESSOR) ), SwerveCommands.getClosedLoopFieldRelativeDriveCommand( - () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()), () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftX()), () -> new FlippableRotation2d(Rotation2d.fromDegrees(90), true) - ).asProxy() - ); + ).asProxy().onlyWhile(() -> CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY) + ).until(() -> RobotContainer.GRIPPER.atState(GripperConstants.GripperState.SCORE_ALGAE_IN_PROCESSOR) && !OperatorConstants.CONTINUE_TRIGGER.getAsBoolean()); } private static Command getAlignToReefCommand() { return new SequentialCommandGroup( SwerveCommands.getDriveToPoseCommand( AlgaeManipulationCommands::calculateClosestAlgaeCollectionPose, - PathPlannerConstants.DRIVE_TO_REEF_CONSTRAINTS + AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS ), SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> fieldRelativePowersToSelfRelativeXPower(OperatorConstants.DRIVER_CONTROLLER.getLeftY(), OperatorConstants.DRIVER_CONTROLLER.getLeftX()), () -> 0, () -> calculateClosestAlgaeCollectionPose().getRotation() ) - ).until(OperatorConstants.CONTINUE_TRIGGER); + ).raceWith( + new WaitCommand(1).andThen(new WaitUntilCommand(RobotContainer.GRIPPER::isMovingSlowly)), + new WaitUntilCommand(OperatorConstants.STOP_ALGAE_AUTO_ALIGN_OVERRIDE_TRIGGER) + ); } - private static Pose2d calculateReefAlgaeCollectionPose() { - final Translation2d reefCenterPositionOnField = new FlippableTranslation2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, true).get(); - final Rotation2d targetAngle = calculateTargetAngle().get(); - final Transform2d reefCenterToTargetPoseTranslation = new Transform2d(FieldConstants.REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS, 0, new Rotation2d()); - - return new Pose2d(reefCenterPositionOnField, targetAngle).transformBy(reefCenterToTargetPoseTranslation); + private static boolean isScoreAlgaeButtonPressed() { + return OperatorConstants.SCORE_ALGAE_IN_NET_TRIGGER.getAsBoolean() || + OperatorConstants.SCORE_ALGAE_IN_PROCESSOR_TRIGGER.getAsBoolean(); } private static FlippableRotation2d calculateTargetAngle() { 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 d60b8a73..e135dc60 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java @@ -9,9 +9,8 @@ import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandclasses.CoralAutoDriveCommand; +import frc.trigon.robot.constants.AutonomousConstants; import frc.trigon.robot.constants.FieldConstants; -import frc.trigon.robot.constants.OperatorConstants; -import frc.trigon.robot.constants.PathPlannerConstants; import frc.trigon.robot.subsystems.coralintake.CoralIntakeCommands; import frc.trigon.robot.subsystems.coralintake.CoralIntakeConstants; import frc.trigon.robot.subsystems.elevator.ElevatorCommands; @@ -73,13 +72,12 @@ public static Command getDriveToCoralCommand(boolean isRight) { } public static Command getFindCoralCommand(boolean isRight) { - return new ParallelCommandGroup( - SwerveCommands.getDriveToPoseCommand(() -> isRight ? FieldConstants.AUTO_FIND_CORAL_POSE_RIGHT : FieldConstants.AUTO_FIND_CORAL_POSE_LEFT, PathPlannerConstants.DRIVE_TO_REEF_CONSTRAINTS).andThen( - SwerveCommands.getClosedLoopSelfRelativeDriveCommand( - () -> 0, - () -> 0, - () -> 0.2 - ) + return new SequentialCommandGroup( + SwerveCommands.getDriveToPoseCommand(() -> isRight ? FieldConstants.AUTO_FIND_CORAL_POSE_RIGHT : FieldConstants.AUTO_FIND_CORAL_POSE_LEFT, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS, 2.3), + SwerveCommands.getClosedLoopSelfRelativeDriveCommand( + () -> 0, + () -> 0, + () -> 0.2 ) ); } @@ -93,10 +91,10 @@ public static Command getDriveToReefAndScoreCommand(FieldConstants.ReefClockPosi public static Command getDriveToReefWithoutHittingAlgaeCommand(FieldConstants.ReefClockPosition[] reefClockPositions) { return new SequentialCommandGroup( - new InstantCommand(() -> TARGET_SCORING_POSE = calculateClosestScoringPose(true, reefClockPositions, false)), + new InstantCommand(() -> TARGET_SCORING_POSE = calculateClosestScoringPose(true, reefClockPositions, false)),//TODO: Always stay behind bc of gripper aiming new WaitUntilCommand(() -> TARGET_SCORING_POSE != null).raceWith(SwerveCommands.getClosedLoopSelfRelativeDriveCommand(() -> 0, () -> 0, () -> 0)), - SwerveCommands.getDriveToPoseCommand(() -> calculateClosestScoringPose(true, reefClockPositions, true), PathPlannerConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly().until(RobotContainer.ELEVATOR::isElevatorOverAlgaeHitRange), - SwerveCommands.getDriveToPoseCommand(() -> TARGET_SCORING_POSE, PathPlannerConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly() + SwerveCommands.getDriveToPoseCommand(() -> calculateClosestScoringPose(true, reefClockPositions, true), AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly().until(RobotContainer.ELEVATOR::isOverAlgaeHitRange), + SwerveCommands.getDriveToPoseCommand(() -> TARGET_SCORING_POSE, AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS).repeatedly() ); } @@ -129,14 +127,27 @@ private static boolean isFirst() { public static Command getPrepareForScoreCommand() { return new ParallelCommandGroup( - ElevatorCommands.getSetTargetStateCommand(OperatorConstants.REEF_CHOOSER.getElevatorState()), + getOpenElevatorWhenCloseToReefCommand(), GripperCommands.getPrepareForScoringInL4Command(() -> TARGET_SCORING_POSE) ); } + private static Command getOpenElevatorWhenCloseToReefCommand() { + return GeneralCommands.runWhen( + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_L4), + () -> calculateDistanceToTargetScoringPose() < AutonomousConstants.MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR + ); + } + + private static double calculateDistanceToTargetScoringPose() { + final Translation2d currentTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); + final Translation2d targetTranslation = TARGET_SCORING_POSE.get().getTranslation(); + return currentTranslation.getDistance(targetTranslation); + } + public static Command getFeedCoralCommand() { return new ParallelCommandGroup( - ElevatorCommands.getSetTargetStateCommand(OperatorConstants.REEF_CHOOSER.getElevatorState()), + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.SCORE_L4), GripperCommands.getScoreInL4Command(() -> TARGET_SCORING_POSE), getAddCurrentScoringBranchToScoredBranchesCommand() ).withTimeout(0.25); @@ -189,8 +200,8 @@ public static FlippablePose2d calculateClosestScoringPose(boolean shouldOnlyChec @AutoLogOutput private static boolean canFeed() { - return RobotContainer.ELEVATOR.atState(OperatorConstants.REEF_CHOOSER.getScoringLevel().elevatorState) && - RobotContainer.GRIPPER.atState(OperatorConstants.REEF_CHOOSER.getScoringLevel().gripperState) && + return RobotContainer.ELEVATOR.atState(ElevatorConstants.ElevatorState.SCORE_L4) && + RobotContainer.GRIPPER.atState(GripperConstants.GripperState.SCORE_L4_CLOSE) && TARGET_SCORING_POSE != null && Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getX()) < 0.085 && Math.abs(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().relativeTo(TARGET_SCORING_POSE.get()).getY()) < 0.03; 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 61dd3d78..ea6276ff 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -1,7 +1,11 @@ package frc.trigon.robot.commands.commandfactories; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand; +import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.coralintake.CoralIntakeCommands; import frc.trigon.robot.subsystems.coralintake.CoralIntakeConstants; @@ -9,43 +13,65 @@ import frc.trigon.robot.subsystems.elevator.ElevatorConstants; import frc.trigon.robot.subsystems.gripper.GripperCommands; import frc.trigon.robot.subsystems.gripper.GripperConstants; +import org.trigon.utilities.flippable.FlippablePose2d; public class CoralCollectionCommands { - public static boolean SHOULD_IGNORE_LOLLIPOP_CORAL = true; - - public static Command getFeederCoralCollectionFromGripperCommand() { - return new ParallelCommandGroup( - GripperCommands.getSetTargetStateCommand(GripperConstants.GripperState.COLLECT_CORAL_FROM_FEEDER), - ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST), - getScheduleCoralLoadingWhenCollectedCommand() - ).until(RobotContainer.GRIPPER::hasGamePiece); - } + public static boolean + SHOULD_IGNORE_LOLLIPOP_CORAL = false, + SHOULD_ASSIST_INTAKE = true, + SHOULD_KEEP_INTAKE_OPEN = true; public static Command getFloorCoralCollectionCommand() { - return getInitiateFloorCoralCollectionCommand(); + return new ParallelCommandGroup( + CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.COLLECT_FROM_FLOOR), + getScheduleCoralLoadingWhenCollectedCommand(), + new IntakeAssistCommand(OperatorConstants.DEFAULT_INTAKE_ASSIST_MODE).asProxy().onlyWhile(() -> SHOULD_ASSIST_INTAKE) + ); } public static Command getFeederCoralCollectionCommand() { - return getInitiateFeederCoralCollectionCommand().unless(RobotContainer.GRIPPER::hasGamePiece); + return new ConditionalCommand( + getFeederCoralCollectionFromIntakeCommand().unless(RobotContainer.GRIPPER::hasGamePiece), + getFeederCoralCollectionFromGripperCommand().asProxy(), + () -> CoralCollectionCommands.isIntakeFacingFeeder() || RobotContainer.ALGAE_MANIPULATOR.isOpen() + ); } - private static Command getInitiateFeederCoralCollectionCommand() { + public static Command getFeederCoralCollectionFromIntakeCommand() { return new ParallelCommandGroup( CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.COLLECT_FROM_FEEDER), getScheduleCoralLoadingWhenCollectedCommand() ); } - private static Command getInitiateFloorCoralCollectionCommand() { + public static Command getFeederCoralCollectionFromGripperCommand() { return new ParallelCommandGroup( - CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.COLLECT_FROM_FLOOR), + GripperCommands.getSetTargetStateCommand(GripperConstants.GripperState.COLLECT_CORAL_FROM_FEEDER), + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST), getScheduleCoralLoadingWhenCollectedCommand() - ); + ).until(RobotContainer.GRIPPER::hasGamePiece); + } + + private static boolean isIntakeFacingFeeder() { + final Pose2d robotPose = new FlippablePose2d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(), true).get(); + final Rotation2d + robotHeading = robotPose.getRotation(), + leftFeederAngle = FieldConstants.LEFT_FEEDER_ANGLE; + + if (robotPose.getY() > FieldConstants.FIELD_WIDTH_METERS / 2) + return robotHeading.plus(leftFeederAngle).plus(Rotation2d.kCW_90deg).getDegrees() > 0; + return robotHeading.minus(leftFeederAngle).plus(Rotation2d.kCCW_90deg).getDegrees() < 0; } private static Command getScheduleCoralLoadingWhenCollectedCommand() { - return GeneralCommands.runWhen(getCollectionConfirmationCommand().alongWith(getScheduleCoralLoadingCommand()), CoralCollectionCommands::didCollectCoral); + return GeneralCommands.runWhen( + new ParallelCommandGroup( + getCollectionConfirmationCommand(), + getScheduleCoralLoadingCommand() + ), + CoralCollectionCommands::didCollectCoral + ); } private static Command getScheduleCoralLoadingCommand() { @@ -81,7 +107,7 @@ public static Command getUnloadCoralCommand() { } private static boolean shouldStopLoadingCoral() { - return RobotContainer.GRIPPER.hasGamePiece() || OperatorConstants.CONTINUE_TRIGGER.getAsBoolean(); + return RobotContainer.GRIPPER.hasGamePiece(); } private static Command getCoralIntakeLoadingSequenceCommand() { @@ -93,11 +119,6 @@ private static Command getCoralIntakeLoadingSequenceCommand() { ).repeatedly(); } - private static boolean isMovingCoralToGripper() { - return RobotContainer.CORAL_INTAKE.getTargetState() == CoralIntakeConstants.CoralIntakeState.LOAD_CORAL_TO_GRIPPER_SEEING_GAME_PIECE_WITH_BEAM_BREAK || - RobotContainer.CORAL_INTAKE.getTargetState() == CoralIntakeConstants.CoralIntakeState.LOAD_CORAL_TO_GRIPPER_NOT_SEEING_GAME_PIECE_WITH_BEAM_BREAK; - } - private static Command getCollectionConfirmationCommand() { return new ParallelCommandGroup( new InstantCommand(() -> OperatorConstants.DRIVER_CONTROLLER.rumble(CoralIntakeConstants.COLLECTION_RUMBLE_DURATION_SECONDS, CoralIntakeConstants.COLLECTION_RUMBLE_POWER)) 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 6754817e..71ab7f12 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -7,9 +7,9 @@ import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.RobotContainer; 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.constants.PathPlannerConstants; import frc.trigon.robot.misc.ReefChooser; import frc.trigon.robot.subsystems.coralintake.CoralIntakeCommands; import frc.trigon.robot.subsystems.coralintake.CoralIntakeConstants; @@ -25,35 +25,27 @@ public class CoralPlacingCommands { public static boolean SHOULD_SCORE_AUTONOMOUSLY = true; private static final ReefChooser REEF_CHOOSER = OperatorConstants.REEF_CHOOSER; - public static Command getScoreInReefCommand() { + public static Command getScoreInReefCommand(boolean shouldScoreRight) { return new ConditionalCommand( - getCoralIntakeScoringSequenceCommand().asProxy(), - getScoreInReefFromGripperCommand().asProxy(), + getScoreInReefFromCoralIntakeCommand().asProxy(), + getScoreInReefFromGripperCommand(shouldScoreRight).asProxy(), () -> REEF_CHOOSER.getScoringLevel() == ScoringLevel.L1_CORAL_INTAKE - ).raceWith(getWaitUntilScoringTargetChangesCommand()).repeatedly(); - } - - private static Command getWaitUntilScoringTargetChangesCommand() { - return new ParallelRaceGroup( - new WaitUntilChangeCommand<>(REEF_CHOOSER::getClockPosition), - new WaitUntilChangeCommand<>(REEF_CHOOSER::getReefSide), - new WaitUntilChangeCommand<>(OperatorConstants.RIGHT_MULTIFUNCTION_TRIGGER::getAsBoolean) ); } - private static Command getScoreInReefFromGripperCommand() { + private static Command getScoreInReefFromGripperCommand(boolean shouldScoreRight) { return GeneralCommands.getContinuousConditionalCommand( - getAutonomouslyScoreInReefFromGripperCommand().asProxy(), + getAutonomouslyScoreInReefFromGripperCommand(shouldScoreRight).asProxy(), getManuallyScoreInReefFromGripperCommand().asProxy(), - () -> SHOULD_SCORE_AUTONOMOUSLY && !OperatorConstants.LEFT_MULTIFUNCTION_TRIGGER.getAsBoolean() && REEF_CHOOSER.getScoringLevel() != ScoringLevel.L1_GRIPPER + () -> SHOULD_SCORE_AUTONOMOUSLY && REEF_CHOOSER.getScoringLevel() != ScoringLevel.L1_GRIPPER ).until(() -> REEF_CHOOSER.getScoringLevel() == ScoringLevel.L1_CORAL_INTAKE); } - private static Command getCoralIntakeScoringSequenceCommand() { + private static Command getScoreInReefFromCoralIntakeCommand() { return new SequentialCommandGroup( CoralCollectionCommands.getUnloadCoralCommand().onlyIf(RobotContainer.GRIPPER::hasGamePiece), CoralIntakeCommands.getPrepareForStateCommand(CoralIntakeConstants.CoralIntakeState.SCORE_L1).until(CoralPlacingCommands::canContinueScoringFromCoralIntake), - CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.SCORE_L1_BOOST).withTimeout(0.07), + CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.SCORE_L1_BOOST).withTimeout(0.05), CoralIntakeCommands.getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.SCORE_L1) ).until(() -> REEF_CHOOSER.getScoringLevel() != ScoringLevel.L1_CORAL_INTAKE); } @@ -67,91 +59,98 @@ private static Command getManuallyScoreInReefFromGripperCommand() { ); } - private static Command getAutonomouslyScoreInReefFromGripperCommand() { - return new ParallelCommandGroup( + private static Command getAutonomouslyScoreInReefFromGripperCommand(boolean shouldScoreRight) { + return new ParallelRaceGroup( CoralCollectionCommands.getLoadCoralCommand().asProxy().andThen( new ParallelCommandGroup( - getOpenElevatorWhenCloseToReefCommand().raceWith(new WaitUntilChangeCommand<>(REEF_CHOOSER::getElevatorState)).repeatedly(), - getAutoGripperScoringSequenceCommand() - ).asProxy() + getOpenElevatorWhenCloseToReefCommand(shouldScoreRight), + getAutoGripperScoringSequenceCommand(shouldScoreRight) + ).asProxy().raceWith(new WaitUntilChangeCommand<>(REEF_CHOOSER::getElevatorState)).repeatedly() ), - getAutonomousDriveToReefThenManualDriveCommand() - ); + getAutonomousDriveToReefThenManualDriveCommand(shouldScoreRight) + ).andThen(); } private static Command getGripperScoringSequenceCommand() { return new SequentialCommandGroup( GripperCommands.getSetTargetStateCommand(GripperConstants.GripperState.OPEN_FOR_NOT_HITTING_REEF) .unless(() -> RobotContainer.ELEVATOR.atState(REEF_CHOOSER.getElevatorState()) || REEF_CHOOSER.getScoringLevel() == ScoringLevel.L2 || REEF_CHOOSER.getScoringLevel() == ScoringLevel.L1_GRIPPER) - .until(() -> RobotContainer.ELEVATOR.atState(REEF_CHOOSER.getElevatorState())), + .until(RobotContainer.ELEVATOR::isCloseEnoughToOpenGripper), scoreFromGripperReefChooserCommand() ); } - private static Command getAutoGripperScoringSequenceCommand() { + private static Command getAutoGripperScoringSequenceCommand(boolean shouldScoreRight) { return new SequentialCommandGroup( GripperCommands.getSetTargetStateCommand(GripperConstants.GripperState.OPEN_FOR_NOT_HITTING_REEF) .unless(() -> RobotContainer.ELEVATOR.atState(REEF_CHOOSER.getElevatorState()) || REEF_CHOOSER.getScoringLevel() == ScoringLevel.L2 || REEF_CHOOSER.getScoringLevel() == ScoringLevel.L1_GRIPPER) - .until(() -> RobotContainer.ELEVATOR.atState(REEF_CHOOSER.getElevatorState())), + .until(RobotContainer.ELEVATOR::isCloseEnoughToOpenGripper), new ConditionalCommand( - scoreFromGripperInL4Command(), - scoreFromGripperReefChooserCommand(), + scoreFromGripperInL4Command(shouldScoreRight), + scoreFromGripperReefChooserCommand(shouldScoreRight), () -> REEF_CHOOSER.getScoringLevel() == ScoringLevel.L4 ) ); } + private static Command scoreFromGripperReefChooserCommand(boolean shouldScoreRight) { + return new SequentialCommandGroup( + GripperCommands.getPrepareForStateCommand(REEF_CHOOSER::getGripperState).raceWith( + new SequentialCommandGroup( + new WaitUntilCommand(() -> canAutonomouslyReleaseFromGripper(shouldScoreRight)), + new WaitUntilChangeCommand<>(RobotContainer.ROBOT_POSE_ESTIMATOR::getEstimatedRobotPose) + ) + ).until(OperatorConstants.CONTINUE_TRIGGER), + GripperCommands.getSetTargetStateCommand(REEF_CHOOSER::getGripperState).finallyDo(OperatorConstants.REEF_CHOOSER::switchReefSide) + ); + } + private static Command scoreFromGripperReefChooserCommand() { return new SequentialCommandGroup( - GripperCommands.getPrepareForStateCommand(REEF_CHOOSER::getGripperState).until(CoralPlacingCommands::canContinueScoringFromGripper), + GripperCommands.getPrepareForStateCommand(REEF_CHOOSER::getGripperState).until(OperatorConstants.CONTINUE_TRIGGER), GripperCommands.getSetTargetStateCommand(REEF_CHOOSER::getGripperState).finallyDo(OperatorConstants.REEF_CHOOSER::switchReefSide) ); } - private static Command scoreFromGripperInL4Command() { + private static Command scoreFromGripperInL4Command(boolean shouldScoreRight) { return new SequentialCommandGroup( - GripperCommands.getPrepareForScoringInL4Command(CoralPlacingCommands::calculateTargetScoringPose).until(CoralPlacingCommands::canContinueScoringFromGripper), - GripperCommands.getScoreInL4Command(CoralPlacingCommands::calculateTargetScoringPose).finallyDo(OperatorConstants.REEF_CHOOSER::switchReefSide) + GripperCommands.getPrepareForScoringInL4Command(REEF_CHOOSER::calculateTargetScoringPose).raceWith( + new WaitUntilCommand(() -> canAutonomouslyReleaseFromGripper(shouldScoreRight)) + ).until(OperatorConstants.CONTINUE_TRIGGER), + GripperCommands.getScoreInL4Command(REEF_CHOOSER::calculateTargetScoringPose).finallyDo(OperatorConstants.REEF_CHOOSER::switchReefSide) ); } - private static Command getOpenElevatorWhenCloseToReefCommand() { + private static Command getOpenElevatorWhenCloseToReefCommand(boolean shouldScoreRight) { return GeneralCommands.runWhen( ElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getElevatorState), - () -> calculateDistanceToTargetScoringPose() < PathPlannerConstants.MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR + () -> calculateDistanceToTargetScoringPose(shouldScoreRight) < AutonomousConstants.MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR ); } - private static Command getAutonomousDriveToReefThenManualDriveCommand() { + private static Command getAutonomousDriveToReefThenManualDriveCommand(boolean shouldScoreRight) { return new SequentialCommandGroup( SwerveCommands.getDriveToPoseCommand( - CoralPlacingCommands::calculateTargetScoringPose, - PathPlannerConstants.DRIVE_TO_REEF_CONSTRAINTS + () -> CoralPlacingCommands.calculateClosestScoringPose(shouldScoreRight), + AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS ), GeneralCommands.getFieldRelativeDriveCommand() ); } - public static FlippablePose2d calculateTargetScoringPose() { - if (OperatorConstants.RIGHT_MULTIFUNCTION_TRIGGER.getAsBoolean()) - return REEF_CHOOSER.calculateTargetScoringPose(); - return calculateClosestScoringPose(); - } - - private static double calculateDistanceToTargetScoringPose() { + private static double calculateDistanceToTargetScoringPose(boolean shouldScoreRight) { final Translation2d currentTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); - final Translation2d targetTranslation = calculateTargetScoringPose().get().getTranslation(); + final Translation2d targetTranslation = calculateClosestScoringPose(shouldScoreRight).get().getTranslation(); return currentTranslation.getDistance(targetTranslation); } - public static FlippablePose2d calculateClosestScoringPose() { + public static FlippablePose2d calculateClosestScoringPose(boolean shouldScoreRight) { final Translation2d robotPositionOnField = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); final Translation2d reefCenterPosition = new FlippableTranslation2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, true).get(); final Rotation2d[] reefClockAngles = FieldConstants.REEF_CLOCK_ANGLES; final Transform2d reefCenterToScoringPose = new Transform2d(FieldConstants.REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS, 0, new Rotation2d()), scoringPoseToRightBranch = new Transform2d(0, FieldConstants.REEF_CENTER_TO_TARGET_SCORING_POSITION_Y_TRANSFORM_METERS, new Rotation2d()); - final boolean shouldScoreOnRightBranch = REEF_CHOOSER.getReefSide().doesFlipYTransformWhenFacingDriverStation; double distanceFromClosestScoringPoseMeters = Double.POSITIVE_INFINITY; Pose2d closestScoringPose = new Pose2d(); @@ -166,19 +165,17 @@ public static FlippablePose2d calculateClosestScoringPose() { } } - return new FlippablePose2d(closestScoringPose.transformBy(shouldScoreOnRightBranch ? scoringPoseToRightBranch : scoringPoseToRightBranch.inverse()), false); + return new FlippablePose2d(closestScoringPose.transformBy(shouldScoreRight ? scoringPoseToRightBranch : scoringPoseToRightBranch.inverse()), false); } private static boolean canContinueScoringFromCoralIntake() { - return RobotContainer.CORAL_INTAKE.atTargetAngle() && - OperatorConstants.CONTINUE_TRIGGER.getAsBoolean(); + return OperatorConstants.CONTINUE_TRIGGER.getAsBoolean(); } - private static boolean canContinueScoringFromGripper() { + private static boolean canAutonomouslyReleaseFromGripper(boolean shouldScoreRight) { return RobotContainer.ELEVATOR.atTargetState() && RobotContainer.GRIPPER.atTargetAngle() && - OperatorConstants.CONTINUE_TRIGGER.getAsBoolean(); -// RobotContainer.SWERVE.atPose(calculateTargetScoringPose()); + RobotContainer.SWERVE.atPose(calculateClosestScoringPose(shouldScoreRight)); } /** diff --git a/src/main/java/frc/trigon/robot/constants/PathPlannerConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java similarity index 92% rename from src/main/java/frc/trigon/robot/constants/PathPlannerConstants.java rename to src/main/java/frc/trigon/robot/constants/AutonomousConstants.java index 1630a416..15fdf5fe 100644 --- a/src/main/java/frc/trigon/robot/constants/PathPlannerConstants.java +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -27,9 +27,9 @@ import java.io.IOException; /** - * A class that contains the constants and configurations for everything related to PathPlanner. + * A class that contains the constants and configurations for everything related to the 15-second autonomous period at the start of the match. */ -public class PathPlannerConstants { +public class AutonomousConstants { public static final PathConstraints DRIVE_TO_REEF_CONSTRAINTS = new PathConstraints(2.5, 4, Units.degreesToRadians(450), Units.degreesToRadians(900)); public static final double MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR = 2.2; public static final String DEFAULT_AUTO_NAME = "FloorAutonomousRight6Branches"; @@ -43,12 +43,6 @@ public class PathPlannerConstants { AUTO_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? new PIDConstants(0, 0, 0) : new PIDConstants(0, 0, 0); - // AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? -// new PIDConstants(6, 0, 0) : -// new PIDConstants(6, 0, 0.1), -// AUTO_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? -// new PIDConstants(5, 0, 0) : -// new PIDConstants(8, 0, 0); private static final PPHolonomicDriveController AUTO_PATH_FOLLOWING_CONTROLLER = new PPHolonomicDriveController( AUTO_TRANSLATION_PID_CONSTANTS, AUTO_ROTATION_PID_CONSTANTS diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 174dcc86..1e9961aa 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -14,11 +14,7 @@ public class CameraConstants { REEF_TAG_CAMERA_STANDARD_DEVIATIONS = new StandardDeviations( 0.015, 0.01 - ), - FEEDER_TAG_CAMERA_STANDARD_DEVIATIONS = new StandardDeviations( - 0.02, - 0.1 - ); + ); public static final double CORAL_POSE_ESTIMATOR_DELETION_THRESHOLD_SECONDS = 0.5; private static final Transform3d @@ -56,10 +52,4 @@ public class CameraConstants { ROBOT_CENTER_TO_RIGHT_REEF_TAG_CAMERA, REEF_TAG_CAMERA_STANDARD_DEVIATIONS ); -// FEEDER_TAG_CAMERA = new AprilTagCamera( -// AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA, -// "CoralStationTagCamera", -// ROBOT_CENTER_TO_FEEDER_TAG_CAMERA, -// FEEDER_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 3e911a4d..26ef4ae0 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -26,8 +26,15 @@ public class FieldConstants { private static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0)); public static final HashMap TAG_ID_TO_POSE = fieldLayoutToTagIdToPoseMap(); - public static final FlippablePose2d AUTO_FIND_CORAL_POSE_LEFT = new FlippablePose2d(3.3, 5.5, Rotation2d.fromDegrees(130), true); - public static final FlippablePose2d AUTO_FIND_CORAL_POSE_RIGHT = new FlippablePose2d(3.3, FieldConstants.FIELD_WIDTH_METERS - 5.5, Rotation2d.fromDegrees(-130), true); + private static final double + AUTO_FIND_CORAL_POSE_X = 3.3, + AUTO_FIND_CORAL_POSE_LEFT_Y = 5.5; + private static final Rotation2d AUTO_FIND_CORAL_POSE_LEFT_ROTATION = Rotation2d.fromDegrees(130); + public static final FlippablePose2d + AUTO_FIND_CORAL_POSE_LEFT = new FlippablePose2d(AUTO_FIND_CORAL_POSE_X, AUTO_FIND_CORAL_POSE_LEFT_Y, AUTO_FIND_CORAL_POSE_LEFT_ROTATION, true), + AUTO_FIND_CORAL_POSE_RIGHT = new FlippablePose2d(AUTO_FIND_CORAL_POSE_X, FieldConstants.FIELD_WIDTH_METERS - AUTO_FIND_CORAL_POSE_LEFT_Y, AUTO_FIND_CORAL_POSE_LEFT_ROTATION.unaryMinus(), true); + + public static final 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); diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index f3b2eb52..a1ffa7d8 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -1,7 +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 org.trigon.hardware.misc.KeyboardController; import org.trigon.hardware.misc.XboxController; @@ -11,59 +14,79 @@ public class OperatorConstants { private static final int DRIVER_CONTROLLER_PORT = 0, REEF_CHOOSER_PORT = 1; - private static final int DRIVER_CONTROLLER_EXPONENT = 2; + private static final int + CONTROLLER_TRANSLATION_EXPONENT = 2, + CONTROLLER_ROTATION_EXPONENT = 1; public static final XboxController DRIVER_CONTROLLER = new XboxController( - DRIVER_CONTROLLER_PORT, DRIVER_CONTROLLER_EXPONENT, DRIVER_CONTROLLER_DEADBAND + DRIVER_CONTROLLER_PORT, CONTROLLER_ROTATION_EXPONENT, CONTROLLER_TRANSLATION_EXPONENT, DRIVER_CONTROLLER_DEADBAND ); public static final KeyboardController OPERATOR_CONTROLLER = new KeyboardController(); public static final ReefChooser REEF_CHOOSER = new ReefChooser(REEF_CHOOSER_PORT); + private static boolean + IS_LEFT_SCORE_BUTTON_PRESSED = false, + IS_RIGHT_SCORE_BUTTON_PRESSED = false; public static final double POV_DIVIDER = 2, ROTATION_STICK_SPEED_DIVIDER = 1; + public static final double INTAKE_ASSIST_SCALAR = 0.0; + public static final IntakeAssistCommand.AssistMode DEFAULT_INTAKE_ASSIST_MODE = IntakeAssistCommand.AssistMode.ALTERNATE_ASSIST; + public static final Trigger - RESET_HEADING_TRIGGER = OPERATOR_CONTROLLER.r(), - DRIVE_FROM_DPAD_TRIGGER = new Trigger(() -> DRIVER_CONTROLLER.getPov() != -1), + LED_AUTO_SETUP_TRIGGER = OPERATOR_CONTROLLER.backtick(), + RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.y(), TOGGLE_BRAKE_TRIGGER = OPERATOR_CONTROLLER.g().or(RobotController::getUserButton), - RESET_AMP_ALIGNER_TRIGGER = OPERATOR_CONTROLLER.v(), - LEFT_MULTIFUNCTION_TRIGGER = DRIVER_CONTROLLER.leftStick().or(OPERATOR_CONTROLLER.b()), - RIGHT_MULTIFUNCTION_TRIGGER = DRIVER_CONTROLLER.rightStick().or(OPERATOR_CONTROLLER.m()), + DEBUGGING_TRIGGER = OPERATOR_CONTROLLER.f2(), 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(), + CONTINUE_TRIGGER = OPERATOR_CONTROLLER.k().or(DRIVER_CONTROLLER.rightStick().and(DRIVER_CONTROLLER.leftStick())); + public static final Trigger + RESET_AMP_ALIGNER_TRIGGER = OPERATOR_CONTROLLER.v(), FLOOR_CORAL_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftTrigger().or(OPERATOR_CONTROLLER.c()), FEEDER_CORAL_COLLECTION_TRIGGER = OPERATOR_CONTROLLER.f().or(DRIVER_CONTROLLER.start()), - SCORE_CORAL_IN_REEF_TRIGGER = DRIVER_CONTROLLER.rightBumper().or(OPERATOR_CONTROLLER.q()), - CONTINUE_TRIGGER = OPERATOR_CONTROLLER.k().or(DRIVER_CONTROLLER.leftBumper()), - EJECT_CORAL_TRIGGER = OPERATOR_CONTROLLER.e().or(DRIVER_CONTROLLER.x()), - DEBUGGING_TRIGGER = OPERATOR_CONTROLLER.f2(), + RIGHT_SCORE_TRIGGER = OPERATOR_CONTROLLER.m().or(createScoreTrigger(DRIVER_CONTROLLER.rightStick().and(() -> !AlgaeManipulationCommands.IS_HOLDING_ALGAE), true)), + LEFT_SCORE_TRIGGER = OPERATOR_CONTROLLER.b().or(createScoreTrigger(DRIVER_CONTROLLER.leftStick().and(() -> !AlgaeManipulationCommands.IS_HOLDING_ALGAE), false)), + EJECT_CORAL_TRIGGER = OPERATOR_CONTROLLER.e().or(DRIVER_CONTROLLER.x().and(() -> !AlgaeManipulationCommands.IS_HOLDING_ALGAE)), UNLOAD_CORAL_TRIGGER = OPERATOR_CONTROLLER.z().or(DRIVER_CONTROLLER.back()), - COLLECT_ALGAE_FROM_REEF_TRIGGER = OPERATOR_CONTROLLER.a().or(DRIVER_CONTROLLER.a()), - COLLECT_ALGAE_FROM_LOLLIPOP_TRIGGER = OPERATOR_CONTROLLER.l().or(DRIVER_CONTROLLER.y()), - FEEDER_CORAL_COLLECTION_WITH_GRIPPER = OPERATOR_CONTROLLER.d(), - SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.n().or(DRIVER_CONTROLLER.b()), - SCORE_ALGAE_IN_PROCESSOR_TRIGGER = OPERATOR_CONTROLLER.j(), - INCREMENT_TARGET_SCORING_LEVEL_TRIGGER = DRIVER_CONTROLLER.povUp(), - DECREMENT_TARGET_SCORING_LEVEL_TRIGGER = DRIVER_CONTROLLER.povDown(), - LED_AUTO_SETUP_TRIGGER = OPERATOR_CONTROLLER.backtick(); + COLLECT_ALGAE_FROM_REEF_TRIGGER = OPERATOR_CONTROLLER.a().or(DRIVER_CONTROLLER.rightBumper()), + COLLECT_ALGAE_FROM_L3_OVERRIDE_TRIGGER = DRIVER_CONTROLLER.leftStick().or(DRIVER_CONTROLLER.rightStick()), + COLLECT_ALGAE_FROM_LOLLIPOP_TRIGGER = OPERATOR_CONTROLLER.l().or(DRIVER_CONTROLLER.leftBumper()), + SCORE_ALGAE_IN_NET_TRIGGER = OPERATOR_CONTROLLER.n().or(createScoreTrigger(DRIVER_CONTROLLER.rightStick().and(() -> AlgaeManipulationCommands.IS_HOLDING_ALGAE), true)), + SCORE_ALGAE_IN_PROCESSOR_TRIGGER = OPERATOR_CONTROLLER.j().or(createScoreTrigger(DRIVER_CONTROLLER.leftStick().and(() -> AlgaeManipulationCommands.IS_HOLDING_ALGAE), false)), + STOP_ALGAE_AUTO_ALIGN_OVERRIDE_TRIGGER = DRIVER_CONTROLLER.x().and(() -> AlgaeManipulationCommands.IS_HOLDING_ALGAE); public static final Trigger + ENABLE_INTAKE_ASSIST_TRIGGER = OPERATOR_CONTROLLER.o(), + DISABLE_INTAKE_ASSIST_TRIGGER = OPERATOR_CONTROLLER.p(), ENABLE_AUTONOMOUS_REEF_SCORING_TRIGGER = OPERATOR_CONTROLLER.u(), DISABLE_AUTONOMOUS_REEF_SCORING_TRIGGER = OPERATOR_CONTROLLER.i(), + TOGGLE_SHOULD_KEEP_INTAKE_OPEN_TRIGGER = DRIVER_CONTROLLER.b(); + public static final Trigger SET_TARGET_SCORING_REEF_LEVEL_L1_FROM_GRIPPER_TRIGGER = OPERATOR_CONTROLLER.numpadDecimal(), - SET_TARGET_SCORING_REEF_LEVEL_L1_FROM_CORAL_INTAKE_TRIGGER = OPERATOR_CONTROLLER.numpad0(), - SET_TARGET_SCORING_REEF_LEVEL_L2_TRIGGER = OPERATOR_CONTROLLER.numpad1(), - SET_TARGET_SCORING_REEF_LEVEL_L3_TRIGGER = OPERATOR_CONTROLLER.numpad2(), - SET_TARGET_SCORING_REEF_LEVEL_L4_TRIGGER = OPERATOR_CONTROLLER.numpad3(), + SET_TARGET_SCORING_REEF_LEVEL_L1_FROM_CORAL_INTAKE_TRIGGER = OPERATOR_CONTROLLER.numpad0().or(DRIVER_CONTROLLER.povDown()), + SET_TARGET_SCORING_REEF_LEVEL_L2_TRIGGER = OPERATOR_CONTROLLER.numpad1().or(DRIVER_CONTROLLER.povRight()), + SET_TARGET_SCORING_REEF_LEVEL_L3_TRIGGER = OPERATOR_CONTROLLER.numpad2().or(DRIVER_CONTROLLER.povLeft()), + SET_TARGET_SCORING_REEF_LEVEL_L4_TRIGGER = OPERATOR_CONTROLLER.numpad3().or(DRIVER_CONTROLLER.povUp()), SET_TARGET_SCORING_REEF_CLOCK_POSITION_2_OCLOCK_TRIGGER = OPERATOR_CONTROLLER.numpad9(), SET_TARGET_SCORING_REEF_CLOCK_POSITION_4_OCLOCK_TRIGGER = OPERATOR_CONTROLLER.numpad6(), SET_TARGET_SCORING_REEF_CLOCK_POSITION_6_OCLOCK_TRIGGER = OPERATOR_CONTROLLER.numpad5(), SET_TARGET_SCORING_REEF_CLOCK_POSITION_8_OCLOCK_TRIGGER = OPERATOR_CONTROLLER.numpad4(), SET_TARGET_SCORING_REEF_CLOCK_POSITION_10_OCLOCK_TRIGGER = OPERATOR_CONTROLLER.numpad7(), SET_TARGET_SCORING_REEF_CLOCK_POSITION_12_OCLOCK_TRIGGER = OPERATOR_CONTROLLER.numpad8(), - SET_TARGET_REEF_SCORING_SIDE_LEFT_TRIGGER = OPERATOR_CONTROLLER.left().or(DRIVER_CONTROLLER.povLeft()), - SET_TARGET_REEF_SCORING_SIDE_RIGHT_TRIGGER = OPERATOR_CONTROLLER.right().or(DRIVER_CONTROLLER.povRight()), - ENABLE_IGNORE_LOLLIPOP_CORAL_TRIGGER = OPERATOR_CONTROLLER.o(), - DISABLE_IGNORE_LOLLIPOP_CORAL_TRIGGER = OPERATOR_CONTROLLER.p(); + SET_TARGET_REEF_SCORING_SIDE_LEFT_TRIGGER = OPERATOR_CONTROLLER.left(), + SET_TARGET_REEF_SCORING_SIDE_RIGHT_TRIGGER = OPERATOR_CONTROLLER.right(); + + private static Trigger createScoreTrigger(Trigger button, boolean isRight) { + if (isRight) + return button + .and(() -> !IS_LEFT_SCORE_BUTTON_PRESSED) + .onTrue(new InstantCommand(() -> IS_RIGHT_SCORE_BUTTON_PRESSED = true)) + .onFalse(new InstantCommand(() -> IS_RIGHT_SCORE_BUTTON_PRESSED = false)); + return button + .and(() -> !IS_RIGHT_SCORE_BUTTON_PRESSED) + .onTrue(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = true)) + .onFalse(new InstantCommand(() -> IS_LEFT_SCORE_BUTTON_PRESSED = false)); + } } \ 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 cd7c5f14..fa5626f5 100644 --- a/src/main/java/frc/trigon/robot/misc/ReefChooser.java +++ b/src/main/java/frc/trigon/robot/misc/ReefChooser.java @@ -65,8 +65,6 @@ private void configureScoringLevelBindings() { OperatorConstants.SET_TARGET_SCORING_REEF_LEVEL_L2_TRIGGER.onTrue(getSetTargetLevelCommand(() -> CoralPlacingCommands.ScoringLevel.L2)); OperatorConstants.SET_TARGET_SCORING_REEF_LEVEL_L3_TRIGGER.onTrue(getSetTargetLevelCommand(() -> CoralPlacingCommands.ScoringLevel.L3)); OperatorConstants.SET_TARGET_SCORING_REEF_LEVEL_L4_TRIGGER.onTrue(getSetTargetLevelCommand(() -> CoralPlacingCommands.ScoringLevel.L4)); - OperatorConstants.INCREMENT_TARGET_SCORING_LEVEL_TRIGGER.onTrue(getSetTargetLevelCommand(this::getIncrementedScoringLevel)); - OperatorConstants.DECREMENT_TARGET_SCORING_LEVEL_TRIGGER.onTrue(getSetTargetLevelCommand(this::getDecrementedScoringLevel)); } private Command getSetTargetLevelCommand(Supplier scoringLevel) { 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 e7ee4c25..2d3c8228 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectDetectionCamera.java @@ -99,7 +99,6 @@ private Translation2d calculateObjectPositionFromRotation(Rotation3d objectRotat if (robotPoseAtResultTimestamp == null) return new Translation2d(); final Pose3d cameraPose = new Pose3d(robotPoseAtResultTimestamp).plus(robotCenterToCamera); - objectRotation = new Rotation3d(objectRotation.getX(), -objectRotation.getY(), objectRotation.getZ()); final Pose3d objectRotationStart = cameraPose.plus(new Transform3d(0, 0, 0, objectRotation)); final double cameraZ = cameraPose.getTranslation().getZ(); diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java index b7532202..64ea2cfe 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/ObjectPoseEstimator.java @@ -7,40 +7,44 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; +import org.littletonrobotics.junction.Logger; import java.util.ArrayList; import java.util.HashMap; public class ObjectPoseEstimator extends SubsystemBase { private final HashMap knownObjectPositions; - private final ObjectDetectionCamera[] cameras; + private final ObjectDetectionCamera camera; private final double deletionThresholdSeconds; private final SimulatedGamePieceConstants.GamePieceType gamePieceType; private final double rotationToTranslation; /** - * Constructs an ObjectPoseEstimator for estimating the positions of objects detected by cameras. + * Constructs an ObjectPoseEstimator for estimating the positions of objects detected by camera. * * @param deletionThresholdSeconds the time in seconds after which an object is considered old and removed * @param gamePieceType the type of game piece to track - * @param cameras the cameras used for detecting objects + * @param camera the camera used for detecting objects */ - public ObjectPoseEstimator(double deletionThresholdSeconds, DistanceCalculationMethod distanceCalculationMethod, SimulatedGamePieceConstants.GamePieceType gamePieceType, ObjectDetectionCamera... cameras) { + public ObjectPoseEstimator(double deletionThresholdSeconds, DistanceCalculationMethod distanceCalculationMethod, + SimulatedGamePieceConstants.GamePieceType gamePieceType, + ObjectDetectionCamera camera) { this.deletionThresholdSeconds = deletionThresholdSeconds; this.gamePieceType = gamePieceType; - this.cameras = cameras; + this.camera = camera; this.knownObjectPositions = new HashMap<>(); this.rotationToTranslation = distanceCalculationMethod.rotationToTranslation; } /** - * Updates the object positions based on the cameras detected objects. + * Updates the object positions based on the camera detected objects. * Removes objects that have not been detected for {@link ObjectPoseEstimator#deletionThresholdSeconds}. */ @Override public void periodic() { updateObjectPositions(); removeOldObjects(); + Logger.recordOutput("ObjectPoseEstimator/knownObjectPositions", getObjectsOnField().toArray(Translation2d[]::new)); } /** @@ -57,7 +61,7 @@ public ArrayList getObjectsOnField() { */ public void removeClosestObjectToRobot() { final Translation2d closestObject = getClosestObjectToRobot(); - knownObjectPositions.remove(closestObject); + removeObject(closestObject); } /** @@ -67,7 +71,7 @@ public void removeClosestObjectToRobot() { */ public void removeClosestObjectToIntake(Transform2d intakeTransform) { final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose(); - knownObjectPositions.remove(getClosestObjectToPose(robotPose.transformBy(intakeTransform))); + removeObject(getClosestObjectToPosition(robotPose.transformBy(intakeTransform).getTranslation())); } /** @@ -76,8 +80,13 @@ public void removeClosestObjectToIntake(Transform2d intakeTransform) { * @param fieldRelativePose the pose to which the removed object is closest */ public void removeClosestObjectToPose(Pose2d fieldRelativePose) { - final Translation2d closestObject = getClosestObjectToPose(fieldRelativePose); - knownObjectPositions.remove(closestObject); + final Translation2d closestObject = getClosestObjectToPosition(fieldRelativePose.getTranslation()); + removeObject(closestObject); + } + + public void removeClosestObjectToPosition(Translation2d position) { + final Translation2d closestObject = getClosestObjectToPosition(position); + removeObject(closestObject); } /** @@ -90,7 +99,7 @@ public void removeObject(Translation2d objectPosition) { } /** - * Gets the position of the closest object on the field from the 3D rotation of the object relative to the cameras. + * Gets the position of the closest object on the field from the 3D rotation of the object relative to the camera. * This assumes the object is on the ground. * Once it is known that the object is on the ground, * one can simply find the transform from the camera to the ground and apply it to the object's rotation. @@ -98,27 +107,30 @@ public void removeObject(Translation2d objectPosition) { * @return the best object's 2D position on the field (z is assumed to be 0) */ public Translation2d getClosestObjectToRobot() { - return getClosestObjectToPose(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose()); + return getClosestObjectToPosition(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation()); } + /** - * Gets the position of the closest object to a given pose on the field. + * Gets the position of the closest object to a given position on the field. * - * @param pose the pose to which the returned object is closest + * @param position the position to which the returned object is closest * @return the closest object's position on the field, or null if no objects are known */ - public Translation2d getClosestObjectToPose(Pose2d pose) { + public Translation2d getClosestObjectToPosition(Translation2d position) { final Translation2d[] objectsTranslations = knownObjectPositions.keySet().toArray(Translation2d[]::new); if (knownObjectPositions.isEmpty()) return null; Translation2d bestObjectTranslation = objectsTranslations[0]; + double closestObjectDistance = position.getDistance(bestObjectTranslation); for (int i = 1; i < objectsTranslations.length; i++) { final Translation2d currentObjectTranslation = objectsTranslations[i]; - final double bestObjectRating = calculateObjectDistanceRating(bestObjectTranslation, pose); - final double currentObjectRating = calculateObjectDistanceRating(currentObjectTranslation, pose); - if (currentObjectRating < bestObjectRating) + final double currentObjectDistance = position.getDistance(currentObjectTranslation); + if (currentObjectDistance < closestObjectDistance) { + closestObjectDistance = currentObjectDistance; bestObjectTranslation = currentObjectTranslation; + } } return bestObjectTranslation; } @@ -142,23 +154,21 @@ private double calculateObjectDistanceRating(Translation2d objectTranslation, Po private void updateObjectPositions() { final double currentTimestamp = Timer.getTimestamp(); - for (ObjectDetectionCamera camera : this.cameras) { - for (Translation2d visibleObject : camera.getObjectPositionsOnField(gamePieceType)) { - Translation2d closestObjectToVisibleObject = new Translation2d(); - double closestObjectToVisibleObjectDistanceMeters = Double.POSITIVE_INFINITY; - - for (Translation2d knownObject : knownObjectPositions.keySet()) { - final double currentObjectDistanceMeters = visibleObject.getDistance(knownObject); - if (currentObjectDistanceMeters < closestObjectToVisibleObjectDistanceMeters) { - closestObjectToVisibleObjectDistanceMeters = currentObjectDistanceMeters; - closestObjectToVisibleObject = knownObject; - } + for (Translation2d visibleObject : camera.getObjectPositionsOnField(gamePieceType)) { + Translation2d closestObjectToVisibleObject = new Translation2d(); + double closestObjectToVisibleObjectDistanceMeters = Double.POSITIVE_INFINITY; + + for (Translation2d knownObject : knownObjectPositions.keySet()) { + final double currentObjectDistanceMeters = visibleObject.getDistance(knownObject); + if (currentObjectDistanceMeters < closestObjectToVisibleObjectDistanceMeters) { + closestObjectToVisibleObjectDistanceMeters = currentObjectDistanceMeters; + closestObjectToVisibleObject = knownObject; } - - if (closestObjectToVisibleObjectDistanceMeters < ObjectDetectionCameraConstants.TRACKED_OBJECT_TOLERANCE_METERS && knownObjectPositions.get(closestObjectToVisibleObject) != currentTimestamp) - knownObjectPositions.remove(closestObjectToVisibleObject); - knownObjectPositions.put(visibleObject, currentTimestamp); } + + if (closestObjectToVisibleObjectDistanceMeters < ObjectDetectionCameraConstants.TRACKED_OBJECT_TOLERANCE_METERS && knownObjectPositions.get(closestObjectToVisibleObject) != currentTimestamp) + removeObject(closestObjectToVisibleObject); + knownObjectPositions.put(visibleObject, currentTimestamp); } } diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java index ad554e2f..541f10e8 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/PhotonObjectDetectionCameraIO.java @@ -1,19 +1,13 @@ package frc.trigon.robot.misc.objectdetectioncamera.io; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraConstants; import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraIO; import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCameraInputsAutoLogged; -import org.littletonrobotics.junction.Logger; -import org.opencv.core.Point; import org.photonvision.PhotonCamera; import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; -import org.photonvision.targeting.TargetCorner; import java.util.ArrayList; import java.util.Arrays; @@ -82,71 +76,10 @@ private Rotation3d[] toArray(List list) { } private Rotation3d extractRotation3d(PhotonTrackedTarget target) { -// final List detectedCorners = target.getMinAreaRectCorners(); -// -// if (detectedCorners.size() != 4) - return calculateMiddleRotation(target); - -// final Point objectPointOnCamera = findAverageOfLowestTwo(detectedCorners); -// final Rotation3d objectRotationOnCamera = getCorrectedPixelRot(objectPointOnCamera); -// -// if (objectRotationOnCamera == null) -// calculateMiddleRotation(target); -// -// return objectRotationOnCamera; - } - - private Rotation3d calculateMiddleRotation(PhotonTrackedTarget target) { return new Rotation3d( 0, - Units.degreesToRadians(target.getPitch()), + Units.degreesToRadians(-target.getPitch()), Units.degreesToRadians(-target.getYaw()) ); } - - private Point findAverageOfLowestTwo(List corners) { - TargetCorner lowest = corners.get(0); - TargetCorner secondLowest = corners.get(1); - - if (secondLowest.y > lowest.y) { - TargetCorner temp = lowest; - lowest = secondLowest; - secondLowest = temp; - } - - for (int i = 2; i < 4; i++) { - final TargetCorner current = corners.get(i); - - if (current.y > lowest.y) { - secondLowest = lowest; - lowest = current; - } else if (current.y > secondLowest.y) { - secondLowest = current; - } - } - - final double averageX = (lowest.x + secondLowest.x) / 2.0; - final double averageY = (lowest.y + secondLowest.y) / 2.0; - - return new Point(averageX, averageY); - } - - private Rotation3d getCorrectedPixelRot(Point point) { - final Matrix cameraIntrinsics = photonCamera.getCameraMatrix().orElse(null); - if (cameraIntrinsics == null) - return null; - - final double fx = cameraIntrinsics.get(0, 0); - final double cx = cameraIntrinsics.get(0, 2); - final double xOffset = cx - point.x; - final double fy = cameraIntrinsics.get(1, 1); - final double cy = cameraIntrinsics.get(1, 2); - final double yOffset = cy - point.y; - - final Rotation2d yaw = new Rotation2d(fx, xOffset); - final Rotation2d pitch = new Rotation2d(fy / Math.cos(Math.atan(xOffset / fx)), -yOffset); - Logger.recordOutput("Yaw", yaw.getDegrees()); - Logger.recordOutput("Pitch", -pitch.getDegrees()); - return new Rotation3d(0.0, -pitch.getRadians(), yaw.getRadians()); - } } diff --git a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java index 4c524b2a..3905b409 100644 --- a/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java +++ b/src/main/java/frc/trigon/robot/misc/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java @@ -103,15 +103,8 @@ private Rotation3d calculateCameraAngleToObject(Pose3d objectPose, Pose3d camera final Translation3d difference = cameraPosition.minus(objectPosition); final Rotation3d differenceAsAngle = getAngle(difference); - return minus(differenceAsAngle, cameraPose.getRotation()); - } - private Rotation3d minus(Rotation3d rotation, Rotation3d other) { - return new Rotation3d( - 0, - rotation.getY() + other.getY(), - rotation.getZ() - other.getZ() - ); + return differenceAsAngle.minus(cameraPose.getRotation()); } private Rotation3d getAngle(Translation3d translation) { @@ -135,7 +128,7 @@ private Rotation2d getYaw(Translation3d vector) { * @return the pitch of the vector */ private Rotation2d getPitch(Translation3d vector) { - return new Rotation2d(-Math.atan2(vector.getZ(), Math.hypot(vector.getX(), vector.getY()))); + return new Rotation2d(Math.atan2(vector.getZ(), Math.hypot(vector.getX(), vector.getY()))); } /** @@ -152,7 +145,7 @@ private boolean isWithinFOV(Rotation3d objectRotation) { private void logVisibleGamePieces(ArrayList>[] visibleGamePieces) { for (int i = 0; i < visibleGamePieces.length; i++) { final String gamePieceTypeName = SimulatedGamePieceConstants.GamePieceType.getNameFromID(i); - Logger.recordOutput(hostname + "/Visible" + gamePieceTypeName, mapSimulatedGamePieceListToPoseArray(visibleGamePieces[i])); + Logger.recordOutput(hostname + "/Visible" + gamePieceTypeName + "poses", mapSimulatedGamePieceListToPoseArray(visibleGamePieces[i])); } } 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 bc3bb55a..8968e145 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePieceConstants.java @@ -14,7 +14,7 @@ public class SimulatedGamePieceConstants { public static final double CORAL_INTAKE_TOLERANCE_METERS = 0.3, CORAL_FEEDER_INTAKE_TOLERANCE_METERS = 1, - ALGAE_INTAKE_TOLERANCE_METERS = 0.4, + ALGAE_INTAKE_TOLERANCE_METERS = 0.3, CORAL_SCORING_TOLERANCE_METERS = 0.1, ALGAE_SCORING_TOLERANCE_METERS = 0.2; @@ -118,7 +118,7 @@ private static FlippablePose3d calculateCoralScorePose(int level, int clockFace, } public enum GamePieceType { - ALGAE(0.15, 0), + ALGAE(0.2, 0), CORAL(0.06, 1); public final double originPointHeightOffGroundMeters; 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 bc3a2168..484cc96b 100644 --- a/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java +++ b/src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java @@ -19,7 +19,7 @@ public class SimulationFieldHandler { CORAL_ON_FIELD = SimulatedGamePieceConstants.CORAL_ON_FIELD, ALGAE_ON_FIELD = SimulatedGamePieceConstants.ALGAE_ON_FIELD; private static Integer - HELD_CORAL_INDEX = 0, + HELD_CORAL_INDEX = null, HELD_ALGAE_INDEX = null; private static boolean IS_CORAL_IN_GRIPPER = true; @@ -78,7 +78,7 @@ private static void updateCollection() { final Pose3d robotPose = new Pose3d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose()); final Pose3d coralCollectionPose = robotPose.plus(toTransform(RobotContainer.CORAL_INTAKE.calculateCoralCollectionPose())), - algaeCollectionPose = robotPose.plus(toTransform(new Pose3d())); + algaeCollectionPose = robotPose.plus(toTransform(RobotContainer.GRIPPER.calculateAlgaeCollectionPose())); updateCoralFeederCollection(robotPose); @@ -131,7 +131,7 @@ private static boolean isCollectingCoralFromFeeder() { } private static boolean isCollectingAlgae() { - return false;//TODO: Implement + return RobotContainer.ALGAE_MANIPULATOR.isOpen(); } private static void updateEjection() { @@ -140,7 +140,7 @@ private static void updateEjection() { if (HELD_ALGAE_INDEX != null && isEjectingAlgae()) { final SimulatedGamePiece heldAlgae = ALGAE_ON_FIELD.get(HELD_ALGAE_INDEX); - heldAlgae.release(); + ejectAlgae(heldAlgae); HELD_ALGAE_INDEX = null; } } @@ -157,6 +157,14 @@ private static void updateCoralEjection() { ejectCoralFromGripper(heldCoral); } + private static void ejectAlgae(SimulatedGamePiece ejectedAlgae) { + final ChassisSpeeds swerveWheelSpeeds = RobotContainer.SWERVE.getSelfRelativeVelocity(); + final Translation3d robotSelfRelativeVelocity = new Translation3d(swerveWheelSpeeds.vxMetersPerSecond, swerveWheelSpeeds.vyMetersPerSecond, 0); + final Translation3d robotRelativeReleaseVelocity = RobotContainer.GRIPPER.calculateAlgaeReleaseVelocity(); + + ejectedAlgae.release(robotSelfRelativeVelocity.plus(robotRelativeReleaseVelocity).rotateBy(new Rotation3d(RobotContainer.SWERVE.getHeading()))); + } + private static void ejectCoralFromGripper(SimulatedGamePiece ejectedCoral) { final Pose3d robotPose = new Pose3d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose()); final Pose3d robotRelativeGripperReleasePose = RobotContainer.GRIPPER.calculateCoralReleasePoint(); @@ -180,7 +188,7 @@ private static boolean isIntakeEjectingCoral() { } private static boolean isEjectingAlgae() { - return false;//TODO: Implement + return RobotContainer.GRIPPER.isReleasingAlgae() || !RobotContainer.ALGAE_MANIPULATOR.isOpen(); } /** @@ -189,7 +197,7 @@ private static boolean isEjectingAlgae() { private static void updateHeldGamePiecePoses() { final Pose3d robotRelativeHeldCoralPosition = IS_CORAL_IN_GRIPPER ? RobotContainer.GRIPPER.calculateHeldCoralVisualizationPose() : RobotContainer.CORAL_INTAKE.calculateCollectedCoralPose(), - robotRelativeHeldAlgaePosition = new Pose3d(); + robotRelativeHeldAlgaePosition = RobotContainer.GRIPPER.calculateAlgaeCollectionPose(); updateHeldGamePiecePose(robotRelativeHeldCoralPosition, CORAL_ON_FIELD, HELD_CORAL_INDEX); updateHeldGamePiecePose(robotRelativeHeldAlgaePosition, ALGAE_ON_FIELD, HELD_ALGAE_INDEX); } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index af77ecf6..1a8eadaf 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -63,7 +63,9 @@ public double getLatestResultTimestampSeconds() { } public boolean hasValidResult() { - return inputs.hasResult && inputs.poseAmbiguity < AprilTagCameraConstants.MAXIMUM_AMBIGUITY && inputs.distancesFromTags[0] < 5; + return inputs.hasResult && + inputs.poseAmbiguity < AprilTagCameraConstants.MAXIMUM_AMBIGUITY && + inputs.distancesFromTags[0] < AprilTagCameraConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_VALID_RESULT_METERS; } /** diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index 29ff6837..1e87d2c5 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -15,7 +15,9 @@ import java.util.function.BiFunction; public class AprilTagCameraConstants { - static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_ACCURATE_SOLVE_PNP_RESULT_METERS = 2; + static final double + MAXIMUM_DISTANCE_FROM_TAG_FOR_ACCURATE_SOLVE_PNP_RESULT_METERS = 2, + MAXIMUM_DISTANCE_FROM_TAG_FOR_VALID_RESULT_METERS = 5; static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; static final double MAXIMUM_AMBIGUITY = 0.4; public static final PhotonPoseEstimator.ConstrainedSolvepnpParams CONSTRAINED_SOLVE_PNP_PARAMS = new PhotonPoseEstimator.ConstrainedSolvepnpParams(false, 0.1); diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index a7848d36..3d3bdccc 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -79,15 +79,6 @@ private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, Photon inputs.visibleTagIDs = getVisibleTagIDs(visibleNotHatefulTags); inputs.poseAmbiguity = latestResult.getMultiTagResult().isPresent() ? 0 : bestTag.getPoseAmbiguity(); inputs.distancesFromTags = getDistancesFromTags(visibleNotHatefulTags); - inputs.hasConstrainedResult = false; - } - - private PhotonTrackedTarget getBestTag(PhotonPipelineResult result) { - for (PhotonTrackedTarget tag : result.getTargets()) { - if (FieldConstants.TAG_ID_TO_POSE.containsKey(tag.getFiducialId())) - return tag; - } - return null; } private void updateSolvePNPPoses(AprilTagCameraInputsAutoLogged inputs, PhotonPipelineResult latestResult, PhotonTrackedTarget bestTag) { @@ -111,6 +102,7 @@ private void updateSolvePNPPoses(AprilTagCameraInputsAutoLogged inputs, PhotonPi inputs.alternateCameraSolvePNPPose = tagPose.transformBy(alternateTargetToCamera); // updateConstrainedSolvePNPPose(inputs, latestResult); + inputs.hasConstrainedResult = false; } private void updateConstrainedSolvePNPPose(AprilTagCameraInputsAutoLogged inputs, PhotonPipelineResult latestResult) { @@ -172,20 +164,20 @@ private PhotonTrackedTarget[] getVisibleNotHatefulTags(PhotonPipelineResult resu return Arrays.copyOf(visibleTagIDs, index); } - private int[] getVisibleTagIDs(PhotonTrackedTarget[] targets) { - final int[] visibleTagIDs = new int[targets.length]; + private int[] getVisibleTagIDs(PhotonTrackedTarget[] tags) { + final int[] visibleTagIDs = new int[tags.length]; for (int i = 0; i < visibleTagIDs.length; i++) - visibleTagIDs[i] = targets[i].getFiducialId(); + visibleTagIDs[i] = tags[i].getFiducialId(); return visibleTagIDs; } - private double[] getDistancesFromTags(PhotonTrackedTarget[] targets) { - final double[] distances = new double[targets.length]; + private double[] getDistancesFromTags(PhotonTrackedTarget[] tags) { + final double[] distances = new double[tags.length]; - for (int i = 0; i < targets.length; i++) - distances[i] = getDistanceFromTarget(targets[i]); + for (int i = 0; i < tags.length; i++) + distances[i] = getDistanceFromTarget(tags[i]); return distances; } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java index c68ef0e0..4bc24b30 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagSimulationCameraIO.java @@ -5,7 +5,6 @@ import org.photonvision.simulation.PhotonCameraSim; public class AprilTagSimulationCameraIO extends AprilTagPhotonCameraIO { - public AprilTagSimulationCameraIO(String cameraName, Transform3d robotToCamera) { super(cameraName, robotToCamera); diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/RobotPoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/RobotPoseEstimator.java index a8563ef1..25b55e81 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/RobotPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/RobotPoseEstimator.java @@ -93,7 +93,7 @@ public void resetHeading() { public void resetPose(Pose2d newPose) { RobotContainer.SWERVE.setHeading(newPose.getRotation()); - swerveDrivePoseEstimator.resetPose(newPose); // TODO: Might not work as intended + swerveDrivePoseEstimator.resetPose(newPose); swerveDriveOdometry.resetPose(newPose); if (shouldUseRelativeRobotPoseSource) relativeRobotPoseSource.resetOffset(newPose); diff --git a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java index 228cdd97..e57911dc 100644 --- a/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java +++ b/src/main/java/frc/trigon/robot/subsystems/MotorSubsystem.java @@ -27,8 +27,7 @@ public abstract class MotorSubsystem extends edu.wpi.first.wpilibj2.command.Subs private static final List REGISTERED_SUBSYSTEMS = new ArrayList<>(); private static final Trigger DISABLED_TRIGGER = new Trigger(DriverStation::isDisabled); private static final Executor BRAKE_MODE_EXECUTOR = Executors.newFixedThreadPool(8); - // private static final LoggedNetworkBoolean ENABLE_EXTENSIVE_LOGGING = new LoggedNetworkBoolean("/SmartDashboard/EnableExtensiveLogging", !DriverStation.isFMSAttached()); - private static final LoggedNetworkBoolean ENABLE_EXTENSIVE_LOGGING = new LoggedNetworkBoolean("/SmartDashboard/EnableExtensiveLogging", false); + private static final LoggedNetworkBoolean ENABLE_EXTENSIVE_LOGGING = new LoggedNetworkBoolean("/SmartDashboard/EnableExtensiveLogging", RobotHardwareStats.isSimulation()); static { DISABLED_TRIGGER.onTrue(new InstantCommand(() -> forEach(MotorSubsystem::stop)).ignoringDisable(true)); @@ -61,7 +60,6 @@ public static void forEach(Consumer toRun) { * @param brake whether the motors should brake or coast */ public static void setAllSubsystemsBrakeAsync(boolean brake) { -// CompletableFuture.runAsync(() -> forEach((subsystem) -> subsystem.setBrake(brake))); BRAKE_MODE_EXECUTOR.execute(() -> forEach((subsystem) -> subsystem.setBrake(brake))); } diff --git a/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulator.java b/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulator.java index 26658a79..f82221cb 100644 --- a/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulator.java +++ b/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulator.java @@ -3,12 +3,14 @@ import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.controls.TorqueCurrentFOC; import com.ctre.phoenix6.controls.VoltageOut; -import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.*; import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.trigon.robot.RobotContainer; import frc.trigon.robot.subsystems.MotorSubsystem; import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; import org.trigon.hardware.phoenix6.talonfx.TalonFXMotor; import org.trigon.hardware.phoenix6.talonfx.TalonFXSignal; import org.trigon.utilities.Conversions; @@ -58,6 +60,8 @@ public void updateMechanism() { getCurrentAngle(), Rotation2d.fromRotations(angleMotor.getSignal(TalonFXSignal.CLOSED_LOOP_REFERENCE)) ); + + Logger.recordOutput("Poses/Components/AlgaeManipulatorPose", calculateVisualizationPose()); } @Override @@ -69,6 +73,10 @@ public boolean isOpen() { return angleMotor.getSignal(TalonFXSignal.POSITION) > Conversions.degreesToRotations(70); } + public void resetPosition() { + angleMotor.setPosition(0); + } + public boolean atState(AlgaeManipulatorConstants.AlgaeManipulatorState state) { return targetState == state && atTargetAngle(); } @@ -100,6 +108,17 @@ void setTargetAngle(Rotation2d targetAngle) { angleMotor.setControl(positionRequest.withPosition(targetAngle.getRotations())); } + private Pose3d calculateVisualizationPose() { + final Pose3d currentGripperPose = RobotContainer.GRIPPER.calculateVisualizationPose(); + final Pose3d gripperOrigin = currentGripperPose.transformBy(AlgaeManipulatorConstants.GRIPPER_TO_ALGAE_MANIPULATOR); + + final Transform3d pitchTransform = new Transform3d( + new Translation3d(0, 0, 0), + new Rotation3d(0, getCurrentAngle().getRadians(), 0) + ); + return gripperOrigin.transformBy(pitchTransform); + } + private Rotation2d getCurrentAngle() { return Rotation2d.fromRotations(angleMotor.getSignal(TalonFXSignal.POSITION)); } diff --git a/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulatorCommands.java index 08ec973d..ef787b9a 100644 --- a/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulatorCommands.java @@ -1,10 +1,10 @@ package frc.trigon.robot.subsystems.algaemanipulator; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.StartEndCommand; +import edu.wpi.first.wpilibj2.command.*; import frc.trigon.robot.RobotContainer; import frc.trigon.robot.commands.commandfactories.GeneralCommands; +import frc.trigon.robot.constants.OperatorConstants; import org.trigon.commands.NetworkTablesCommand; import java.util.Set; @@ -45,7 +45,10 @@ public static Command getPressLimitSwitchCommand() { RobotContainer.ALGAE_MANIPULATOR::closeToLimit, RobotContainer.ALGAE_MANIPULATOR::stop, RobotContainer.ALGAE_MANIPULATOR - ).until(RobotContainer.ALGAE_MANIPULATOR::hasHitReverseLimit); + ).raceWith( + new WaitUntilCommand(RobotContainer.ALGAE_MANIPULATOR::hasHitReverseLimit), + new WaitUntilCommand(OperatorConstants.CONTINUE_TRIGGER).andThen(new InstantCommand(RobotContainer.ALGAE_MANIPULATOR::resetPosition)) + ); } public static Command getSetTargetStateCommand(AlgaeManipulatorConstants.AlgaeManipulatorState targetState) { diff --git a/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulatorConstants.java index 5a749685..d020fc55 100644 --- a/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/algaemanipulator/AlgaeManipulatorConstants.java @@ -2,7 +2,7 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.*; -import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.util.Color; @@ -47,6 +47,16 @@ public class AlgaeManipulatorConstants { Units.Second.of(1000) ); + static final Pose3d ALGAE_MANIPULATOR_VISUALIZATION_ORIGIN_POINT = new Pose3d( + new Translation3d(0, 0, 0), + new Rotation3d(0, edu.wpi.first.math.util.Units.degreesToRadians(0), 0) + ); + static final Transform3d + GRIPPER_TO_ALGAE_MANIPULATOR = new Transform3d( + GripperConstants.GRIPPER_VISUALIZATION_ORIGIN_POINT, + ALGAE_MANIPULATOR_VISUALIZATION_ORIGIN_POINT + ); + static final SingleJointedArmMechanism2d ANGLE_MECHANISM = new SingleJointedArmMechanism2d( "AlgaeManipulatorAngleMechanism", ARM_LENGTH_METERS, @@ -57,8 +67,8 @@ public class AlgaeManipulatorConstants { static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(1); static final Rotation2d MAXIMUM_RESTING_GRIPPER_ANGLE = Rotation2d.fromDegrees(0); static final double - OPEN_TORQUE_CURRENT = 20, - CLOSE_TO_LIMIT_TORQUE_CURRENT = -20; + OPEN_TORQUE_CURRENT = 17, + CLOSE_TO_LIMIT_TORQUE_CURRENT = -18; static { final TalonFXConfiguration config = new TalonFXConfiguration(); diff --git a/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntake.java b/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntake.java index 506169c7..38b9694b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntake.java +++ b/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntake.java @@ -6,7 +6,6 @@ import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.commands.commandfactories.CoralCollectionCommands; @@ -122,6 +121,11 @@ public boolean isEarlyCoralCollectionDetected() { return CoralIntakeConstants.EARLY_CORAL_COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); } + @AutoLogOutput + public boolean isMovingSlowly() { + return Math.abs(intakeMotor.getSignal(TalonFXSignal.VELOCITY)) < 4; + } + /** * Calculates the pose where the coral should actually be collected from relative to the robot. * @@ -230,16 +234,14 @@ private Rotation2d getCurrentEncoderAngle() { return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION) + CoralIntakeConstants.ANGLE_ENCODER_POSITION_OFFSET_VALUE); } - /** - * Logs the current match time and target reef placement for the dashboard. - * We use {@link SmartDashboard} instead of {@link Logger} because the {@link Logger} inputs don't show up in QDashboard for some reason. - */ private void logToDashboard() { Logger.recordOutput("GameTime", DriverStation.getMatchTime()); Logger.recordOutput("TargetCoralPlacementStates/IsTargetSideLeft", !OperatorConstants.REEF_CHOOSER.getReefSide().doesFlipYTransformWhenFacingDriverStation); Logger.recordOutput("TargetCoralPlacementStates/TargetLevel", OperatorConstants.REEF_CHOOSER.getScoringLevel().level); Logger.recordOutput("TargetCoralPlacementStates/TargetClockPositionForElastic", getClockPositionForElastic()); Logger.recordOutput("ShouldIgnoreLollipopCoral", CoralCollectionCommands.SHOULD_IGNORE_LOLLIPOP_CORAL); + Logger.recordOutput("ShouldAssistIntake", CoralCollectionCommands.SHOULD_ASSIST_INTAKE); + Logger.recordOutput("ShouldKeepIntakeOpen", CoralCollectionCommands.SHOULD_KEEP_INTAKE_OPEN); Logger.recordOutput("ShouldScoreCoralAutonomously", CoralPlacingCommands.SHOULD_SCORE_AUTONOMOUSLY); } diff --git a/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeCommands.java b/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeCommands.java index 74ac0ae0..754bd625 100644 --- a/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeCommands.java @@ -5,6 +5,10 @@ import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.commandfactories.CoralCollectionCommands; +import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands; +import frc.trigon.robot.commands.commandfactories.GeneralCommands; +import frc.trigon.robot.constants.OperatorConstants; import org.trigon.commands.GearRatioCalculationCommand; import org.trigon.commands.NetworkTablesCommand; @@ -35,6 +39,14 @@ public static Command getGearRatioCalculationCommand() { ); } + public static Command getCoralIntakeDefaultCommand() { + return GeneralCommands.getContinuousConditionalCommand( + getPrepareForStateCommand(CoralIntakeConstants.CoralIntakeState.COLLECT_FROM_FLOOR), + getSetTargetStateCommand(CoralIntakeConstants.CoralIntakeState.REST), + () -> CoralCollectionCommands.SHOULD_KEEP_INTAKE_OPEN && !OperatorConstants.REEF_CHOOSER.getScoringLevel().equals(CoralPlacingCommands.ScoringLevel.L1_CORAL_INTAKE) + ); + } + public static Command getCenterCoralWithPulsingCommand() { return new FunctionalCommand( RobotContainer.CORAL_INTAKE::initializePulsing, diff --git a/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeConstants.java b/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeConstants.java index 1d370fd6..e4b7be7e 100644 --- a/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/coralintake/CoralIntakeConstants.java @@ -172,6 +172,7 @@ public class CoralIntakeConstants { configureAngleMotor(); configureAngleEncoder(); configureBeamBreak(); + configureBackupBeamBreak(); configureDistanceSensor(); } @@ -189,6 +190,7 @@ private static void configureIntakeMotor() { INTAKE_MOTOR.setPhysicsSimulation(INTAKE_SIMULATION); INTAKE_MOTOR.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); + INTAKE_MOTOR.registerSignal(TalonFXSignal.VELOCITY, 100); INTAKE_MOTOR.registerSignal(TalonFXSignal.STATOR_CURRENT, 100); } @@ -277,6 +279,10 @@ private static void configureBeamBreak() { BEAM_BREAK.setSimulationSupplier(BEAM_BREAK_SIMULATION_VALUE_SUPPLIER); } + private static void configureBackupBeamBreak() { + BACKUP_BEAM_BREAK.setSimulationSupplier(BEAM_BREAK_SIMULATION_VALUE_SUPPLIER); + } + private static void configureDistanceSensor() { DISTANCE_SENSOR.setScalingConstants(DISTANCE_SENSOR_SCALING_SLOPE, DISTANCE_SENSOR_SCALING_INTERCEPT_POINT); DISTANCE_SENSOR.setSimulationSupplier(DISTANCE_SENSOR_SIMULATION_VALUE_SUPPLIER); @@ -293,7 +299,10 @@ public enum CoralIntakeState { REST(0, 0, LOAD_CORAL_TO_GRIPPER_SEEING_GAME_PIECE_WITH_BEAM_BREAK.targetAngle), SCORE_L1_BOOST(-3, -3, Rotation2d.fromDegrees(33)), SCORE_L1(-2, 3, Rotation2d.fromDegrees(33)), - COLLECT_ALGAE_FROM_FLOOR(-6, 0, Rotation2d.fromDegrees(-40)); + ROLL_ALGAE_ON_FLOOR(-2, 0, Rotation2d.fromDegrees(-45)), + COLLECT_ALGAE_FROM_FLOOR(5, 0, Rotation2d.fromDegrees(-15)), + PREPARE_SCORE_ALGAE_IN_PROCESSOR(5, 0, Rotation2d.fromDegrees(-15)), + SCORE_ALGAE_IN_PROCESSOR(-2, 0, PREPARE_SCORE_ALGAE_IN_PROCESSOR.targetAngle); public final double targetIntakeVoltage, diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index f7fd7ece..76a74022 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -75,11 +75,15 @@ public void sysIdDrive(double targetVoltage) { masterMotor.setControl(voltageRequest.withOutput(targetVoltage)); } + public boolean isCloseEnoughToOpenGripper() { + return calculateTargetStateDistance() < 0.3; + } + public boolean willCurrentMovementMoveThroughHitRange() { return willMovementMoveThroughHitRange(metersToRotations(targetState.targetPositionMeters)); } - public boolean isElevatorOverAlgaeHitRange() { + public boolean isOverAlgaeHitRange() { return getPositionRotations() > ElevatorConstants.GRIPPER_HITTING_ALGAE_UPPER_BOUND_POSITION_ROTATIONS; } @@ -93,8 +97,11 @@ public boolean atState(ElevatorConstants.ElevatorState targetState) { @AutoLogOutput(key = "Elevator/AtTargetState") public boolean atTargetState() { - final double currentToTargetStateDifference = Math.abs(targetState.targetPositionMeters - getPositionMeters()); - return currentToTargetStateDifference < ElevatorConstants.POSITION_TOLERANCE_METERS; + return calculateTargetStateDistance() < ElevatorConstants.POSITION_TOLERANCE_METERS; + } + + private double calculateTargetStateDistance() { + return Math.abs(targetState.targetPositionMeters - getPositionMeters()); } public double metersToRotations(double positionMeters) { diff --git a/src/main/java/frc/trigon/robot/subsystems/gripper/Gripper.java b/src/main/java/frc/trigon/robot/subsystems/gripper/Gripper.java index f700909a..cdaa2756 100644 --- a/src/main/java/frc/trigon/robot/subsystems/gripper/Gripper.java +++ b/src/main/java/frc/trigon/robot/subsystems/gripper/Gripper.java @@ -8,9 +8,11 @@ import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.misc.simulatedfield.SimulationFieldHandler; import frc.trigon.robot.subsystems.MotorSubsystem; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +import org.trigon.hardware.RobotHardwareStats; import org.trigon.hardware.misc.simplesensor.SimpleSensor; import org.trigon.hardware.phoenix6.cancoder.CANcoderEncoder; import org.trigon.hardware.phoenix6.cancoder.CANcoderSignal; @@ -106,10 +108,18 @@ public boolean isEjecting() { return grippingMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE) <= GripperConstants.MINIMUM_VOLTAGE_FOR_EJECTING; } + public boolean isReleasingAlgae() { + return grippingMotor.getSignal(TalonFXSignal.MOTOR_VOLTAGE) > -2; + } + public boolean isOpenForElevator() { return getCurrentEncoderAngle().getDegrees() > GripperConstants.MINIMUM_OPEN_FOR_ELEVATOR_ANGLE.getDegrees(); } + public Translation3d calculateAlgaeReleaseVelocity() { + return new Translation3d(3, 0, 0).rotateBy(new Rotation3d(0, getCurrentEncoderAngle().getRadians() + 2, 0)); + } + @AutoLogOutput(key = "Gripper/HasGamePiece") public boolean hasGamePiece() { return GripperConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); @@ -143,7 +153,12 @@ public Translation3d getRobotRelativeExitVelocity() { @AutoLogOutput public boolean isMovingSlowly() { - return Math.abs(grippingMotor.getSignal(TalonFXSignal.VELOCITY)) < 4; + return Math.abs(grippingMotor.getSignal(TalonFXSignal.VELOCITY)) < 4 || (RobotHardwareStats.isSimulation() && SimulationFieldHandler.isHoldingAlgae()); + } + + public Pose3d calculateAlgaeCollectionPose() { + return calculateVisualizationPose() + .transformBy(GripperConstants.GRIPPER_TO_HELD_ALGAE); } void prepareForState(GripperConstants.GripperState targetState) { @@ -173,11 +188,11 @@ void setTargetState(Rotation2d targetAngle, double targetGrippingVoltage) { setTargetVoltage(targetGrippingVoltage); } - void setTargetStateWithCurrent(Rotation2d targetAngle, double targetGrippingCurrent) { + void setTargetStateWhileHoldingAlgae(Rotation2d targetAngle, double targetGrippingVoltage) { positionRequest.Slot = 1; scalePositionRequestSpeed(targetState.speedScalar); setTargetAngle(targetAngle); - grippingMotor.setControl(voltageRequest.withOutput(targetGrippingCurrent)); + setTargetVoltage(targetGrippingVoltage); } private void scalePositionRequestSpeed(double speedScalar) { @@ -195,7 +210,7 @@ void setTargetVoltage(double targetGrippingVoltage) { grippingMotor.setControl(voltageRequest.withOutput(targetGrippingVoltage)); } - private Pose3d calculateVisualizationPose() { + public Pose3d calculateVisualizationPose() { final Pose3d currentElevatorPose = RobotContainer.ELEVATOR.getFirstStageComponentPose(); final Pose3d gripperOrigin = currentElevatorPose.transformBy(GripperConstants.ELEVATOR_TO_GRIPPER); diff --git a/src/main/java/frc/trigon/robot/subsystems/gripper/GripperCommands.java b/src/main/java/frc/trigon/robot/subsystems/gripper/GripperCommands.java index 9bb1c802..369a0c57 100644 --- a/src/main/java/frc/trigon/robot/subsystems/gripper/GripperCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/gripper/GripperCommands.java @@ -42,9 +42,9 @@ public static Command getGripperDefaultCommand() { ); } - public static Command getSetTargetStateWithCurrentCommand(GripperConstants.GripperState targetState) { + public static Command getSetTargetStateWhileHoldingAlgaeCommand(GripperConstants.GripperState targetState) { return new StartEndCommand( - () -> RobotContainer.GRIPPER.setTargetStateWithCurrent(targetState.targetAngle, targetState.targetGripperVoltage), + () -> RobotContainer.GRIPPER.setTargetStateWhileHoldingAlgae(targetState.targetAngle, targetState.targetGripperVoltage), RobotContainer.GRIPPER::stop, RobotContainer.GRIPPER ); diff --git a/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java b/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java index 7f01a7f1..edcf9f01 100644 --- a/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/gripper/GripperConstants.java @@ -98,7 +98,7 @@ public class GripperConstants { GRIPPER_LENGTH_METERS, Color.kRed ); - private static final Pose3d GRIPPER_VISUALIZATION_ORIGIN_POINT = new Pose3d( + public static final Pose3d GRIPPER_VISUALIZATION_ORIGIN_POINT = new Pose3d( new Translation3d(-0.224, 0, 0.8622), new Rotation3d(0, edu.wpi.first.math.util.Units.degreesToRadians(62), 0) ); @@ -114,11 +114,14 @@ public class GripperConstants { GRIPPER_TO_HELD_CORAL = new Transform3d( new Translation3d(0.05, 0, -0.1), new Rotation3d(0, 0, 0) + ), + GRIPPER_TO_HELD_ALGAE = new Transform3d( + new Translation3d(-0.1, 0, -0.35), + new Rotation3d(0, edu.wpi.first.math.util.Units.degreesToRadians(0), 0) ); static final double WHEEL_DIAMETER_METERS = edu.wpi.first.math.util.Units.inchesToMeters(2.5); static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(2); - static final double SCORE_IN_REEF_FOR_AUTO_VOLTAGE = -5; static final double MINIMUM_VOLTAGE_FOR_EJECTING = -3; static final Rotation2d MINIMUM_OPEN_FOR_ELEVATOR_ANGLE = Rotation2d.fromDegrees(-34); private static final double COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.10; @@ -233,7 +236,7 @@ private static void configureBeamBreak() { public enum GripperState { REST(Rotation2d.fromDegrees(-56), 0, 1), EJECT(Rotation2d.fromDegrees(55), -3, 1), - EJECT_UPWARDS(Rotation2d.fromDegrees(107), -3, 1), + EJECT_UPWARDS(Rotation2d.fromDegrees(106), -3, 1), SCORE_L4_CLOSE(Rotation2d.fromDegrees(49), -6, 1), SCORE_L4_FAR(Rotation2d.fromDegrees(55), -6, 1), SCORE_L3_OR_L2(Rotation2d.fromDegrees(60), SCORE_L4_CLOSE.targetGripperVoltage, 1), @@ -242,13 +245,13 @@ public enum GripperState { UNLOAD_CORAL(Rotation2d.fromDegrees(-50), -3, 1), COLLECT_ALGAE_FROM_REEF(Rotation2d.fromDegrees(30), -4, 1), COLLECT_ALGAE_FROM_LOLLIPOP(Rotation2d.fromDegrees(-35), COLLECT_ALGAE_FROM_REEF.targetGripperVoltage, 1), - HOLD_ALGAE(Rotation2d.fromDegrees(40), -40, 0.3), + HOLD_ALGAE(Rotation2d.fromDegrees(40), -2, 0.3), SCORE_ALGAE_IN_NET(Rotation2d.fromDegrees(60), 11, 1), PREPARE_FOR_SCORING_ALGAE_IN_NET(Rotation2d.fromDegrees(100), COLLECT_ALGAE_FROM_REEF.targetGripperVoltage, 0.3), SCORE_ALGAE_IN_PROCESSOR(Rotation2d.fromDegrees(-35), 11, 1), - PREPARE_FOR_SCORING_ALGAE_IN_PROCESSOR(Rotation2d.fromDegrees(-35), COLLECT_ALGAE_FROM_REEF.targetGripperVoltage, 0.3), + PREPARE_FOR_SCORING_ALGAE_IN_PROCESSOR(Rotation2d.fromDegrees(-35), -3, 0.3), COLLECT_CORAL_FROM_FEEDER(Rotation2d.fromDegrees(90), 8, 1), - OPEN_FOR_NOT_HITTING_REEF(Rotation2d.fromDegrees(107), 0.4, 1); + OPEN_FOR_NOT_HITTING_REEF(EJECT_UPWARDS.targetAngle, 0.4, 1); final Rotation2d targetAngle; final double targetGripperVoltage; 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 dede48c8..9ba131c8 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -13,7 +13,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; @@ -32,10 +32,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"); @@ -71,7 +71,7 @@ public void updatePeriodically() { updatePoseEstimatorStates(); RobotContainer.ROBOT_POSE_ESTIMATOR.periodic(); -// updateNetworkTables(); + updateNetworkTables(); } @Override @@ -157,7 +157,7 @@ public void drivePathPlanner(ChassisSpeeds targetPathPlannerFeedforwardSpeeds, b if (isFromPathPlanner && DriverStation.isAutonomous() && !isPathPlannerDriving) return; final ChassisSpeeds pidSpeeds = calculateSelfRelativePIDToPoseSpeeds(new FlippablePose2d(targetPathPlannerPose, false)); - final ChassisSpeeds scaledSpeeds = targetPathPlannerFeedforwardSpeeds.times(PathPlannerConstants.FEEDFORWARD_SCALAR); + final ChassisSpeeds scaledSpeeds = targetPathPlannerFeedforwardSpeeds.times(AutonomousConstants.FEEDFORWARD_SCALAR); final ChassisSpeeds combinedSpeeds = pidSpeeds.plus(scaledSpeeds); selfRelativeDrive(combinedSpeeds); } @@ -191,7 +191,7 @@ public Rotation2d getDriveRelativeAngle() { } public void resetSetpoint() { - previousSetpoint = new SwerveSetpoint(getSelfRelativeVelocity(), getModuleStates(), DriveFeedforwards.zeros(PathPlannerConstants.ROBOT_CONFIG.numModules)); + previousSetpoint = new SwerveSetpoint(getSelfRelativeVelocity(), getModuleStates(), DriveFeedforwards.zeros(AutonomousConstants.ROBOT_CONFIG.numModules)); } public void initializeDrive(boolean shouldUseClosedLoop) { @@ -356,8 +356,9 @@ private boolean atTranslationPosition(double currentTranslationPosition, double } private double calculateProfiledAngleSpeedToTargetAngle(FlippableRotation2d targetAngle) { - if (targetAngle != null) - SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.setGoal(targetAngle.get().getDegrees()); + if (targetAngle == null) + return 0; + SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.setGoal(targetAngle.get().getDegrees()); final Rotation2d currentAngle = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation(); final double output = Units.degreesToRadians(SwerveConstants.PROFILED_ROTATION_PID_CONTROLLER.calculate(currentAngle.getDegrees())); diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java index d50798f6..98722f0c 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java @@ -135,6 +135,10 @@ public static Command getDriveToPoseCommand(Supplier targetPose return new DeferredCommand(() -> getCurrentDriveToPoseCommand(targetPose.get(), constraints), Set.of(RobotContainer.SWERVE)); } + public static Command getDriveToPoseCommand(Supplier targetPose, PathConstraints constraints, double endVelocity) { + return new DeferredCommand(() -> getCurrentDriveToPoseCommand(targetPose.get(), constraints, endVelocity), Set.of(RobotContainer.SWERVE)); + } + private static Command getCurrentDriveToPoseCommand(FlippablePose2d targetPose, PathConstraints constraints) { return new SequentialCommandGroup( new InstantCommand(() -> RobotContainer.SWERVE.initializeDrive(true)), @@ -143,6 +147,13 @@ private static Command getCurrentDriveToPoseCommand(FlippablePose2d targetPose, ); } + private static Command getCurrentDriveToPoseCommand(FlippablePose2d targetPose, PathConstraints constraints, double endVelocity) { + return new SequentialCommandGroup( + new InstantCommand(() -> RobotContainer.SWERVE.initializeDrive(true)), + AutoBuilder.pathfindToPose(targetPose.get(), constraints, endVelocity) + ); + } + private static Command getPIDToPoseCommand(FlippablePose2d targetPose) { return new FunctionalCommand( RobotContainer.SWERVE::resetRotationController, 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 5bb1ac14..050fad3f 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 99367104..1d28efa0 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 org.trigon.hardware.RobotHardwareStats; import org.trigon.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;