Add lots of drive code that needed to be updated.

This code is working code for the omni drive system. There might be an
error with going north-east, and the current code is very slow.
Fixed some formatting of NXT code.