From 095f4efa10c257c70eb06221794a308f8db89f16 Mon Sep 17 00:00:00 2001 From: Jonathan Smith Date: Thu, 28 Mar 2024 11:10:43 -0500 Subject: [PATCH 1/5] PP rotation target degrees instead of radians. --- src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index caa123b..65b78e3 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -153,7 +153,7 @@ public Optional getRotationTargetOverride() { // drivetrain.setChassisSpeeds(HOLO_CONTROLLER.calculate(current, new // Pose2d(current.getTranslation(), new Rotation2d(desiredRotation)), 0, new // Rotation2d(desiredRotation))); - return Optional.of(new Rotation2d(desiredRotation + 90)); + return Optional.of(Rotation2d.fromDegrees(desiredRotation + 90.0)); } else { // return an empty optional when we don't want to override the path's rotation return Optional.empty(); From 7c4a2f9aaceafbf8674c3530d51d34069601b747 Mon Sep 17 00:00:00 2001 From: Jonathan Smith Date: Thu, 28 Mar 2024 15:15:53 -0500 Subject: [PATCH 2/5] Spotless apply. --- src/main/java/frc/robot/RobotContainer.java | 30 ++++++++----------- .../control/auto/AutoAimLockWrist.java | 2 +- .../control/auto/AutoIdleShooter.java | 1 - .../subsystems/CommandSwerveDrivetrain.java | 8 ++--- 4 files changed, 18 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 97334ee..fed2bd7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,7 +21,6 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -42,8 +41,6 @@ import frc.robot.commands.control.ShootSubwooferFlat; import frc.robot.commands.control.ShootTall; import frc.robot.commands.control.StopAll; -import frc.robot.commands.control.amp.FireRevAmp; -import frc.robot.commands.control.amp.PrepRevAmp; import frc.robot.commands.control.auto.AutoAimLockWrist; import frc.robot.commands.control.auto.AutoIdleShooter; import frc.robot.commands.control.auto.InstantShoot; @@ -58,7 +55,6 @@ import frc.robot.commands.movement.CollectNoteAuto; import frc.robot.commands.movement.DriveToNote; import frc.robot.commands.movement.DriveToNoteAuto; -import frc.robot.commands.movement.PointAtAprilTag; import frc.robot.commands.movement.SwerveCommand; import frc.robot.commands.qol.AsyncRumble; import frc.robot.commands.qol.DefaultCANdle; @@ -122,7 +118,7 @@ public RobotContainer() { configureShuffleboard(); configureDrivetrain(); configureDriverController(); - //configureCoDriverController(); + // configureCoDriverController(); configureDefaultCommands(); } @@ -548,18 +544,18 @@ public void updatePoseVision() { // if (poseRight.tagCount >= 2) { // DRIVETRAIN.addVisionMeasurement( // poseRight.pose, poseRight.timestampSeconds, VecBuilder.fill(1.5, 1.5,1.5)); - // } else { - // if (pose.rawFiducials.length > 0 && pose.rawFiducials[0].ambiguity < 0.07) { - // DriverStation.reportWarning( - // "ADDING POSE BASED ON AMBIGUITY OF " - // + pose.rawFiducials[0].ambiguity - // + " ON TAG " - // + pose.rawFiducials[0].id, - // false); - // DRIVETRAIN.addVisionMeasurement( - // pose.pose, pose.timestampSeconds, VecBuilder.fill(.7, .7, 9999999)); - // } - //} + // } else { + // if (pose.rawFiducials.length > 0 && pose.rawFiducials[0].ambiguity < 0.07) { + // DriverStation.reportWarning( + // "ADDING POSE BASED ON AMBIGUITY OF " + // + pose.rawFiducials[0].ambiguity + // + " ON TAG " + // + pose.rawFiducials[0].id, + // false); + // DRIVETRAIN.addVisionMeasurement( + // pose.pose, pose.timestampSeconds, VecBuilder.fill(.7, .7, 9999999)); + // } + // } } public static RobotContainer get() { diff --git a/src/main/java/frc/robot/commands/control/auto/AutoAimLockWrist.java b/src/main/java/frc/robot/commands/control/auto/AutoAimLockWrist.java index b361874..ec291c9 100644 --- a/src/main/java/frc/robot/commands/control/auto/AutoAimLockWrist.java +++ b/src/main/java/frc/robot/commands/control/auto/AutoAimLockWrist.java @@ -28,7 +28,7 @@ public void initialize() {} public void execute() { double degrees = Util.getInterpolatedWristAngle(); // TODO find actual values, prevent wrist collision when the elevator is all the way down. - DriverStation.reportWarning("Wrist Degrees Angle " + degrees, false); + DriverStation.reportWarning("Wrist Degrees Angle " + degrees, false); if (degrees > 10.0 && degrees < 35.0) { return; diff --git a/src/main/java/frc/robot/commands/control/auto/AutoIdleShooter.java b/src/main/java/frc/robot/commands/control/auto/AutoIdleShooter.java index 16cd251..cac412c 100644 --- a/src/main/java/frc/robot/commands/control/auto/AutoIdleShooter.java +++ b/src/main/java/frc/robot/commands/control/auto/AutoIdleShooter.java @@ -5,7 +5,6 @@ package frc.robot.commands.control.auto; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.RobotContainer; import frc.robot.constants.Constants; import frc.robot.subsystems.Shooter; diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 65b78e3..b42438d 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -148,11 +148,11 @@ public Optional getRotationTargetOverride() { if (RobotContainer.aimAtTargetAuto) { Pose2d current = getPose(); double desiredRotation = - current.getRotation().minus(Util.getRotationToAllianceSpeaker(current)).getDegrees(); + current.getRotation().minus(Util.getRotationToAllianceSpeaker(current)).getDegrees(); - // drivetrain.setChassisSpeeds(HOLO_CONTROLLER.calculate(current, new - // Pose2d(current.getTranslation(), new Rotation2d(desiredRotation)), 0, new - // Rotation2d(desiredRotation))); + // drivetrain.setChassisSpeeds(HOLO_CONTROLLER.calculate(current, new + // Pose2d(current.getTranslation(), new Rotation2d(desiredRotation)), 0, new + // Rotation2d(desiredRotation))); return Optional.of(Rotation2d.fromDegrees(desiredRotation + 90.0)); } else { // return an empty optional when we don't want to override the path's rotation From 50a7729142a13bbc9a79bd01b18fbbb21e45f4ef Mon Sep 17 00:00:00 2001 From: Mallen220 <68817732+Mallen220@users.noreply.github.com> Date: Fri, 29 Mar 2024 13:51:44 -0500 Subject: [PATCH 3/5] WPILIB and PathPlannerLib Updates --- build.gradle | 2 +- src/main/java/frc/robot/RobotContainer.java | 30 ++++++++----------- .../control/auto/AutoAimLockWrist.java | 2 +- .../control/auto/AutoIdleShooter.java | 1 - .../subsystems/CommandSwerveDrivetrain.java | 8 ++--- vendordeps/PathplannerLib.json | 6 ++-- 6 files changed, 22 insertions(+), 27 deletions(-) diff --git a/build.gradle b/build.gradle index 4ddc2cf..31f9077 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.3.1" + id "edu.wpi.first.GradleRIO" version "2024.3.2" id 'com.diffplug.spotless' version '6.25.0' } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 97334ee..fed2bd7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,7 +21,6 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -42,8 +41,6 @@ import frc.robot.commands.control.ShootSubwooferFlat; import frc.robot.commands.control.ShootTall; import frc.robot.commands.control.StopAll; -import frc.robot.commands.control.amp.FireRevAmp; -import frc.robot.commands.control.amp.PrepRevAmp; import frc.robot.commands.control.auto.AutoAimLockWrist; import frc.robot.commands.control.auto.AutoIdleShooter; import frc.robot.commands.control.auto.InstantShoot; @@ -58,7 +55,6 @@ import frc.robot.commands.movement.CollectNoteAuto; import frc.robot.commands.movement.DriveToNote; import frc.robot.commands.movement.DriveToNoteAuto; -import frc.robot.commands.movement.PointAtAprilTag; import frc.robot.commands.movement.SwerveCommand; import frc.robot.commands.qol.AsyncRumble; import frc.robot.commands.qol.DefaultCANdle; @@ -122,7 +118,7 @@ public RobotContainer() { configureShuffleboard(); configureDrivetrain(); configureDriverController(); - //configureCoDriverController(); + // configureCoDriverController(); configureDefaultCommands(); } @@ -548,18 +544,18 @@ public void updatePoseVision() { // if (poseRight.tagCount >= 2) { // DRIVETRAIN.addVisionMeasurement( // poseRight.pose, poseRight.timestampSeconds, VecBuilder.fill(1.5, 1.5,1.5)); - // } else { - // if (pose.rawFiducials.length > 0 && pose.rawFiducials[0].ambiguity < 0.07) { - // DriverStation.reportWarning( - // "ADDING POSE BASED ON AMBIGUITY OF " - // + pose.rawFiducials[0].ambiguity - // + " ON TAG " - // + pose.rawFiducials[0].id, - // false); - // DRIVETRAIN.addVisionMeasurement( - // pose.pose, pose.timestampSeconds, VecBuilder.fill(.7, .7, 9999999)); - // } - //} + // } else { + // if (pose.rawFiducials.length > 0 && pose.rawFiducials[0].ambiguity < 0.07) { + // DriverStation.reportWarning( + // "ADDING POSE BASED ON AMBIGUITY OF " + // + pose.rawFiducials[0].ambiguity + // + " ON TAG " + // + pose.rawFiducials[0].id, + // false); + // DRIVETRAIN.addVisionMeasurement( + // pose.pose, pose.timestampSeconds, VecBuilder.fill(.7, .7, 9999999)); + // } + // } } public static RobotContainer get() { diff --git a/src/main/java/frc/robot/commands/control/auto/AutoAimLockWrist.java b/src/main/java/frc/robot/commands/control/auto/AutoAimLockWrist.java index b361874..ec291c9 100644 --- a/src/main/java/frc/robot/commands/control/auto/AutoAimLockWrist.java +++ b/src/main/java/frc/robot/commands/control/auto/AutoAimLockWrist.java @@ -28,7 +28,7 @@ public void initialize() {} public void execute() { double degrees = Util.getInterpolatedWristAngle(); // TODO find actual values, prevent wrist collision when the elevator is all the way down. - DriverStation.reportWarning("Wrist Degrees Angle " + degrees, false); + DriverStation.reportWarning("Wrist Degrees Angle " + degrees, false); if (degrees > 10.0 && degrees < 35.0) { return; diff --git a/src/main/java/frc/robot/commands/control/auto/AutoIdleShooter.java b/src/main/java/frc/robot/commands/control/auto/AutoIdleShooter.java index 16cd251..cac412c 100644 --- a/src/main/java/frc/robot/commands/control/auto/AutoIdleShooter.java +++ b/src/main/java/frc/robot/commands/control/auto/AutoIdleShooter.java @@ -5,7 +5,6 @@ package frc.robot.commands.control.auto; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.RobotContainer; import frc.robot.constants.Constants; import frc.robot.subsystems.Shooter; diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 65b78e3..b42438d 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -148,11 +148,11 @@ public Optional getRotationTargetOverride() { if (RobotContainer.aimAtTargetAuto) { Pose2d current = getPose(); double desiredRotation = - current.getRotation().minus(Util.getRotationToAllianceSpeaker(current)).getDegrees(); + current.getRotation().minus(Util.getRotationToAllianceSpeaker(current)).getDegrees(); - // drivetrain.setChassisSpeeds(HOLO_CONTROLLER.calculate(current, new - // Pose2d(current.getTranslation(), new Rotation2d(desiredRotation)), 0, new - // Rotation2d(desiredRotation))); + // drivetrain.setChassisSpeeds(HOLO_CONTROLLER.calculate(current, new + // Pose2d(current.getTranslation(), new Rotation2d(desiredRotation)), 0, new + // Rotation2d(desiredRotation))); return Optional.of(Rotation2d.fromDegrees(desiredRotation + 90.0)); } else { // return an empty optional when we don't want to override the path's rotation diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index a019706..6dc648d 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.2.7", + "version": "2024.2.8", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.7" + "version": "2024.2.8" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.7", + "version": "2024.2.8", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, From a07f47fd0a1039b4e62b121da0de6510cec46f9d Mon Sep 17 00:00:00 2001 From: Jonathan Smith Date: Fri, 29 Mar 2024 14:53:32 -0500 Subject: [PATCH 4/5] Proposed LL pose filtering. Not tested. --- src/main/java/frc/robot/Robot.java | 3 +- src/main/java/frc/robot/RobotContainer.java | 75 +++++++++++-------- .../java/frc/robot/constants/Constants.java | 16 ++++ .../frc/robot/util/RectanglePoseArea.java | 51 +++++++++++++ 4 files changed, 114 insertions(+), 31 deletions(-) create mode 100644 src/main/java/frc/robot/util/RectanglePoseArea.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8c4a93f..bd67d5a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.constants.Constants; public class Robot extends TimedRobot { private Command autoCommand; @@ -28,7 +29,7 @@ public void robotInit() { @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); - robotContainer.updatePoseVision(); + robotContainer.updatePoseVision(Constants.Vision.SPEAKER_LIMELIGHT); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fed2bd7..1c27e29 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -16,6 +16,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; @@ -521,41 +522,55 @@ public Pose2d getPose() { return DRIVETRAIN.getPose(); } - public void updatePoseVision() { + // Reference FRC 6391 + // https://github.com/6391-Ursuline-Bearbotics/2024-6391-Crescendo/blob/4759dfd37c960cf3493b0eafd901519c5c36b239/src/main/java/frc/robot/Vision/Limelight.java#L44-L88 + public void updatePoseVision(final String limelight) { ChassisSpeeds currentSpeed = DRIVETRAIN.getCurrentRobotChassisSpeeds(); - if (Math.abs(currentSpeed.vxMetersPerSecond) > 1.5 - || Math.abs(currentSpeed.vyMetersPerSecond) > 1.5) { + // Reject pose updates when moving fast. + if (Math.abs(currentSpeed.vxMetersPerSecond) > 2.0 + || Math.abs(currentSpeed.vyMetersPerSecond) > 2.0 + || Math.abs(currentSpeed.omegaRadiansPerSecond) > Math.PI) { return; } - Limelight.PoseEstimate pose = Limelight.getBotPoseEstimate_wpiBlue(SPEAKER_LIMELIGHT); - // TODO throw away any botpose that is far away from previous pose - // Sometimes teleports when the ll gets confused - // Usually at a longer distance, could filter out long distance from tags - if (pose.tagCount >= 2) { + Limelight.PoseEstimate botPose = Limelight.getBotPoseEstimate_wpiBlue(limelight); + + // Reject an empty pose. + if (botPose.tagCount < 1) { + return; + } + // Reject a pose outside of the field. + if (!Constants.Vision.fieldBoundary.isPoseWithinArea(botPose.pose)) { + return; + } + // Reject pose from long disance. + if ((botPose.tagCount == 1 + && botPose.avgTagDist > Constants.Vision.maxSingleTagDistanceToAccept) + || (botPose.tagCount >= 2 + && botPose.avgTagDist > Constants.Vision.maxMutiTagDistToAccept)) { + return; + } + // Trust close multi tag pose when disabled with increased confidence. + if (DriverStation.isDisabled() + && botPose.tagCount >= 2 + && botPose.avgTagDist < Constants.Vision.maxTagDistToTrust) { DRIVETRAIN.addVisionMeasurement( - pose.pose, pose.timestampSeconds, VecBuilder.fill(.7, .7, .7)); + botPose.pose, botPose.timestampSeconds, Constants.Vision.absoluteTrustVector); + return; + } + final double botPoseToPoseDistance = + botPose.pose.getTranslation().getDistance(DRIVETRAIN.getPose().getTranslation()); + // Reject a pose that is far away from the current robot pose. + if (botPoseToPoseDistance > 0.5) { + return; + } + double tagDistanceFeet = Units.metersToFeet(botPose.avgTagDist); + // Have a lower confidence with single tag pose proportionate to distance. + if (botPose.tagCount == 1) { + tagDistanceFeet *= 2; } - // Limelight.PoseEstimate poseAmp = Limelight.getBotPoseEstimate_wpiBlue(AMP_LIMELIGHT); - // if (poseAmp.tagCount >= 2) { - // DRIVETRAIN.addVisionMeasurement( - // poseAmp.pose, poseAmp.timestampSeconds, VecBuilder.fill(.9, .9, .9)); - // } - // Limelight.PoseEstimate poseRight = Limelight.getBotPoseEstimate_wpiBlue(RIGHT_LIMEKIGHT); - // if (poseRight.tagCount >= 2) { - // DRIVETRAIN.addVisionMeasurement( - // poseRight.pose, poseRight.timestampSeconds, VecBuilder.fill(1.5, 1.5,1.5)); - // } else { - // if (pose.rawFiducials.length > 0 && pose.rawFiducials[0].ambiguity < 0.07) { - // DriverStation.reportWarning( - // "ADDING POSE BASED ON AMBIGUITY OF " - // + pose.rawFiducials[0].ambiguity - // + " ON TAG " - // + pose.rawFiducials[0].id, - // false); - // DRIVETRAIN.addVisionMeasurement( - // pose.pose, pose.timestampSeconds, VecBuilder.fill(.7, .7, 9999999)); - // } - // } + double confidence = 0.7 + (tagDistanceFeet / 100); + DRIVETRAIN.addVisionMeasurement( + botPose.pose, botPose.timestampSeconds, VecBuilder.fill(confidence, confidence, 99)); } public static RobotContainer get() { diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java index fee83a4..b9bf1c2 100644 --- a/src/main/java/frc/robot/constants/Constants.java +++ b/src/main/java/frc/robot/constants/Constants.java @@ -1,7 +1,13 @@ package frc.robot.constants; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; import frc.robot.util.InterpolatingDouble; import frc.robot.util.InterpolatingTreeMap; +import frc.robot.util.RectanglePoseArea; public class Constants { @@ -297,4 +303,14 @@ public static class Climber { // DISTANCE_WRIST_ANGLE_MAP_NONELEVATOR.put( // new InterpolatingDouble(-14.3), new InterpolatingDouble(28.5)); // } + + public static class Vision { + public static final String SPEAKER_LIMELIGHT = "limelight-speaker"; + public static final RectanglePoseArea fieldBoundary = + new RectanglePoseArea(new Translation2d(0, 0), new Translation2d(16.541, 8.211)); + public static final double maxMutiTagDistToAccept = Units.feetToMeters(15.0); + public static final double maxTagDistToTrust = Units.feetToMeters(10.0); + public static final double maxSingleTagDistanceToAccept = Units.feetToMeters(10.0); + public static final Vector absoluteTrustVector = VecBuilder.fill(.2, .2, 99); + } } diff --git a/src/main/java/frc/robot/util/RectanglePoseArea.java b/src/main/java/frc/robot/util/RectanglePoseArea.java new file mode 100644 index 0000000..d82606d --- /dev/null +++ b/src/main/java/frc/robot/util/RectanglePoseArea.java @@ -0,0 +1,51 @@ +package frc.robot.util; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; + +public class RectanglePoseArea { + private final Translation2d bottomLeft; + private final Translation2d topRight; + + /** + * Create a 2D rectangular area for pose calculations. + * + * @param bottomLeft bottom left corner of the rectangle. + * @param topRight top right corner of the rectangle. + */ + public RectanglePoseArea(Translation2d bottomLeft, Translation2d topRight) { + this.bottomLeft = bottomLeft; + this.topRight = topRight; + } + + public double getMinX() { + return bottomLeft.getX(); + } + + public double getMaxX() { + return topRight.getX(); + } + + public double getMinY() { + return bottomLeft.getY(); + } + + public double getMaxY() { + return topRight.getY(); + } + + public Translation2d getBottomLeftPoint() { + return bottomLeft; + } + + public Translation2d getTopRightPoint() { + return topRight; + } + + public boolean isPoseWithinArea(Pose2d pose) { + return pose.getX() >= bottomLeft.getX() + && pose.getX() <= topRight.getX() + && pose.getY() >= bottomLeft.getY() + && pose.getY() <= topRight.getY(); + } +} From 29f4207484fbc2ff1469aa8e5c36b8546aadd905 Mon Sep 17 00:00:00 2001 From: Jonathan Smith Date: Fri, 29 Mar 2024 15:11:39 -0500 Subject: [PATCH 5/5] Include filtering LL pose by ambiguity. --- src/main/java/frc/robot/RobotContainer.java | 5 +++-- src/main/java/frc/robot/util/Util.java | 9 +++++++++ 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1c27e29..b1c3be5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -542,9 +542,10 @@ public void updatePoseVision(final String limelight) { if (!Constants.Vision.fieldBoundary.isPoseWithinArea(botPose.pose)) { return; } - // Reject pose from long disance. + // Reject pose from long disance or high ambiguity. if ((botPose.tagCount == 1 - && botPose.avgTagDist > Constants.Vision.maxSingleTagDistanceToAccept) + && (botPose.avgTagDist > Constants.Vision.maxSingleTagDistanceToAccept + || botPose.rawFiducials[0].ambiguity >= 0.9)) || (botPose.tagCount >= 2 && botPose.avgTagDist > Constants.Vision.maxMutiTagDistToAccept)) { return; diff --git a/src/main/java/frc/robot/util/Util.java b/src/main/java/frc/robot/util/Util.java index 52c5e14..b9a9886 100644 --- a/src/main/java/frc/robot/util/Util.java +++ b/src/main/java/frc/robot/util/Util.java @@ -23,6 +23,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotContainer; import frc.robot.constants.Constants; +import frc.robot.util.Limelight.RawFiducial; import java.util.List; public class Util { @@ -203,4 +204,12 @@ public static Pose2d findNearestPose(Pose2d currentPose, Pose2d... otherPoses) { public static Pose2d findNearestPoseToTrapClimbs(Pose2d currentPose) { return currentPose.nearest(TRAP_TAGS); } + + public static double maxFiducialAmbiguity(final RawFiducial[] fiducials) { + double maxAmbiguity = 0.0; + for (RawFiducial fiducial : fiducials) { + maxAmbiguity = Math.max(maxAmbiguity, fiducial.ambiguity); + } + return maxAmbiguity; + } }