imu and motor code is pretty well tested now