A Successful Approach to the 2016 RobotX Challenge