I made a few changes:

1. I fixed a few bugs with ending paths with the A* pathing

2. I added a few unit tests to allow me to test the code without the robot

3. I added velocity profiles for the paths run, so that the robot can be tuned for paths

Update: The tests dont work at all :(