Ca a l'air correct.
Quel est le problème exactement?
Tu devrais utiliser Serial.println() pour envoyer les valeurs calculées sur le moniteur série, ça aide à voir vraiment ce qui se passe.
Est-ce qu'avec un sketch minimal ton servo tourne bien de la valeur voulue?