Changed angle threshold back to 0.3 radians (was too strict). Fixed heading errorsgit add .! Huzzah!