Trigger warmup before lineup instead of when lineup begins

[aidnem]

Purpose
Based on the current performance in sim, it seems that drive can line up much faster than scoring can warmup to score, especially in high positions such as L4. We should set a configurable distance away from lineup to begin warming up scoring.

Project Scope

• Make OTF state fire the BeginWarmup trigger when within a configurable distance of its goal

[jkleiber]

@aidnem fwiw the sim performance is sus to me. In shop on Monday we were able to get to the top of the elevator's range in like 2 seconds. That being said, I don't think I have a problem with us getting the elevator up earlier provided we aren't in the middle of a dynamic maneuver while carrying algae or something

[aidnem]

Okay, when I retuned I think I clamped the speed down too far I might just try letting it go faster and seeing what happens