Fixes arithmetic bug in path planner