Added enable servo command, and re-wired ARM to turn off servo when in safe mode
Simplified utils_time and fixed borrowing related bug