Switched to a raytracing method (which works a LOT better) Also added slowing down when approaching a wall/tight corner/obstacle H to toggle raytracer S to toggle settings Next step: avoiding smaller obstacles