LED initialization moved to it's own function.
Robot now drives in a straight line. Does NOT use PID yet.