Timing Desync on "new-pathing-algorithm" Branch #10
Description
Because the motion profile is entirely time-dependent, if there is any physical change to the velocity or acceleration constants (ie. more weight is added) or a bump/unexpected force occurs, the entire path will get off as the timing will be off. To fix this, we can adjust the time elapsed to where the robot is on the path. If the true time elapsed is t=2, but the robot's position on the path lines up with t=1.8, we can set the elapsed time to t=1.8 to have the robot stay on the intended path. If the robot is off the path, we find the closest point on the path to base calculations.