Added some functions

Added driving function
Added thread functions
Added print mark function