Improved clime/drive function
Added z axis debug
Other stuff