Added set calibration and get IMU functions + added lots of commenting