Finished Code

Cleaned it up and added speed control