Camera assisted intaking

[jonahsnider]

Use a note tracking Limelight to have driver assist features when intaking notes.

[saikiranra]

We need to split this behavior up into auto behavior and teleop behavior.

For teleop, we will want to do something simple, where

intake_v = speed robot is moving in the direction of the intake
tx = note tx angle, which is angle from the center

robot_translate_v = p * intake_v * tx

Where p is tuned and robot_translate is added to the commanded robot translate.

[saikiranra]

I think the implementation we should try out is when we are in assisted intake mode, we get the requested vector of movement relative to the field and compare it to the angle the note is from the robot with tx. We subtract the two angles, multiply it by p and find the new x y speeds with the previous magnitude and the new angle. The angular change should be clamped to some tuned amount (35 degrees seems good for now).

Translation2d requested_field_relative_drive = whatever the driver requests
Rotation2d note_angle_relative_to_field = robot angle + note tx (might be minus, you need to thinf this through)
Float angle_error = requested_field_relative_drive.rotateBy(note_angle_relative_to_field).degrees()
Float angle_change = max(-35, min(35, angle_error * p))
Translation2d new_drive_request = requested_field_relative_drive.rotateBy(rotation2d.fromDegrees(angle_change))