cleaned up code

added switch for commands and created motor functions