Motor rotates in open-loop mode, either direction

Made some structural changes throughout code to get rotation.