Vision Target Alignment
Most of our alignment algorithm was based off of team 254's presentation from 2016.
Initial state. Robot is not aligning or attempting to. PID integrators always stopped.
PIDs are running. Robot is aligning.
Wait 300 ms so that the vision system can catch up with the robot. Eliminates lag.
Checks with vision system. Makes sure that it is aligned. When confirmed, tells outside world it is on target as it goes to On Target state.
Robot is on target. Stays here until is becomes unaligned (e.g. robots gets bumped) or vision alignment is no longer desired.
The delta angle and delta range are within tolerances.
The driver has pressed the button to signal that alignment is desired (Desired). Vision is online and gyro is working (Possible)
The amount of time we needed to wait has expired.
