Navgrid Based Start Tolerance for Pathfinding

[c-eid]

Changed the minimum start distance from a hardcoded 0.5 to the diagonal of the current navgrid size.

[c-eid]

@mjansen4857 Friendly ping, this is a very simple change, even if there's plans to completely overhaul pathfinding itd be nice to have this work till then.

[mjansen4857]

Needs C++ and python. I can help if needed

[c-eid]

Needs C++ and python. I can help if needed

Im not familiar with wpilib Python and C++ so I probably would not be able to help with that unfortunately.

[mjansen4857]

Actually, looking at this more, this needs some reworking. Calling the get navgrid size method in execute means reading the file every loop, which is very slow. Also, the pathfinding command can't use anything related to a navgrid without breaking the ability to use custom pathfinders. This really just has to be an overload constructor for tolerance or something. Finally, the check for being within 2 meters should remain 2 meters instead of based on that tolerance. That's just ignoring updates when unnecessary which shouldn't really change.

[c-eid]

Finally, the check for being within 2 meters should remain 2 meters

I just assumed that was necessary because I seen another PR with that in it