Worked on MotionControlPlay

* added configs for p,i,d (need to tune them)
* cleaned up unused stuff