Made goalie line up with ball and kicker in penalty

[franklin-yuan]

Description

Made goalie line up with the ball and the closest enemy robot to the ball

Associated / Resolved Issue

Resolves #86b6ubvaq or ClickUp card

Design Documents

Link

Steps to Test

Test Case 1

Setup penalty, set goalie state to penalty

Expected result:
Goalie should move automatically between ball and opposing kicker

Key Files to Review

Group 1

• File 1
• File 2

Group 2

• File 3
• File 4

Review Checklist

• Docstrings: All methods and classes should have the file appropriate docstrings which follow the guidelines in the "Contributing" page of our docs.
• Remove extra print statements: Any print statements used for debugging should be removed
• Tag reviewers: Tag some people for review and ping them on Slack

(Optional) Sub-issues (for drafts)

Note: if you find yourself breaking this PR into many smaller features, it may make sense to break up the PR into logical units based on these features.

• Step 1
• Step 2