Yes, multithreading can lead to prettier/less verbose auto modes, in my experience. We have been prototyping a trapezoidal motion profile as well, and I really like how smoothly it achieves distance setpoints with sub-inch accuracy. Are you using a trapezoidal profile for turning in place, as well? (I didn't notice it, but haven't gone through line by line - yet)
EDIT: Yes, I see that you did
