move stpSound(), and fix small logical bug in motorRoutine()

It should be the first functional version.