Added new code with a proportional controller on position

please check the todos -- the derivative terms needs to be added by you.