Added functions to get and set IMU en/freq
Finished commands and added new IMU functions to main loop