4940's Robotics code from Aerial Assist 2014