Readded previous code that was deleted for the autonomous test