Added experimental non-zero final velocity

Based on derived non-zero final velocity formula, coded simple version.
Initial tests using simulator and zero final velocity show normal
looking behavior, but we need to test with HAL scope or something to be
sure.
Whitespace and commenting fixes in tp.c
Refactor blend speed calculation into a separate function
Refactoring of tcRunCycle for ease of reading
More commenting and formatting cleanup
Refactoring of tp error function