Added generic PID function to the motion controller. Integrated it with the rotation behavior.