Watching the video of this, at around 10:30, you mention the issue of the stepper motors stopping when measurements are taken.
Refactoring the code to use interrupts to drive the stepper motors would probably fix this. If you're interested in that, I would be willing to refactor it. Acceleration could be baked into that pretty easily too. Let me know!