Hi Harrison,
Attached is a copy of your code with my comments in it. It seems mostly OK.
All my comments are marked // R2 so they should be easy to find.
It would be useful if you want me to look at a future version if you mark your changes similarly.
I put a question about the treatment of occupied blocks. I don't have an answer for it. I guess in part I am wondering whether it is legitimate to set a route if there is an occupied block.
I think you are at the stage where you need to test the code properly rather than continue development in a vacuum. "No plan survives contact with the enemy"
If this had been my project I would have had a great deal of practical testing before the code got this complex.
...R
HarrCode2.ino (11.6 KB)