Implemented PID control