Fixed Motor Code and Tested