Initial code for autonomous with just driving functions