From 27e000c0746301b12ac76e98cb723e1c76438913 Mon Sep 17 00:00:00 2001 From: Otto Watson-Brown Date: Wed, 12 Nov 2025 12:28:49 -0600 Subject: [PATCH 1/9] add finder class --- .../frc/robot/Subsystems/Drive/Drive.java | 34 ++------ .../Subsystems/Drive/DriveConstants.java | 3 - .../GamePieceFinder/GamePieceFinder.java | 85 +++++++++++++++++++ .../frc/robot/Subsystems/Vision/Vision.java | 5 +- 4 files changed, 92 insertions(+), 35 deletions(-) create mode 100644 src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java diff --git a/src/main/java/frc/robot/Subsystems/Drive/Drive.java b/src/main/java/frc/robot/Subsystems/Drive/Drive.java index 43c5db4..daa48aa 100644 --- a/src/main/java/frc/robot/Subsystems/Drive/Drive.java +++ b/src/main/java/frc/robot/Subsystems/Drive/Drive.java @@ -101,21 +101,12 @@ public void runState() { } logOutputs(driveIO.getDrive().getState()); - if (!DRIVER_CONTROLLER.getAButton()) { - getState().driveRobot(); - } else { - if (vision.getYawToTarget() != null) - { - Matrix robotToFRMatrix = ROBOT_TO_FRONT_RIGHT_CAMERA.toMatrix(); - Matrix cameraToObj= new Transform3d(Translation3d.kZero, new Rotation3d(0,0, vision.getYawToTarget().in(Radians))).toMatrix(); - Matrix robotToObj = robotToFRMatrix.times(cameraToObj); - double currentAngle = getDriveTrain().getPigeon2().getYaw().getValueAsDouble() % 360; - // double wantedAngle = currentAngle - (vision.getYawToTarget().in(Degree) + ROBOT_TO_FRONT_RIGHT_CAMERA_ROTATION.getZ()); - - driveFieldRelative(0, 0, .1 * thetaController.calculate(currentAngle, new Transform3d(robotToObj).getRotation().getMeasureZ().in(Degree))); - } - } + + + + + field.setRobotPose(getPose()); SmartDashboard.putData("Field", field); } @@ -153,13 +144,6 @@ public void zeroGyro() { driveIO.zeroGyro(); } - // SYSId Trash (no hate ofc) - public enum SysIdMode { - TRANSLATION, - STEER, - ROTATION, - } - // Util public Pose2d getPose() { return driveIO.getDrive().getState().Pose; @@ -176,12 +160,4 @@ public LinearVelocity getVelocity() { public TunerSwerveDrivetrain getDriveTrain() { return driveIO.getDrive(); } - - public void addVisionMeasurement(Pose2d visionPose, double timestamp, Matrix visionMeasurementStdDevs) { - if (ROBOT_MODE == RobotMode.REAL) { - driveIO.addVisionMeasurement(visionPose, Utils.fpgaToCurrentTime(timestamp), visionMeasurementStdDevs); - } else { - driveIO.addVisionMeasurement(visionPose, timestamp, visionMeasurementStdDevs); - } - } } diff --git a/src/main/java/frc/robot/Subsystems/Drive/DriveConstants.java b/src/main/java/frc/robot/Subsystems/Drive/DriveConstants.java index e214f84..1552ab5 100644 --- a/src/main/java/frc/robot/Subsystems/Drive/DriveConstants.java +++ b/src/main/java/frc/robot/Subsystems/Drive/DriveConstants.java @@ -11,7 +11,6 @@ import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearAcceleration; import edu.wpi.first.units.measure.LinearVelocity; -import frc.robot.Subsystems.Drive.Drive.SysIdMode; public class DriveConstants { @@ -35,8 +34,6 @@ public class DriveConstants { public static final LinearVelocity TIPPING_LIMITER_THRESHOLD = MetersPerSecond.of(3); - // Change to change the sysID test that gets run for drive - public static final SysIdMode SYS_ID_MODE = SysIdMode.STEER; public static final String SUBSYSTEM_NAME = "Drive"; // For zeroing on robot init diff --git a/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java b/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java new file mode 100644 index 0000000..9d015b4 --- /dev/null +++ b/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java @@ -0,0 +1,85 @@ +package frc.robot.Subsystems.GamePieceFinder; + +import static edu.wpi.first.units.Units.*; + +import java.util.Queue; +import java.util.concurrent.atomic.AtomicReference; + +import org.photonvision.targeting.PhotonTrackedTarget; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.units.measure.Time; + +public class GamePieceFinder { + + private static AtomicReference instance; + + private Queue parllaxSamples; + private Queue visionSamples; + + private Pose2d estimate; + + private final Time ALLOWED_TIMESTAMP_DEVIATION = Seconds.of(0.2); + + private class GamePieceParallaxSample { + public Pose2d robotPose; + public PhotonTrackedTarget visionSample; + public Time timestamp; + + public GamePieceParallaxSample(Pose2d robotPose, Time timestamp, PhotonTrackedTarget visionSample) { + this.robotPose = robotPose; + this.visionSample = visionSample; + this.timestamp = timestamp; + } + } + + private class GamePieceVisionSample { + public PhotonTrackedTarget visionSample; + public Time timestamp; + + public GamePieceVisionSample(Time timestamp, PhotonTrackedTarget visionSample) { + this.visionSample = visionSample; + this.timestamp = timestamp; + } + } + + public static GamePieceFinder getInstance() { + if (instance.get() == null) { + instance.set(new GamePieceFinder()); + } + return instance.get(); + } + + private GamePieceFinder() { + + } + + public void addVisionSample(PhotonTrackedTarget sample) { + visionSamples.add(new GamePieceVisionSample(Milliseconds.of(System.currentTimeMillis()), sample)); + } + + public void addDrivePose(Pose2d pose, Time timestamp) { + for (GamePieceVisionSample t : visionSamples) { + if (Math.abs(t.timestamp.in(Milliseconds) - timestamp.in(Milliseconds)) < ALLOWED_TIMESTAMP_DEVIATION.in(Milliseconds)) { + parllaxSamples.add(new GamePieceParallaxSample(pose, timestamp, t.visionSample)); + visionSamples.remove(t); + continue; + } + } + + } + + public Pose2d getLatestGamepieceEstimate() { + return estimate; + } + + private void updateEstimate() { + // Parallax Calc Here + } + + public void Periodic() { + if (parllaxSamples.size() > 1) { + updateEstimate(); + } + } +} diff --git a/src/main/java/frc/robot/Subsystems/Vision/Vision.java b/src/main/java/frc/robot/Subsystems/Vision/Vision.java index bcd3bdb..e310fc5 100644 --- a/src/main/java/frc/robot/Subsystems/Vision/Vision.java +++ b/src/main/java/frc/robot/Subsystems/Vision/Vision.java @@ -13,8 +13,8 @@ public class Vision extends SubsystemBase { private PhotonCamera camera; private static Vision instance; - private Angle targetYaw; - + private Angle targetYaw; + public static Vision getInstance() { if (instance == null) { instance = new Vision(FRONT_RIGHT_CAM_NAME); @@ -41,7 +41,6 @@ public void periodic() { if (target != null) { targetYaw = Degrees.of(target.getYaw()); System.out.println(targetYaw.in(Degrees)); - } } } } From d105254c5bbea2e4f683b7031eeb07ca469804e6 Mon Sep 17 00:00:00 2001 From: Otto Watson-Brown Date: Wed, 12 Nov 2025 12:28:58 -0600 Subject: [PATCH 2/9] fix smth --- src/main/java/frc/robot/Subsystems/Vision/Vision.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/Subsystems/Vision/Vision.java b/src/main/java/frc/robot/Subsystems/Vision/Vision.java index e310fc5..26746ad 100644 --- a/src/main/java/frc/robot/Subsystems/Vision/Vision.java +++ b/src/main/java/frc/robot/Subsystems/Vision/Vision.java @@ -43,4 +43,5 @@ public void periodic() { System.out.println(targetYaw.in(Degrees)); } } + } } From 2fc0a6cfa633d8b53eb2e582642447126a123f7a Mon Sep 17 00:00:00 2001 From: Otto Watson-Brown Date: Wed, 12 Nov 2025 21:01:36 -0600 Subject: [PATCH 3/9] Little bit of this little bit of that --- .../GamePieceFinder/GamePieceFinder.java | 151 ++++++++++++++---- 1 file changed, 116 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java b/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java index 9d015b4..6d28fe7 100644 --- a/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java +++ b/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java @@ -2,44 +2,55 @@ import static edu.wpi.first.units.Units.*; -import java.util.Queue; +import java.util.*; import java.util.concurrent.atomic.AtomicReference; import org.photonvision.targeting.PhotonTrackedTarget; -import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.*; import edu.wpi.first.units.measure.Time; +import frc.robot.Subsystems.Drive.Drive; public class GamePieceFinder { - private static AtomicReference instance; - - private Queue parllaxSamples; - private Queue visionSamples; + private static final AtomicReference instance = new AtomicReference<>(); + + private final Queue parallaxSamples = new LinkedList<>(); + private final List confirmedPieces = new ArrayList<>(); + + private final Time SAMPLE_EXPIRATION = Seconds.of(10.0); + + private final double MIN_YAW_DIFFERENCE_DEG = 10.0; + private final double AREA_DISTANCE_TOLERANCE = 0.3; // 30% + + private final Translation2d CAMERA_OFFSET = new Translation2d(0.0, 0.0); - private Pose2d estimate; + private Pose2d latestEstimate; - private final Time ALLOWED_TIMESTAMP_DEVIATION = Seconds.of(0.2); + private record Ray2d(Translation2d origin, Translation2d dir, double length, double area) {} private class GamePieceParallaxSample { - public Pose2d robotPose; - public PhotonTrackedTarget visionSample; - public Time timestamp; + public final Pose2d robotPose; + public final PhotonTrackedTarget visionSample; + public final Time timestamp; public GamePieceParallaxSample(Pose2d robotPose, Time timestamp, PhotonTrackedTarget visionSample) { this.robotPose = robotPose; this.visionSample = visionSample; this.timestamp = timestamp; } - } - private class GamePieceVisionSample { - public PhotonTrackedTarget visionSample; - public Time timestamp; + public Ray2d toRay() { + double yawDeg = visionSample.getYaw(); + double distance = estimateDistanceFromArea(visionSample.getArea()); - public GamePieceVisionSample(Time timestamp, PhotonTrackedTarget visionSample) { - this.visionSample = visionSample; - this.timestamp = timestamp; + Translation2d cameraPos = robotPose.transformBy(new Transform2d(CAMERA_OFFSET, new Rotation2d())).getTranslation(); + + // Wait lwky is the getYaw function in deg + double globalAngle = robotPose.getRotation().getRadians() + Math.toRadians(yawDeg); + Translation2d dir = new Translation2d(Math.cos(globalAngle), Math.sin(globalAngle)); + + return new Ray2d(cameraPos, dir, distance, visionSample.getArea()); } } @@ -50,36 +61,106 @@ public static GamePieceFinder getInstance() { return instance.get(); } - private GamePieceFinder() { + private GamePieceFinder() {} + + public void addVisionSample(PhotonTrackedTarget sample) { + parallaxSamples.add(new GamePieceParallaxSample(Drive.getInstance().getPose(), Milliseconds.of(System.currentTimeMillis()), sample)); } - public void addVisionSample(PhotonTrackedTarget sample) { - visionSamples.add(new GamePieceVisionSample(Milliseconds.of(System.currentTimeMillis()), sample)); + public List getDetectedPiecesSortedByDistance(Pose2d robotPose) { + return confirmedPieces.stream() + .sorted(Comparator.comparingDouble(p -> + p.getTranslation().getDistance(robotPose.getTranslation()))) + .toList(); + } + + public Pose2d getLatestGamepieceEstimate() { + return latestEstimate; + } + + public void periodic() { + cleanupOldSamples(); + if (parallaxSamples.size() > 1) updateEstimates(); } - public void addDrivePose(Pose2d pose, Time timestamp) { - for (GamePieceVisionSample t : visionSamples) { - if (Math.abs(t.timestamp.in(Milliseconds) - timestamp.in(Milliseconds)) < ALLOWED_TIMESTAMP_DEVIATION.in(Milliseconds)) { - parllaxSamples.add(new GamePieceParallaxSample(pose, timestamp, t.visionSample)); - visionSamples.remove(t); - continue; + + private void updateEstimates() { + List samples = new ArrayList<>(parallaxSamples); + Set toRemove = new HashSet<>(); + + for (int i = 0; i < samples.size(); i++) { + for (int j = i + 1; j < samples.size(); j++) { + var s1 = samples.get(i); + var s2 = samples.get(j); + + double yawDiff = Math.abs(s1.visionSample.getYaw() - s2.visionSample.getYaw()); + if (yawDiff < MIN_YAW_DIFFERENCE_DEG) continue; + + Ray2d r1 = s1.toRay(); + Ray2d r2 = s2.toRay(); + + Optional intersection = intersectRays(r1, r2); + // Chat will it continue if the first condition isnt met and then leave my thingy that might get a null pointer alone + if (intersection.isPresent() && + areasAtIntersection(r1, r2, intersection.get())) { + + Pose2d found = new Pose2d(intersection.get(), new Rotation2d()); + confirmedPieces.add(found); + latestEstimate = found; + toRemove.add(s1); + toRemove.add(s2); + } } } + parallaxSamples.removeAll(toRemove); + } + private void cleanupOldSamples() { + double now = System.currentTimeMillis(); + parallaxSamples.removeIf(s -> + now - s.timestamp.in(Milliseconds) > SAMPLE_EXPIRATION.in(Milliseconds)); } - public Pose2d getLatestGamepieceEstimate() { - return estimate; + // Yeah no idea what this relationship is gona be, linear, sqrt under, 6 or 7 + private double estimateDistanceFromArea(double area) { + double k = 6.7; + return k / Math.sqrt(Math.max(area, 0.2)); } - private void updateEstimate() { - // Parallax Calc Here + private boolean areasAtIntersection(Ray2d r1, Ray2d r2, Translation2d intersection) { + double distAlongR1 = intersection.minus(r1.origin()).getNorm(); + double distAlongR2 = intersection.minus(r2.origin()).getNorm(); + + double predictedR1 = r1.length(); + double predictedR2 = r2.length(); + + boolean ok1 = Math.abs(distAlongR1 - predictedR1) / predictedR1 < AREA_DISTANCE_TOLERANCE; + boolean ok2 = Math.abs(distAlongR2 - predictedR2) / predictedR2 < AREA_DISTANCE_TOLERANCE; + + return ok1 && ok2; } - public void Periodic() { - if (parllaxSamples.size() > 1) { - updateEstimate(); - } + // Silly vector intersection solution, idrk if this is optimal or works @william feel free to replace with your calc + private Optional intersectRays(Ray2d r1, Ray2d r2) { + double x1 = r1.origin().getX(), y1 = r1.origin().getY(); + double x2 = x1 + r1.dir().getX(), y2 = y1 + r1.dir().getY(); + double x3 = r2.origin().getX(), y3 = r2.origin().getY(); + double x4 = x3 + r2.dir().getX(), y4 = y3 + r2.dir().getY(); + + double denominator = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4); + if (Math.abs(denominator) < 1e-2) return Optional.empty(); // if theyre parallel then throw ts out + + double px = ((x1*y2 - y1*x2)*(x3 - x4) - (x1 - x2)*(x3*y4 - y3*x4)) / denominator; + double py = ((x1*y2 - y1*x2)*(y3 - y4) - (y1 - y2)*(x3*y4 - y3*x4)) / denominator; + + Translation2d p = new Translation2d(px, py); + + // Ignore balls that are fall away + if (p.minus(r1.origin()).getNorm() > r1.length() * 1.5 || + p.minus(r2.origin()).getNorm() > r2.length() * 1.5) + return Optional.empty(); + + return Optional.of(p); } } From 8f035dabec1928cb9dd120e024c38b179f46dcbf Mon Sep 17 00:00:00 2001 From: William Zhong Date: Wed, 12 Nov 2025 22:13:07 -0600 Subject: [PATCH 4/9] added parallax calculations --- .../GamePieceFinder/GamePieceFinder.java | 60 ++++++++++++++++--- 1 file changed, 51 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java b/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java index 6d28fe7..6da3583 100644 --- a/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java +++ b/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java @@ -1,6 +1,7 @@ package frc.robot.Subsystems.GamePieceFinder; import static edu.wpi.first.units.Units.*; +import static frc.robot.Subsystems.Vision.VisionConstants.*; import java.util.*; import java.util.concurrent.atomic.AtomicReference; @@ -8,6 +9,8 @@ import org.photonvision.targeting.PhotonTrackedTarget; import edu.wpi.first.math.geometry.*; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Time; import frc.robot.Subsystems.Drive.Drive; @@ -27,7 +30,7 @@ public class GamePieceFinder { private Pose2d latestEstimate; - private record Ray2d(Translation2d origin, Translation2d dir, double length, double area) {} + private record Ray2d(Translation2d origin, Translation2d dir, double length, double area, Angle measuredAngle, Pose2d robotPose) {} private class GamePieceParallaxSample { public final Pose2d robotPose; @@ -50,7 +53,7 @@ public Ray2d toRay() { double globalAngle = robotPose.getRotation().getRadians() + Math.toRadians(yawDeg); Translation2d dir = new Translation2d(Math.cos(globalAngle), Math.sin(globalAngle)); - return new Ray2d(cameraPos, dir, distance, visionSample.getArea()); + return new Ray2d(cameraPos, dir, distance, visionSample.getArea(), Degrees.of(yawDeg), robotPose); } } @@ -91,21 +94,23 @@ private void updateEstimates() { for (int i = 0; i < samples.size(); i++) { for (int j = i + 1; j < samples.size(); j++) { - var s1 = samples.get(i); - var s2 = samples.get(j); + //I need the "first" sample to have the smaller rotation so that I can set it as 0 and go off of that for calculations + //TODO: Logic will mess up if rotation can be negative, so need to confirm that its not/deal with it if it is + //TODO: Probably better way to implement this logic lol + var s1 = samples.get(i).robotPose.getRotation().getDegrees() < samples.get(j).robotPose.getRotation().getDegrees() ? samples.get(i) : samples.get(j); + var s2 = samples.get(i).robotPose.getRotation().getDegrees() > samples.get(j).robotPose.getRotation().getDegrees() ? samples.get(i) : samples.get(j); - double yawDiff = Math.abs(s1.visionSample.getYaw() - s2.visionSample.getYaw()); + double yawDiff = Math.abs(s2.visionSample.getYaw() - s1.visionSample.getYaw()); if (yawDiff < MIN_YAW_DIFFERENCE_DEG) continue; Ray2d r1 = s1.toRay(); Ray2d r2 = s2.toRay(); - Optional intersection = intersectRays(r1, r2); + Translation2d objectPoint = getPoseThroughParallax(r1, r2); // Chat will it continue if the first condition isnt met and then leave my thingy that might get a null pointer alone - if (intersection.isPresent() && - areasAtIntersection(r1, r2, intersection.get())) { + if (areasAtIntersection(r1, r2, objectPoint)) { - Pose2d found = new Pose2d(intersection.get(), new Rotation2d()); + Pose2d found = new Pose2d(objectPoint, new Rotation2d()); confirmedPieces.add(found); latestEstimate = found; toRemove.add(s1); @@ -163,4 +168,41 @@ private Optional intersectRays(Ray2d r1, Ray2d r2) { return Optional.of(p); } + + //TODO: Wait what if the bot has moved its center between samples? Is that gonna be a problem + private Translation2d getPoseThroughParallax(Ray2d r1, Ray2d r2) { + Angle deltaYaw = Degrees.of(r2.robotPose().getRotation().minus(r1.robotPose().getRotation()).getDegrees()); + + Translation2d firstCameraPos = ROBOT_TO_FRONT_RIGHT_CAMERA_TRANSLATION.toTranslation2d(); + //TODO: Need to check if this rotates the right way - should be to the left + Translation2d secondCameraPos = firstCameraPos.rotateAround(Translation2d.kZero, Rotation2d.fromDegrees(deltaYaw.in(Degrees))); + + Translation2d vecBtwnCameraPos = firstCameraPos.minus(secondCameraPos); + + Angle cameraRot = ROBOT_TO_FRONT_RIGHT_CAMERA_ROTATION.getMeasureZ(); + Angle angleOffset = Radians.of(Math.atan2(firstCameraPos.getY(), firstCameraPos.getX())); + Angle firstYaw = r1.measuredAngle; + Angle secondYaw = r2.measuredAngle; + + //TODO: There's some redudant math here, need to come back and simplify/clean it up later + Angle alpha = Radians.of(Math.atan2(vecBtwnCameraPos.getY(), vecBtwnCameraPos.getX())); + Angle beta = Degrees.of(90 - alpha.in(Degrees)); + Angle A = Degrees.of(cameraRot.in(Degrees) - firstYaw.in(Degrees) - alpha.in(Degrees)); + Angle B = Degrees.of(360 - (cameraRot.in(Degrees) - secondYaw.in(Degrees) + angleOffset.in(Degrees) + ((180 - deltaYaw.in(Degrees))/2 - beta.in(Degrees)))); + Angle C = Degrees.of(180 - A.in(Degrees) - B.in(Degrees)); + + //TODO: Need to make sure coordinate system matches (im assuming right is positive and up is positive) + Distance b = Meters.of((vecBtwnCameraPos.getDistance(Translation2d.kZero)/Math.sin(C.in(Radians)))*Math.sin(A.in(Radians))); + Translation2d robotToObject = new Translation2d( + -b.in(Meters)*Math.cos(A.in(Radians) + alpha.in(Radians)) + firstCameraPos.getMeasureX().in(Meters), + b.in(Meters)*Math.sin(A.in(Radians) + alpha.in(Radians)) + firstCameraPos.getMeasureY().in(Meters) + ); + + //I did the above calculations assuming the robot was facing straight forward, so now I need to rotate the point to match the rotation of the robot + //I could change it to work for any initial direction of the bot in my calculations but whatever + //TODO: I assume it's gonna rotate in the right direction? + robotToObject = robotToObject.rotateAround(Translation2d.kZero, r1.robotPose.getRotation()); + + return r1.robotPose.getTranslation().plus(robotToObject); + } } From ad3f926d7d4f0790979924b018dbb7a6ce30e6af Mon Sep 17 00:00:00 2001 From: Otto Watson-Brown Date: Fri, 14 Nov 2025 09:36:07 -0600 Subject: [PATCH 5/9] Little bit of cleanup --- .../Subsystems/GamePieceFinder/GamePieceFinder.java | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java b/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java index 6da3583..61fa820 100644 --- a/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java +++ b/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java @@ -9,6 +9,7 @@ import org.photonvision.targeting.PhotonTrackedTarget; import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Time; @@ -28,6 +29,8 @@ public class GamePieceFinder { private final Translation2d CAMERA_OFFSET = new Translation2d(0.0, 0.0); + private final Rotation2d CAMERA_ROTATION_OFFSET = new Rotation2d(Units.degreesToRadians(27.8)); + private Pose2d latestEstimate; private record Ray2d(Translation2d origin, Translation2d dir, double length, double area, Angle measuredAngle, Pose2d robotPose) {} @@ -47,9 +50,8 @@ public Ray2d toRay() { double yawDeg = visionSample.getYaw(); double distance = estimateDistanceFromArea(visionSample.getArea()); - Translation2d cameraPos = robotPose.transformBy(new Transform2d(CAMERA_OFFSET, new Rotation2d())).getTranslation(); + Translation2d cameraPos = robotPose.transformBy(new Transform2d(CAMERA_OFFSET, CAMERA_ROTATION_OFFSET)).getTranslation(); - // Wait lwky is the getYaw function in deg double globalAngle = robotPose.getRotation().getRadians() + Math.toRadians(yawDeg); Translation2d dir = new Translation2d(Math.cos(globalAngle), Math.sin(globalAngle)); @@ -87,6 +89,11 @@ public void periodic() { if (parallaxSamples.size() > 1) updateEstimates(); } + public void clearPoseEstimates() { + confirmedPieces.clear(); + latestEstimate = null; + } + private void updateEstimates() { List samples = new ArrayList<>(parallaxSamples); From dd82b9ac71b2c79c2b807af8bda751708d0a75b7 Mon Sep 17 00:00:00 2001 From: Otto Watson-Brown Date: Fri, 14 Nov 2025 11:51:46 -0600 Subject: [PATCH 6/9] Reimplement intersection soluton --- .../robot/Subsystems/GamePieceFinder/GamePieceFinder.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java b/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java index 61fa820..420a526 100644 --- a/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java +++ b/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java @@ -113,11 +113,11 @@ private void updateEstimates() { Ray2d r1 = s1.toRay(); Ray2d r2 = s2.toRay(); - Translation2d objectPoint = getPoseThroughParallax(r1, r2); + Optional objectPoint = intersectRays(r1, r2); // Chat will it continue if the first condition isnt met and then leave my thingy that might get a null pointer alone - if (areasAtIntersection(r1, r2, objectPoint)) { + if (!objectPoint.isEmpty() && areasAtIntersection(r1, r2, objectPoint.get())) { - Pose2d found = new Pose2d(objectPoint, new Rotation2d()); + Pose2d found = new Pose2d(objectPoint.get(), new Rotation2d()); confirmedPieces.add(found); latestEstimate = found; toRemove.add(s1); From 29613a3ba3bef647863864547980ef2c33de6601 Mon Sep 17 00:00:00 2001 From: Otto Watson-Brown Date: Mon, 17 Nov 2025 16:17:49 -0600 Subject: [PATCH 7/9] Implemented Ts --- src/main/java/frc/robot/Robot.java | 2 ++ .../frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java | 4 ++++ src/main/java/frc/robot/Subsystems/Vision/Vision.java | 3 ++- 3 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 928668b..3b8d823 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.Subsystems.Drive.Drive; +import frc.robot.Subsystems.GamePieceFinder.GamePieceFinder; import frc.robot.Subsystems.Vision.Vision; /** @@ -28,6 +29,7 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); Vision.getInstance().periodic(); Drive.getInstance().periodic(); + GamePieceFinder.getInstance().periodic(); } @Override diff --git a/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java b/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java index 420a526..19c8cd8 100644 --- a/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java +++ b/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java @@ -5,6 +5,7 @@ import java.util.*; import java.util.concurrent.atomic.AtomicReference; +import org.littletonrobotics.junction.Logger; import org.photonvision.targeting.PhotonTrackedTarget; @@ -13,6 +14,7 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Time; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Subsystems.Drive.Drive; public class GamePieceFinder { @@ -87,6 +89,8 @@ public Pose2d getLatestGamepieceEstimate() { public void periodic() { cleanupOldSamples(); if (parallaxSamples.size() > 1) updateEstimates(); + Logger.recordOutput("GamePieceFinder/ConfirmedPieces", confirmedPieces.size()); + Logger.recordOutput("GamePieceFinder/Pose", getLatestGamepieceEstimate()); } public void clearPoseEstimates() { diff --git a/src/main/java/frc/robot/Subsystems/Vision/Vision.java b/src/main/java/frc/robot/Subsystems/Vision/Vision.java index 26746ad..5b754f9 100644 --- a/src/main/java/frc/robot/Subsystems/Vision/Vision.java +++ b/src/main/java/frc/robot/Subsystems/Vision/Vision.java @@ -7,6 +7,7 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Subsystems.GamePieceFinder.GamePieceFinder; public class Vision extends SubsystemBase { @@ -37,7 +38,7 @@ public void periodic() { for (var result : results) { var target = result.getBestTarget(); - + GamePieceFinder.getInstance().addVisionSample(target); if (target != null) { targetYaw = Degrees.of(target.getYaw()); System.out.println(targetYaw.in(Degrees)); From cd969132ea538228fc8e20ebd42bfb385fcfc2b8 Mon Sep 17 00:00:00 2001 From: Otto Watson-Brown Date: Mon, 1 Dec 2025 09:40:34 -0600 Subject: [PATCH 8/9] Not a ton of clogging samples anymore??? hopefully --- .../frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java b/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java index 19c8cd8..573a553 100644 --- a/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java +++ b/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java @@ -71,6 +71,10 @@ public static GamePieceFinder getInstance() { private GamePieceFinder() {} public void addVisionSample(PhotonTrackedTarget sample) { + // Order of if statements matters here + if (parallaxSamples.size() > 0 && Math.abs(sample.getYaw() - parallaxSamples.element().visionSample.getYaw())< MIN_YAW_DIFFERENCE_DEG) { + return; + } parallaxSamples.add(new GamePieceParallaxSample(Drive.getInstance().getPose(), Milliseconds.of(System.currentTimeMillis()), sample)); } From 86c65fa6314a02f095abd74055b5b3df62c9ad11 Mon Sep 17 00:00:00 2001 From: Johnny Date: Mon, 1 Dec 2025 18:45:28 -0600 Subject: [PATCH 9/9] 12/1 testing --- src/main/java/frc/robot/Robot.java | 11 ++++++++-- .../GamePieceFinder/GamePieceFinder.java | 20 ++++++++++++------- .../frc/robot/Subsystems/Vision/Vision.java | 5 ++--- 3 files changed, 24 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3b8d823..3f37fb8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,24 +4,31 @@ package frc.robot; +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.NT4Publisher; + import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.Subsystems.Drive.Drive; import frc.robot.Subsystems.GamePieceFinder.GamePieceFinder; -import frc.robot.Subsystems.Vision.Vision; +import frc.robot.Subsystems.Vision.Vision; + /** * The methods in this class are called automatically corresponding to each mode, as described in * the TimedRobot documentation. If you change the name of this class or the package after creating * this project, you must also update the Main.java file in the project. */ -public class Robot extends TimedRobot { +public class Robot extends LoggedRobot { /** * This function is run when the robot is first started up and should be used for any * initialization code. */ public Robot() { Drive.getInstance().zeroGyro(); + Logger.addDataReceiver(new NT4Publisher()); + Logger.start(); } @Override diff --git a/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java b/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java index 573a553..34bd832 100644 --- a/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java +++ b/src/main/java/frc/robot/Subsystems/GamePieceFinder/GamePieceFinder.java @@ -21,7 +21,7 @@ public class GamePieceFinder { private static final AtomicReference instance = new AtomicReference<>(); - private final Queue parallaxSamples = new LinkedList<>(); + private final Deque parallaxSamples = new LinkedList<>(); private final List confirmedPieces = new ArrayList<>(); private final Time SAMPLE_EXPIRATION = Seconds.of(10.0); @@ -72,7 +72,7 @@ private GamePieceFinder() {} public void addVisionSample(PhotonTrackedTarget sample) { // Order of if statements matters here - if (parallaxSamples.size() > 0 && Math.abs(sample.getYaw() - parallaxSamples.element().visionSample.getYaw())< MIN_YAW_DIFFERENCE_DEG) { + if (parallaxSamples.size() > 0 && Math.abs(sample.getYaw() - parallaxSamples.peekLast().visionSample.getYaw())< MIN_YAW_DIFFERENCE_DEG) { return; } parallaxSamples.add(new GamePieceParallaxSample(Drive.getInstance().getPose(), Milliseconds.of(System.currentTimeMillis()), sample)); @@ -95,6 +95,7 @@ public void periodic() { if (parallaxSamples.size() > 1) updateEstimates(); Logger.recordOutput("GamePieceFinder/ConfirmedPieces", confirmedPieces.size()); Logger.recordOutput("GamePieceFinder/Pose", getLatestGamepieceEstimate()); + Logger.recordOutput("GamePiceceFinder/VisionSamples", parallaxSamples.size()); } public void clearPoseEstimates() { @@ -112,8 +113,8 @@ private void updateEstimates() { //I need the "first" sample to have the smaller rotation so that I can set it as 0 and go off of that for calculations //TODO: Logic will mess up if rotation can be negative, so need to confirm that its not/deal with it if it is //TODO: Probably better way to implement this logic lol - var s1 = samples.get(i).robotPose.getRotation().getDegrees() < samples.get(j).robotPose.getRotation().getDegrees() ? samples.get(i) : samples.get(j); - var s2 = samples.get(i).robotPose.getRotation().getDegrees() > samples.get(j).robotPose.getRotation().getDegrees() ? samples.get(i) : samples.get(j); + var s1 = samples.get(i); + var s2 = samples.get(j); double yawDiff = Math.abs(s2.visionSample.getYaw() - s1.visionSample.getYaw()); if (yawDiff < MIN_YAW_DIFFERENCE_DEG) continue; @@ -121,11 +122,15 @@ private void updateEstimates() { Ray2d r1 = s1.toRay(); Ray2d r2 = s2.toRay(); - Optional objectPoint = intersectRays(r1, r2); + Logger.recordOutput("R1", r1.dir); + Logger.recordOutput("R2", r2.dir); + + Translation2d objectPoint = getPoseThroughParallax(r1, r2); + System.out.println(objectPoint); // Chat will it continue if the first condition isnt met and then leave my thingy that might get a null pointer alone - if (!objectPoint.isEmpty() && areasAtIntersection(r1, r2, objectPoint.get())) { + if (areasAtIntersection(r1, r2, objectPoint)) { - Pose2d found = new Pose2d(objectPoint.get(), new Rotation2d()); + Pose2d found = new Pose2d(objectPoint, new Rotation2d()); confirmedPieces.add(found); latestEstimate = found; toRemove.add(s1); @@ -175,6 +180,7 @@ private Optional intersectRays(Ray2d r1, Ray2d r2) { double py = ((x1*y2 - y1*x2)*(y3 - y4) - (y1 - y2)*(x3*y4 - y3*x4)) / denominator; Translation2d p = new Translation2d(px, py); + System.out.println(p); // Ignore balls that are fall away if (p.minus(r1.origin()).getNorm() > r1.length() * 1.5 || diff --git a/src/main/java/frc/robot/Subsystems/Vision/Vision.java b/src/main/java/frc/robot/Subsystems/Vision/Vision.java index 5b754f9..15ad461 100644 --- a/src/main/java/frc/robot/Subsystems/Vision/Vision.java +++ b/src/main/java/frc/robot/Subsystems/Vision/Vision.java @@ -38,11 +38,10 @@ public void periodic() { for (var result : results) { var target = result.getBestTarget(); - GamePieceFinder.getInstance().addVisionSample(target); if (target != null) { + GamePieceFinder.getInstance().addVisionSample(target); targetYaw = Degrees.of(target.getYaw()); - System.out.println(targetYaw.in(Degrees)); + } } } - } }