Note txty robot relative pose

[simonstoryparker]

This is how 9408 does their txty robot relative note pose:

  private Pose2d computeNotePositionRobotFrame(NoteDetectionTxTy txty) {
    // Convert tx and ty to angles, tx on the limelight does not follow RHR
    // convention, so we need to negate by one
    double thetaX = -1 * Units.degreesToRadians(txty.tx);
    double thetaY = Units.degreesToRadians(txty.ty);
    double adjustedThetaY = intakeCameraInRobotFrame.getRotation().getY() - thetaY;
    double distanceOnGround = (intakeCameraInRobotFrame.getZ() / Math.tan(adjustedThetaY))
        + intakeCameraInRobotFrame.getTranslation().getX();

    double xOffset = distanceOnGround * Math.tan(thetaX);
    Translation3d objectPositionInCameraFrame = new Translation3d(distanceOnGround, xOffset, 0.0);

    return new Pose2d(objectPositionInCameraFrame.toTranslation2d(),new Rotation2d());