Update to functions to allow for planned motion
Revert "Update to functions to allow for planned motion"

This reverts commit 371d56647751bf8660867ec990e1dac0352007bf.
Allow for planned movement