guarda che il PID serve proprio per evitare valori così hardcodati..
setpoint è l'inclinazione desisderata, x è la tua angolazione rilevata, e vel è la velocità a cui muovere il motore. Tu devi giocare con i valori di P I e D (in teoria solo P, ed eentualmente un pizzico di I, bastano)