From 565278d0c55ab95a867bd24cc721fc51516cccdb Mon Sep 17 00:00:00 2001 From: PGgit08 Date: Thu, 14 Mar 2024 23:56:14 -0400 Subject: [PATCH] worked on some vision stuff --- src/main/java/frc/robot/Constants.java | 2 + src/main/java/frc/robot/RobotContainer.java | 6 +- .../subsystems/SwerveDriveSubsystem.java | 2 +- .../frc/robot/subsystems/VisionSubsystem.java | 104 +++++++++--------- src/main/java/frc/robot/utils/UtilFuncs.java | 14 ++- 5 files changed, 72 insertions(+), 56 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 79d4c38..4b3dba2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -156,7 +156,9 @@ public static class FieldConstants { public static final double SPEAKER_HEIGHT = 2.13; public static final double SHOOTER_SLOW_THRESHOLD = 2; + public static final double TAG_DISTANCE_THRESHOLD = 2; + public static final double TAG_DISTANCE_RESET_THRESHOLD = 1; public static final int SPEAKER_TAG_BLUE = 7; public static final int SPEAKER_TAG_RED = 4; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 388d4a0..4886b86 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -86,9 +86,7 @@ public class RobotContainer { /** * The container for the robot. Contains subsystems, OI devices, and commands. */ - public RobotContainer() { - NamedCommands.registerCommand("brake", new BrakeSwerve(_swerveSubsystem, _ledSubsystem)); - + public RobotContainer() { NamedCommands.registerCommand("actuateOut", new SetShooter(_shooterSubsystem, () -> Presets.ACTUATE_SHOOTER_ANGLE).andThen( new FeedActuate(_intakeSubsystem, ActuatorState.OUT, FeedMode.INTAKE)) ); @@ -180,7 +178,7 @@ private void configureBindings() { ) ); - _operatorController.R2().whileTrue( + _driveController.R2().whileTrue( new AutoAim( _shooterSubsystem, _elevatorSubsystem, diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java index 4481504..caa5581 100644 --- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java @@ -76,7 +76,7 @@ public class SwerveDriveSubsystem extends SubsystemBase { private final Orchestra _orchestra = new Orchestra(); String song = "output.chrp"; - private DrivingSpeeds _drivingState = DrivingSpeeds.SLOW; + private DrivingSpeeds _drivingState = DrivingSpeeds.FAST; private Field2d _field = new Field2d(); diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java index db82230..215b7c3 100644 --- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java @@ -51,17 +51,42 @@ public double getLatency() { return Timer.getFPGATimestamp() - (tl / 1000.0) - (cl / 1000.0); } + /** Return a boolean for whether a tag is seen. */ + public boolean isApriltagVisible() { + double tv = _limelight.getEntry("tv").getDouble(0); + + if (tv == 0) { + return false; + } + + if (tv == 1) { + return true; + } + + return false; + } + /** - * Returns the "wpiblue" botpose of the robot from the limelight. + * Return a boolean for whether the desired tag is seen. * - * @return An Optional of Pose2d which is necessary if no valid (at least 2 tags, good distance) data is found from - * the limelight. + * @param ID + * The id of the desired tag. + */ + public boolean isApriltagVisible(int ID) { + if (!isApriltagVisible()) + return false; + + return _limelight.getTag(ID) != null; + } + + /** + * Returns the botpose as an array from the limelight if it is valid (at least 2 tags, good distance). + * + * @return NT "botpose_wpiblue" limelight topic. * * @see Optional */ - public Optional getBotpose() { - // return Optional.empty(); - + public Optional getValidBotpose() { if (!isApriltagVisible()) return Optional.empty(); NetworkTableEntry botpose_entry = _limelight.getEntry("botpose_wpiblue"); @@ -76,61 +101,42 @@ public Optional getBotpose() { // if (tags < 2) return Optional.empty(); if (distance > FieldConstants.TAG_DISTANCE_THRESHOLD) return Optional.empty(); - double botposeX = _xFilter.calculate(botpose_array[0]); // to get rid of the weird origin outlier - double botposeY = _yFilter.calculate(botpose_array[1]); // to get rid of the weird origin outlier - double botposeYaw = _yawFilter.calculate(botpose_array[5]); // to get rid of weird 0 heading outlier - Rotation2d botposeRotation = Rotation2d.fromDegrees(botposeYaw); - - Pose2d botpose2D = new Pose2d(botposeX, botposeY, botposeRotation); - - return Optional.of(botpose2D); + return Optional.of(botpose_array); } /** - * Tells whether limelight data is valid or not. For the data to be valid, the limelight - * must be looking at at least one apriltag, and the apriltag must be in the specific distance range. + * If the limelight is in perfect condition with the apriltags to reset the robot's pose. */ - public boolean isValid() { - if (!isApriltagVisible()) return false; - - JsonNode tags = _limelight.getTags(); - - for (JsonNode tag : tags) { - double distance = ((ArrayNode) tag.get("t6t_cs")).get(2).asDouble(); - if (distance <= FieldConstants.TAG_DISTANCE_THRESHOLD) { - return true; - } - } - - return false; - } - - /** Return a boolean for whether a tag is seen. */ - public boolean isApriltagVisible() { - double tv = _limelight.getEntry("tv").getDouble(0); - - if (tv == 0) { - return false; - } - - if (tv == 1) { - return true; - } + public boolean canResetPose() { + Optional botpose = getValidBotpose(); + if (botpose.isEmpty()) return false; + if (botpose.get()[9] <= FieldConstants.TAG_DISTANCE_RESET_THRESHOLD) return true; return false; } /** - * Return a boolean for whether the desired tag is seen. + * Returns the "wpiblue" Pose2d of the robot from the limelight. * - * @param ID - * The id of the desired tag. + * @return An Optional of Pose2d which is necessary if no valid (at least 2 tags, good distance) data is found from + * the limelight. + * + * @see Optional */ - public boolean isApriltagVisible(int ID) { - if (!isApriltagVisible()) - return false; + public Optional getBotpose() { + Optional botpose = getValidBotpose(); + if (botpose.isEmpty()) return Optional.empty(); - return _limelight.getTag(ID) != null; + double[] botpose_array = botpose.get(); + + double botposeX = _xFilter.calculate(botpose_array[0]); // to get rid of the weird origin outlier + double botposeY = _yFilter.calculate(botpose_array[1]); // to get rid of the weird origin outlier + double botposeYaw = _yawFilter.calculate(botpose_array[5]); // to get rid of weird 0 heading outlier + Rotation2d botposeRotation = Rotation2d.fromDegrees(botposeYaw); + + Pose2d botpose2D = new Pose2d(botposeX, botposeY, botposeRotation); + + return Optional.of(botpose2D); } /** diff --git a/src/main/java/frc/robot/utils/UtilFuncs.java b/src/main/java/frc/robot/utils/UtilFuncs.java index 9cf05a6..199951f 100644 --- a/src/main/java/frc/robot/utils/UtilFuncs.java +++ b/src/main/java/frc/robot/utils/UtilFuncs.java @@ -5,6 +5,7 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; @@ -20,19 +21,28 @@ public final class UtilFuncs { private static DoubleSupplier _distanceSupplier; private static AprilTagFieldLayout _field; + /** + * Loads the AprilTag field. + */ public static void LoadField() { _field = AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo); + _field.setOrigin(OriginPosition.kBlueAllianceWallRightSide); } + /** + * Gets the necessary speaker pose for auto-aim. + * + * @return 3d point to aim at. + */ public static Pose3d GetSpeakerPose() { Pose3d pose; if (GetAlliance() == Alliance.Red) { pose = _field.getTagPose(FieldConstants.SPEAKER_TAG_RED).get(); + } else { + pose = _field.getTagPose(FieldConstants.SPEAKER_TAG_BLUE).get(); } - pose = _field.getTagPose(FieldConstants.SPEAKER_TAG_BLUE).get(); - return new Pose3d( new Translation3d(pose.getX(), pose.getY(), pose.getZ() + FieldConstants.SPEAKER_TAG_OFFSET), new Rotation3d()