Function turn_to_cp using PID controller