Drive Code

Reimplimented drive code.