PID control

added untested PID controller, currently not used.