Print debugging messages when setting pid goal pos