Fixed not stopping in pressure mode.

We were killing thread 5 before it was stopping motion. Fixed and
removed some unused functions.