Added Drive Classes and Utils

Driving should work now, some Victor ports and things must be updated
once the electric board is finished. They are marked with "//TODO". Motor groups must also be tested.