Made changes to StepperDrive.cpp to improve speed