Only perform solvePnP once (on the goal we're actually going to use)

[andrewda]

Currently, solvePnP is run on all goals, regardless of whether or not we're actually going to use those numbers. We're actually using the numbers from solvePnP to determine which goal is the most central when "locking" onto a new goal, but that can easily be replaced by the goals pixel center offset.

All the solvePnP stuff should be moved out into a new function that is run after we determine which goal we specifically want to use, either from the lock id or as the center-most goal.

[andrewda]

It's actually working insanely fast right now, but this might still be a good QoL improvement.