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
Rough cut of tangent blend implementation in tp

Still have some tweaks to make here, but the basic structure is in
place. Did a lot of pruning and refactoring to break up tpRunCycle.
Finally fixed dumb mistake.

Stupidly changed the #defines for blend types to be 0-indexed without
realizing they were redundantly defined in another spot. Basic motion
seems to be working normally now, suggesting that the code surgery
didn't break anything critical.

TODO: test other functions, rigid tapping, etc. to make sure there are
no gross errors
Hacks to show tangent blends, trying to isolate velocity glitch
Successful hack to show x-axis tangent blends
Small tweaks to show that it works with circular arcs too.

Still need to verify that acceleration isn't glitchy, but so far it
looks good.

Also, pausing might be broken from some of the changes. Need to roll
back to master to compare changes.
cleanup of unneeded vars
Merge branch 'feature/tp-final-velocity' into develop