Controllo PID

assolutamente no, stai usando sia la parte integrale che la parte derivativa. Il comportamento che dici te lo avresti usando SOLO la parte proporzionale. per questo metti KI e KD a 0, e vedi se solo parte KP funziona; in otler è facile da calcolare il valore atteso, dato che l'algorimto diventa

risultato= (valoreNuovo-valoreVecchio)*kp