REVERT THIS WHEN SERVO POSITIONS CORRECTED