PID loop implemented