Added steering and driving algorithm (untested)