Cleaned up main.c.
Made code much more modular.
Working ADC, Motors, Servos, and QEI