I don't see where you are re-reading the commanded position within the main loop. I only see where you are reading the feedback position in the interrupt routine... Maybe my eyes are going bad...