Path following finally works. Removed redundant nested for loop.
Cleaned up code, linted.