Adding person avoid stop, new planning code

Tried new moving code.  Might be broken. stop/all clear implemented