Added servo config and initialization.
Finished drive control.
Removed unused code.