Cleaned up code
added MoveTo code

in order to make this work, change the drivestraight and rotate commands
so that their isFinished methods actually work.