Yes the whole code looks wacky. There is no need to contillully write to the motors, only write when something changes.
Work with relitave time from the millis() not absoloute time.
Use the blink without delay method to do this.
The other thing is that there're not an interger number of milliseconds in the step interval you need so you will have to be a bit more clever than this.