Add vision subsystem

[saikiranra]

We need to update the vision subsystem to account for 2 limelights. The current plan for this robot is to have 2 limelights mounted above and to the left and the right of the shooter, statically. These 2 limelights will be angled inwards and slightly up.

Questions to answer before we write code for it:

1. How do we want to combine the poses from both cameras?
2. How will interpolated vision work/be initially calibrated?
3. How will any logic of gating shots/lights/etc work with 2 cameras? Do we check min 1 is up?

[jonahsnider]

Adam provided some information on LL position in the robot, which will be used once we're doing bringup on the cameras and configuring the camera to robot transform.

[jonahsnider]

Also writing here before I forget:

• Probably want to make a Limelight class for containing lower-level logic for interfacing with the cameras directly
  • This means the vision subsystem can focus on just fusing the data from each camera together & doing higher-level computations with vision data
  • Having a shared class for the cameras seems useful since they're doing basically the exact same logic (for parsing & validating LL pose, streaming IMU data, doing health checks with LL heartbeat, etc.)
• For interpolated vision, I believe the approach we discussed was
  1. Get raw poses from each camera
  2. Have a separate set of interpolation data points for each camera, use those to interpolate the raw vision poses
  3. Take each interpolated pose and add it to the pose estimator individually