Fixed, finished, refined path

Speed is now consistent, remove equal consecutive points