Added the RobotInit() call to the Controller::Init() function. This way only one function has to be called to do all of the initializtion needed to start the algorithm