Program to just drive forward + modifications of main