Add photon vision

[aidnem]

Purpose The purpose of this addition is to add the java side of the photon vision implementation. Coprocessor will need to be configured once we get it.

Project Scope

• [x] Add localization subsystem
• [x] Add boilerplate photonlib code to approximate final version as closely as we can without a robot.
• [x] Try to simulate it.

[aidnem]

@jkleiber do we want to set up sim in a separate ticket or add it to each branch for its specific features right now?

[jkleiber]

@aidnem each branch should be able to demonstrate its features in sim. I recommend leveraging advantage scope for this

[aidnem]

@jkleiber will you be at shop tonight or should I begin HITL testing without you?

[aidnem]

@jkleiber vision simulates now but it's incredibly sketchy and the robot's heading seems to be fully random at this point. I'll work on making it actually accurate next.

https://github.com/user-attachments/assets/bd9dea6d-d4c5-48be-8ce0-3720651f19b4

[aidnem]

I'm a bit silly, the poses were outputting rotations in degrees and not radians :( As you can see, changing advantagescope to use degrees shows that the vision poses are fairly stable and consistent.

https://github.com/user-attachments/assets/32c5fe2e-dc9d-4af0-adf3-3b94719f5b8d

I'd like to see if I can force photonvision to use radians because it might be bad to have to convert manually everywhere between radians and degrees.

[aidnem]

The photon pose estimate rotation doesn't match the real robot rotation because the actual pose is using radians, so it can't be seen rotating, but the pose estimate rotation seems to be correct, as it rotates when I command it to rotate and it does so pretty smoothly and consistently.

[aidnem]

Upon further inspection, the pose from Vision/groundTruth is using radians. I had to fix the logger before I could view it.

https://github.com/user-attachments/assets/57f96220-3159-41b9-84ad-d7596b6acfe3

It also seems to be more similar to the actual pose estimate accuracy we were getting in real life last year. I've pushed these changes.