Centripetal Swerve Pose Estimator

[barel13]

We can't really know where the robot is using vision, because we can see the target from every position of the field, and we would classify it the same, so we drop the position, we just know how far are we, and the angle from the target

[Se-gol]

  public Translation2d getRobotTranslation2d() {
       if (!photonCamera.hasTargets()) return null;
       double distance = getTargetDistance();
       Rotation2d gyroAngle = RobotContainer.navx.getRotation2d();
       Rotation2d relativeAngle = photonCamera.getLatestResult().getBestTarget().getCameraToTarget().getRotation();
       Rotation2d perpendicularAngle = gyroAngle.minus(relativeAngle);
       Translation2d translation2d = new Translation2d(distance * Math.cos(perpendicularAngle.getRadians()), distance * Math.sin(perpendicularAngle.getRadians()));
       return HUB_POSITION.minus(translation2d);
   }

You can use the navx to know where you're looking at the target from.

[barel13]

You can't know from the vision where are you in the field due the fact that the target has a circle shape and you can look at the target from every position (there is no notion of "I'm right to the target"), you only can know how far are you from the target, and how much you need to rotate in order to align to the target. And when you don't see the camera you can't use the data from photonvision...

[katzuv]

@Se-gol, FTFY.
@barel13, what @Se-gol says that if you roughly know your heading, you can infer what part of the vision target you see.
This doesn't mean that "radial" localization isn't helpful, it's good for aiming towards the Hub.

[barel13]

Could be helpful