Another commit. This is from a week ago so I can't remember what it does.
Initial work on more intelligent motion planning. Not working yet.