Tweaked controller. Modified EndNavTest

Tweaked the controller to work better.
Modified the EndNavTest so that it better reflects the functionality of
the controller. Should be ready to be tested. Primary constraint is the
boundary of the block