Clean up code. IMU and ESC interfaces self contained.