Speed improvements

Improved speed of integer square root function
Improved speed of output to stepper motors