Added the IMU cutout and fixed the subgoal aborting bug