updated motion controller

+modified controller state handler
+added generic PID controller
- need to work on a gait controller
- need to test motion code