Added path following algorithm. Enable/disable with PATH_FOLLOWING flag.