Moved threshold check and done flag

[kjm16216]

Added the logic we discussed at the end of the meeting last night.

STATUS

REVIEW

DESCRIPTION

Incorporates the logic we discussed at the end of the meeting Mon night. We were finding the turn was frequently overshooting, thinking that is a latency issue from when IsFinished returns true and end method actually executes. Revision would check that angle is within threshold during execute, and if so, disable the PID and stop the motors. Creates a separate flag to trigger IsFinished that's raised in the same code block.

FILES THAT CONTAINTED MERGE CONFLICTS

-

TO DO

• [ ] On Robot testing
• [ ] Resolved Merge Conflicts
• [ ] Reviewed by Peers
• [ ] Reviewed by Senior Dev
• [ ] When tested merge into AutoTurnPID

[ptoil]

Auto-Turn PID tested and is reliable to a margin of error up to ( +/- ) 1 degrees.