Align to double substation pickup

[UserC2]

Can either use AprilTags on double substation for alignment and positioning (probably overly complicated) or align with gyro angle.

Limelight:

• Find field-relative position using AprilTags
• Plan path from current position to field-relative position (See Drive-To-Pose and limelight branches)
• If field-relative position is unknown, use odometry once it is initially set by limelight (limelight branch already does this)
• Make sure to have two paths for left/right double substation (1114 didn't do this and they could only score on the left double substation...)

Gyro Angle:

• Find current gyro angle from SwerveDriveOdometry
• Use a PID controller (P of 1 is probably fine...) on the gyro angle and use output to control robot rotational velocity.

[UserC2]

If doing for McMaster, align to it using the gyro. Do not implement the limelight version yet. (Will take too much time.)

[beaverworks2609]

Gyro angle version is implemented in branch vision-align.

[UserC2]

Closed, see #44, already merged to main