Fixed some bugs with using the IMU