Fixing up and adding stepper control