MPU6050 e controllo PID

Salve,
ho un MPU6050 e vorrei realizzare un semplice controllo PID di un motore, in modo che sia in equilibrio sul suo asse orizzontale.
Il motore è posto sull'estremità di un braccio, mentre l'altra estremita è il centro di rotazion.
Quindi spostando il braccio verso destra, il motore si attiva spostandolo verso sinistra e viceversa.
I valori dell'MPU sono già in grado di acquisirli, il giroscopio quindi mi restituisce un valore che varia sull'asse Z, questo valore lo devo elaborare in modo che venga riportato automaticamente al valore nullo.
Da dove posso partire?

Ho usato la libreria ‘PID_v1’ ma quando inserisco come ingresso il valore restituito dal gyro, l’uscita mi sale a 255 anche se ho zero in ingresso ovvero il punto impostato come Setpoint.

Nessuno mi può dare un consiglio?