added basic pid
added stop and fleshed out closed loop test some
ready to start testing closed loop control