controller works great in sim, thanks to danny
business
set kpdd constants
moveTo working nicely, may require some tweaking of the pd controller constants