diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 8d62f410..3aab31ed 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.trigon.lib.utilities.flippable.Flippable; import frc.trigon.robot.commands.CommandConstants; +import frc.trigon.robot.commands.commandclasses.ShootingSafeDriveCommand; import frc.trigon.robot.commands.commandfactories.ClimbCommands; import frc.trigon.robot.commands.commandfactories.FuelIntakeCommands; import frc.trigon.robot.commands.commandfactories.GeneralCommands; @@ -93,6 +94,9 @@ private void bindControllerCommands() { OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND); OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); + OperatorConstants.SHOOTING_SAFE_DRIVE_TRIGGER.whileTrue(new ShootingSafeDriveCommand()); + OperatorConstants.CAMERAS_DISCONNECTED_TRIGGER.onTrue(CommandConstants.INDICATE_CAMERAS_DISCONNECTED_COMMAND); + OperatorConstants.INDICATE_ALLIANCE_SHIFT_TRIGGER.onTrue(CommandConstants.INDICATE_ALLIANCE_SHIFT_COMMAND); OperatorConstants.INTAKE_TRIGGER.whileTrue(FuelIntakeCommands.getIntakeCommand()); OperatorConstants.TOGGLE_SHOULD_KEEP_INTAKE_OPEN_TRIGGER.onTrue(FuelIntakeCommands.getToggleDefaultIntakeStateCommand()); @@ -107,8 +111,10 @@ private void bindControllerCommands() { OperatorConstants.SET_FIXED_SHOOTING_POSITION_LEFT_CORNER_TRIGGER.onTrue(ShootingCommands.getChangeFixedShootingPositionCommand(ShootingCommands.FixedShootingPosition.LEFT_CORNER)); OperatorConstants.SET_FIXED_SHOOTING_POSITION_CLOSE_TO_TOWER_TRIGGER.onTrue(ShootingCommands.getChangeFixedShootingPositionCommand(ShootingCommands.FixedShootingPosition.CLOSE_TO_TOWER)); OperatorConstants.SET_FIXED_SHOOTING_POSITION_CLOSE_TO_OUTPOST_TRIGGER.onTrue(ShootingCommands.getChangeFixedShootingPositionCommand(ShootingCommands.FixedShootingPosition.CLOSE_TO_OUTPOST)); - OperatorConstants.SHORT_EJECTION_TRIGGER.whileTrue(ShootingCommands.getShortEjectFuelCommand()); + OperatorConstants.OPEN_CLIMBER_TRIGGER.onTrue(ClimbCommands.getClimbToL1Command()); + + OperatorConstants.SHORT_EJECTION_TRIGGER.whileTrue(ShootingCommands.getShortEjectFuelCommand()); } private void configureSysIDBindings(MotorSubsystem subsystem) { diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index d9d41127..c643a9ed 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -26,9 +26,19 @@ public class CommandConstants { MINIMUM_TRANSLATION_SHIFT_POWER = 0.30, MINIMUM_ROTATION_SHIFT_POWER = 0.4; private static final double JOYSTICK_ORIENTED_ROTATION_DEADBAND = 0.07; + private static final double + INDICATE_CAMERAS_DISCONNECTED_RUMBLE_DURATION_SECONDS = 0.5, + INDICATE_CAMERAS_DISCONNECTED_RUMBLE_POWER = 1; + private static final double + INDICATE_ALLIANCE_SHIFT_RUMBLE_DURATION_SECONDS = 1, + INDICATE_ALLIANCE_SHIFT_RUMBLE_POWER = 0.5; - public static final Command + public static final Command //General Commands RESET_HEADING_COMMAND = new InstantCommand(RobotContainer.ROBOT_POSE_ESTIMATOR::resetHeading), + INDICATE_CAMERAS_DISCONNECTED_COMMAND = new InstantCommand(() -> OperatorConstants.DRIVER_CONTROLLER.rumble( + INDICATE_CAMERAS_DISCONNECTED_RUMBLE_DURATION_SECONDS, + INDICATE_CAMERAS_DISCONNECTED_RUMBLE_POWER + )), 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), @@ -57,6 +67,11 @@ public class CommandConstants { (omegaRadiansPerSecond) -> RobotContainer.SWERVE.selfRelativeDrive(new ChassisSpeeds(0, 0, omegaRadiansPerSecond)), RobotContainer.SWERVE ); + public static final Command //Robot-specific commands + INDICATE_ALLIANCE_SHIFT_COMMAND = new InstantCommand(() -> OperatorConstants.DRIVER_CONTROLLER.rumble( + INDICATE_ALLIANCE_SHIFT_RUMBLE_DURATION_SECONDS, + INDICATE_ALLIANCE_SHIFT_RUMBLE_POWER + )); /** * Calculates the target drive power from an axis value by dividing it by the shift mode value. diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java index 9536eab2..20a95303 100644 --- a/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java @@ -119,7 +119,7 @@ private Translation2d calculateSelfRelativeDistanceFromBestGamePiece() { final Translation2d fieldRelativeDistanceFromBestGamePiece = robotPose.getTranslation().minus(bestGamePieceFieldRelativePosition); final Translation2d selfRelativeDistanceFromBestGamePiece = fieldRelativeDistanceFromBestGamePiece.rotateBy(robotPose.getRotation().unaryMinus()); - Logger.recordOutput("IntakeAssist/selfRelativeDistanceFromBestGamePiece", selfRelativeDistanceFromBestGamePiece); + Logger.recordOutput("Assists/selfRelativeDistanceFromBestGamePiece", selfRelativeDistanceFromBestGamePiece); return selfRelativeDistanceFromBestGamePiece; } diff --git a/src/main/java/frc/trigon/robot/commands/commandclasses/ShootingSafeDriveCommand.java b/src/main/java/frc/trigon/robot/commands/commandclasses/ShootingSafeDriveCommand.java new file mode 100644 index 00000000..bc8cc8ed --- /dev/null +++ b/src/main/java/frc/trigon/robot/commands/commandclasses/ShootingSafeDriveCommand.java @@ -0,0 +1,108 @@ +package frc.trigon.robot.commands.commandclasses; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.subsystems.swerve.SwerveCommands; +import frc.trigon.robot.subsystems.swerve.SwerveConstants; + +/** + * A command class that limits the swerve powers to ease shooting to the hub and increase accuracy. + */ +public class ShootingSafeDriveCommand extends SequentialCommandGroup { + private static final double + TRANSLATION_SLEW_RATE = 2, + MAXIMUM_DRIVE_POWER_TOWARDS_HUB = 3 / SwerveConstants.MAXIMUM_SPEED_METERS_PER_SECOND; + private static final SlewRateLimiter + X_SLEW_RATE_LIMITER = new SlewRateLimiter(TRANSLATION_SLEW_RATE), + Y_SLEW_RATE_LIMITER = new SlewRateLimiter(TRANSLATION_SLEW_RATE), + ROTATION_SLEW_RATE_LIMITER = new SlewRateLimiter(1); + + public ShootingSafeDriveCommand() { + addCommands( + getInitializeCommand(), + SwerveCommands.getClosedLoopFieldRelativeDriveCommand( + ShootingSafeDriveCommand::calculateSafeDriveTranslationPower, + ShootingSafeDriveCommand::calculateSafeDriveRotationPower + ).asProxy() + ); + } + + private static Command getInitializeCommand() { + return new InstantCommand(() -> { + X_SLEW_RATE_LIMITER.reset(0); + Y_SLEW_RATE_LIMITER.reset(0); + ROTATION_SLEW_RATE_LIMITER.reset(0); + }); + } + + private static Translation2d calculateSafeDriveTranslationPower() { + return limitTranslationPowerToEaseShooting(getSlewRateLimitedTranslationPower()); + } + + private static double calculateSafeDriveRotationPower() { + final double rawPower = OperatorConstants.DRIVER_CONTROLLER.getRightX(); + return ROTATION_SLEW_RATE_LIMITER.calculate(rawPower); + } + + private static Translation2d getSlewRateLimitedTranslationPower() { + final double + rawXPower = OperatorConstants.DRIVER_CONTROLLER.getLeftY(), + rawYPower = OperatorConstants.DRIVER_CONTROLLER.getLeftX(); + + return new Translation2d( + X_SLEW_RATE_LIMITER.calculate(rawXPower), + Y_SLEW_RATE_LIMITER.calculate(rawYPower) + ); + } + + private static Translation2d limitTranslationPowerToEaseShooting(Translation2d targetPower) { + final Rotation2d hubDirection = calculateFieldRelativeAngleToHub().plus(Rotation2d.k180deg); + + final double radialVelocity = calculateRadialVelocityToHub(targetPower, hubDirection); + final double limitedRadialVelocity = limitRadialVelocity(radialVelocity); + + final Translation2d tangentialComponent = calculateTangentialComponent(targetPower, radialVelocity, hubDirection); + final Translation2d limitedRadialComponent = calculateRadialComponent(limitedRadialVelocity, hubDirection); + + return tangentialComponent.plus(limitedRadialComponent); + } + + private static double calculateRadialVelocityToHub(Translation2d velocity, Rotation2d hubDirection) { + return velocity.getX() * hubDirection.getCos() + velocity.getY() * hubDirection.getSin(); + } + + private static double limitRadialVelocity(double radialVelocity) { + return MathUtil.clamp( + radialVelocity, + -MAXIMUM_DRIVE_POWER_TOWARDS_HUB, + MAXIMUM_DRIVE_POWER_TOWARDS_HUB + ); + } + + private static Translation2d calculateTangentialComponent(Translation2d velocity, double radialVelocity, Rotation2d hubDirection) { + final Translation2d radialComponent = calculateRadialComponent(radialVelocity, hubDirection); + return velocity.minus(radialComponent); + } + + private static Translation2d calculateRadialComponent(double radialVelocity, Rotation2d hubDirection) { + return new Translation2d( + radialVelocity * hubDirection.getCos(), + radialVelocity * hubDirection.getSin() + ); + } + + private static Rotation2d calculateFieldRelativeAngleToHub() { + final Translation2d + robotPosition = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(), + hubPosition = FieldConstants.HUB_POSITION.get(); + return hubPosition.minus(robotPosition).getAngle(); + } +} diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java index 29da7f77..d7157117 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/ClimbCommands.java @@ -1,33 +1,37 @@ package frc.trigon.robot.commands.commandfactories; -import edu.wpi.first.wpilibj2.command.*; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.trigon.robot.constants.OperatorConstants; import frc.trigon.robot.subsystems.climber.ClimberCommands; import frc.trigon.robot.subsystems.climber.ClimberConstants; +import org.littletonrobotics.junction.networktables.LoggedNetworkBoolean; public class ClimbCommands { - public static boolean IS_CLIMBING = false; + public static final LoggedNetworkBoolean IS_CLIMBING = new LoggedNetworkBoolean("IsClimbing", false); public static Command getClimbToL1Command() { return new SequentialCommandGroup( - ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.CLIMB_PREPARE).until(OperatorConstants.CONTINUE_CLIMB_TRIGGER), - new InstantCommand(() -> IS_CLIMBING = true), + new InstantCommand(() -> IS_CLIMBING.set(true)), + ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.PREPARE_CLIMB).until(OperatorConstants.CONTINUE_CLIMB_TRIGGER), ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.CLIMB_L1) ).until(OperatorConstants.CANCEL_CLIMB_TRIGGER); } - public static Command getClimbDownFromL1Command() { + public static Command getReleaseL1Command() { return new SequentialCommandGroup( - new WaitUntilCommand(OperatorConstants.CANCEL_CLIMB_TRIGGER.negate()), - ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.CLIMB_DOWN).until(OperatorConstants.CANCEL_CLIMB_TRIGGER) + ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.RELEASE_CLIMB).until(OperatorConstants.CANCEL_CLIMB_TRIGGER.negate()), + new InstantCommand(() -> IS_CLIMBING.set(false)) ); } public static Command getClimberDefaultCommand() { return new ConditionalCommand( - getClimbDownFromL1Command().andThen(() -> IS_CLIMBING = false), + getReleaseL1Command(), ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.REST), - () -> IS_CLIMBING + IS_CLIMBING::get ); } } diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/FuelIntakeCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/FuelIntakeCommands.java index 30ce2611..da4e4490 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/FuelIntakeCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/FuelIntakeCommands.java @@ -28,19 +28,19 @@ public static Command getIntakeCommand() { public static Command getToggleDefaultIntakeStateCommand() { return new InstantCommand( () -> SHOULD_KEEP_INTAKE_OPEN.set(!SHOULD_KEEP_INTAKE_OPEN.get()) - ); + ).ignoringDisable(true); } public static Command getEnableIntakeAssistCommand() { return new InstantCommand( () -> SHOULD_ASSIST_INTAKE.set(true) - ); + ).ignoringDisable(true); } public static Command getDisableIntakeAssistCommand() { return new InstantCommand( () -> SHOULD_ASSIST_INTAKE.set(false) - ); + ).ignoringDisable(true); } private static Command getIntakeAssistCommand() { diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java index 16792f78..fcf52bd6 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/autonomous/GeneralAutonomousCommands.java @@ -120,8 +120,8 @@ public static Command getClimbCommand(Supplier climbPosition) { private static Command getClimbToL1Command(Supplier climbPosition) { return new SequentialCommandGroup( - ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.CLIMB_PREPARE).until(() -> RobotContainer.CLIMBER.atTargetState() && RobotContainer.SWERVE.atPose(climbPosition.get())), - new InstantCommand(() -> ClimbCommands.IS_CLIMBING = true), + ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.PREPARE_CLIMB).until(() -> RobotContainer.CLIMBER.atTargetState() && RobotContainer.SWERVE.atPose(climbPosition.get())), + new InstantCommand(() -> ClimbCommands.IS_CLIMBING.set(true)), ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.CLIMB_L1) ).until(OperatorConstants.CANCEL_CLIMB_TRIGGER); } diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index c32157a2..fed4d0a6 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -9,6 +9,7 @@ import frc.trigon.lib.hardware.misc.XboxController; import frc.trigon.lib.utilities.flippable.FlippablePose2d; import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.commands.commandfactories.ClimbCommands; import frc.trigon.robot.misc.MatchTracker; import java.util.function.DoubleUnaryOperator; @@ -23,6 +24,8 @@ public class OperatorConstants { DRIVER_CONTROLLER_PORT, DRIVER_CONTROLLER_RIGHT_STICK_EXPONENT, DRIVER_CONTROLLER_LEFT_STICK_EXPONENT, DRIVER_CONTROLLER_DEADBAND ); public static final KeyboardController OPERATOR_CONTROLLER = new KeyboardController(); + private static final double ARE_CAMERAS_DISCONNECTED_CHECK_DEBOUNCE_SECONDS = 3; + private static final double DOUBLE_TAP_TIMEOUT_SECONDS = 0.5; public static final double POV_DIVIDER = 2, @@ -41,10 +44,13 @@ public class OperatorConstants { ); public static final Trigger //General Triggers - RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.y(), + RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.y().multiPress(2, DOUBLE_TAP_TIMEOUT_SECONDS), DRIVE_FROM_DPAD_TRIGGER = new Trigger(() -> DRIVER_CONTROLLER.getPov() != -1), TOGGLE_BRAKE_TRIGGER = OPERATOR_CONTROLLER.g().or(RobotController::getUserButton), DEBUGGING_TRIGGER = OPERATOR_CONTROLLER.f2(), + SHOOTING_SAFE_DRIVE_TRIGGER = DRIVER_CONTROLLER.leftStick(), + CAMERAS_DISCONNECTED_TRIGGER = new Trigger(() -> !RobotContainer.ROBOT_POSE_ESTIMATOR.hasUpdateFromCameras()).debounce(ARE_CAMERAS_DISCONNECTED_CHECK_DEBOUNCE_SECONDS), + INDICATE_ALLIANCE_SHIFT_TRIGGER = new Trigger(MatchTracker::shouldIndicateAllianceShift), FORWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.right(), BACKWARD_QUASISTATIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.left(), FORWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.up(), @@ -52,8 +58,8 @@ public class OperatorConstants { public static final Trigger //Intake Triggers INTAKE_TRIGGER = DRIVER_CONTROLLER.leftTrigger(), TOGGLE_SHOULD_KEEP_INTAKE_OPEN_TRIGGER = DRIVER_CONTROLLER.b().or(OPERATOR_CONTROLLER.u()), - ENABLE_INTAKE_ASSIST_TRIGGER = OPERATOR_CONTROLLER.o(), - DISABLE_INTAKE_ASSIST_TRIGGER = DRIVER_CONTROLLER.a().or(OPERATOR_CONTROLLER.p()); + ENABLE_INTAKE_ASSIST_TRIGGER = OPERATOR_CONTROLLER.o().multiPress(2, DOUBLE_TAP_TIMEOUT_SECONDS), + DISABLE_INTAKE_ASSIST_TRIGGER = DRIVER_CONTROLLER.a().or(OPERATOR_CONTROLLER.p().multiPress(2, DOUBLE_TAP_TIMEOUT_SECONDS)); public static final Trigger //Shooting Triggers DISABLE_AUTO_SHOOT_TRIGGER = DRIVER_CONTROLLER.rightStick().or(OPERATOR_CONTROLLER.a()), AUTO_SHOOT_AT_HUB_TRIGGER = new Trigger(OperatorConstants::shouldAutoShootAtHub), @@ -65,9 +71,9 @@ public class OperatorConstants { SET_FIXED_SHOOTING_POSITION_CLOSE_TO_TOWER_TRIGGER = DRIVER_CONTROLLER.povDown().or(OPERATOR_CONTROLLER.k()), SET_FIXED_SHOOTING_POSITION_CLOSE_TO_OUTPOST_TRIGGER = DRIVER_CONTROLLER.povRight().or(OPERATOR_CONTROLLER.l()); public static final Trigger //Climb Triggers - OPEN_CLIMBER_TRIGGER = DRIVER_CONTROLLER.back().or(OPERATOR_CONTROLLER.c()), - CANCEL_CLIMB_TRIGGER = DRIVER_CONTROLLER.leftBumper().or(OPERATOR_CONTROLLER.x()), - CONTINUE_CLIMB_TRIGGER = DRIVER_CONTROLLER.rightBumper().or(OPERATOR_CONTROLLER.v()); + OPEN_CLIMBER_TRIGGER = DRIVER_CONTROLLER.back().or(OPERATOR_CONTROLLER.c()).multiPress(2, DOUBLE_TAP_TIMEOUT_SECONDS), + CANCEL_CLIMB_TRIGGER = new Trigger(ClimbCommands.IS_CLIMBING).and(DRIVER_CONTROLLER.leftBumper()).or(OPERATOR_CONTROLLER.x()), + CONTINUE_CLIMB_TRIGGER = new Trigger(ClimbCommands.IS_CLIMBING).and(DRIVER_CONTROLLER.rightBumper()).or(OPERATOR_CONTROLLER.v()); public static final Trigger //Debugging Triggers UNJAM_TRIGGER = DRIVER_CONTROLLER.start().or(OPERATOR_CONTROLLER.q()), SHORT_EJECTION_TRIGGER = DRIVER_CONTROLLER.x().or(OPERATOR_CONTROLLER.e()); diff --git a/src/main/java/frc/trigon/robot/misc/MatchTracker.java b/src/main/java/frc/trigon/robot/misc/MatchTracker.java index 5ba8b378..c4a0592f 100644 --- a/src/main/java/frc/trigon/robot/misc/MatchTracker.java +++ b/src/main/java/frc/trigon/robot/misc/MatchTracker.java @@ -5,7 +5,20 @@ import org.littletonrobotics.junction.AutoLogOutput; public final class MatchTracker { + private static final double TIME_BEFORE_ALLIANCE_SHIFT_TO_INDICATE_SECONDS = 5; + + @AutoLogOutput(key = "Assists/ShouldIndicateAllianceShift") + public static boolean shouldIndicateAllianceShift() { + return DriverStation.getMatchType() != DriverStation.MatchType.None && + isHubActive(getMatchTimeSeconds() - TIME_BEFORE_ALLIANCE_SHIFT_TO_INDICATE_SECONDS); + } + + @AutoLogOutput(key = "IsHubActive") public static boolean isHubActive() { + return isHubActive(getMatchTimeSeconds()); + } + + public static boolean isHubActive(double matchTimeSeconds) { if (!DriverStation.isTeleop()) return true; @@ -14,16 +27,16 @@ public static boolean isHubActive() { if (!"R".equals(gameMessage) && !"B".equals(gameMessage)) return true; final boolean didRedAllianceWinAutonomous = "R".equals(gameMessage); - final boolean isRedHubActive = isRedHubActive(didRedAllianceWinAutonomous); - return isRedAlliance ? isRedHubActive : !isRedHubActive; + final boolean isRedHubActive = isRedHubActive(didRedAllianceWinAutonomous, matchTimeSeconds); + return isRedAlliance == isRedHubActive; } public static double getMatchTimeSeconds() { return DriverStation.getMatchTime(); } - private static boolean isRedHubActive(boolean didRedAllianceWinAutonomous) { - final int currentShiftNumber = getCurrentShiftNumber(); + private static boolean isRedHubActive(boolean didRedAllianceWinAutonomous, double matchTimeSeconds) { + final int currentShiftNumber = getShiftNumber(matchTimeSeconds); if (currentShiftNumber == -1) return true; if (didRedAllianceWinAutonomous && currentShiftNumber % 2 != 0) @@ -33,8 +46,7 @@ private static boolean isRedHubActive(boolean didRedAllianceWinAutonomous) { return true; } - private static int getCurrentShiftNumber() { - final double matchTimeSeconds = getMatchTimeSeconds(); + private static int getShiftNumber(double matchTimeSeconds) { if (matchTimeSeconds > 30 && matchTimeSeconds <= 55) return 4; if (matchTimeSeconds > 55 && matchTimeSeconds <= 80) diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java index 84c63c08..7bbed318 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/robotposeestimator/RobotPoseEstimator.java @@ -36,6 +36,7 @@ public class RobotPoseEstimator implements AutoCloseable { private final AprilTagCamera[] aprilTagCameras; private final RelativeRobotPoseSource relativeRobotPoseSource; private final boolean shouldUseRelativeRobotPoseSource; + private boolean hasUpdateFromCameras = false; /** * Constructs a new RobotPoseEstimator and sets the relativeRobotPoseSource. @@ -49,7 +50,7 @@ public RobotPoseEstimator(RelativeRobotPoseSource relativeRobotPoseSource, April this.relativeRobotPoseSource = relativeRobotPoseSource; this.shouldUseRelativeRobotPoseSource = true; - initialize(); + initializeFieldWidget(); } /** @@ -63,7 +64,7 @@ public RobotPoseEstimator(AprilTagCamera... aprilTagCameras) { this.relativeRobotPoseSource = null; this.shouldUseRelativeRobotPoseSource = false; - initialize(); + initializeFieldWidget(); } @Override @@ -120,6 +121,10 @@ public Pose2d getEstimatedOdometryPose() { return swerveDriveOdometry.getPoseMeters(); } + public boolean hasUpdateFromCameras() { + return hasUpdateFromCameras; + } + /** * Updates the pose estimator with the given swerve wheel positions and gyro rotations. * This function accepts an array of swerve wheel positions and an array of gyro rotations because the odometry can be updated at a faster rate than the main loop (which is 50 hertz). @@ -161,7 +166,7 @@ public Pose2d getPredictedRobotPose(double seconds) { return getEstimatedRobotPose().transformBy(new Transform2d(predictedX, predictedY, predictedRotation)); } - private void initialize() { + private void initializeFieldWidget() { putAprilTagsOnFieldWidget(); SmartDashboard.putData("Field", field); logTargetPath(); @@ -209,6 +214,8 @@ private void updateFromRelativeRobotPoseSource() { private void updateFromAprilTagCameras() { final AprilTagCamera[] newResultCameras = getCamerasWithResults(); + + this.hasUpdateFromCameras = newResultCameras.length > 0; // sortCamerasByLatestResultTimestamp(newResultCameras); for (AprilTagCamera aprilTagCamera : newResultCameras) { diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java index fe930b86..ddc1ad58 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/Climber.java @@ -74,7 +74,7 @@ public void sysIDDrive(double targetVoltage) { } public boolean isClimbing() { - return targetState == ClimberConstants.ClimberState.CLIMB_PREPARE || + return targetState == ClimberConstants.ClimberState.PREPARE_CLIMB || targetState == ClimberConstants.ClimberState.CLIMB_L1; } diff --git a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java index e92a23cc..13b99576 100644 --- a/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/climber/ClimberConstants.java @@ -134,9 +134,9 @@ private static void configureMotor() { public enum ClimberState { REST(0, false), - CLIMB_PREPARE(0.8, false), + PREPARE_CLIMB(0.8, false), CLIMB_L1(0.5, true), - CLIMB_DOWN(0.8, true); + RELEASE_CLIMB(0.8, true); final double targetPositionMeters; final boolean affectedByRobotWeight; 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 85f7a96b..c337be1b 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java @@ -59,11 +59,11 @@ public void updateLog(SysIdRoutineLog log) { @Override public void updatePeriodically() { - Phoenix6SignalThread.SIGNALS_LOCK.lock(); + Phoenix6SignalThread.QUEUES_LOCK.lock(); try { updateHardware(); } finally { - Phoenix6SignalThread.SIGNALS_LOCK.unlock(); + Phoenix6SignalThread.QUEUES_LOCK.unlock(); } updatePoseEstimatorStates(); @@ -232,6 +232,17 @@ void fieldRelativeDrive(double xPower, double yPower, double thetaPower) { selfRelativeDrive(speeds); } + /** + * Drives the swerve with the given powers, relative to the field's frame of reference. + * + * @param translationPowers the translation powers + * @param rotationPower the rotation power + */ + void fieldRelativeDrive(Translation2d translationPowers, double rotationPower) { + final ChassisSpeeds speeds = selfRelativeSpeedsFromFieldRelativePowers(translationPowers.getX(), translationPowers.getY(), rotationPower); + selfRelativeDrive(speeds); + } + /** * Drives the swerve with the given powers, relative to the robot's frame of reference. * 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 66aaa320..1d452c27 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveCommands.java @@ -3,6 +3,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.*; import frc.trigon.lib.commands.InitExecuteCommand; import frc.trigon.lib.utilities.flippable.FlippablePose2d; @@ -30,6 +31,21 @@ public static Command getClosedLoopFieldRelativeDriveCommand(DoubleSupplier xSup ); } + /** + * Creates a command that drives the swerve with the given powers, relative to the field's frame of reference, in closed loop mode. + * + * @param translationSupplier the target translation powers + * @param rotationSupplier the target rotation power, CCW+ + * @return the command + */ + public static Command getClosedLoopFieldRelativeDriveCommand(Supplier translationSupplier, DoubleSupplier rotationSupplier) { + return new InitExecuteCommand( + () -> RobotContainer.SWERVE.initializeDrive(true), + () -> RobotContainer.SWERVE.fieldRelativeDrive(translationSupplier.get(), rotationSupplier.getAsDouble()), + RobotContainer.SWERVE + ); + } + /** * Creates a command that drives the swerve with the given powers, relative to the field's frame of reference, in closed loop mode. * This command will use pid to reach the target angle.