Added more generic IMU code and path finding alg