Got it working! The problem was a missing line from the original code which returned an angle for Compass Method to target, in calculateHeadingForExit.