FRC C++ Command Based Robot Program