reorgnization, servoblaster up to date, faster clock, clean up not used fucntions. New file called functions.cpp with all functions and all definitions in code.h