Code refactoring, upgrading to 16-bit speed

 - Speed/counter are now 16-bit
 - Refactoring stepper control code into a library