[Olá colegas, estou fazendo um PID de controle de nível de tanque, porem a minha integral e derivada não esta funcionando queria a ajuda de vcs]

#define sensor A1
#define controle 6 // pino do mosfet
#define altTanque 14.5 
#include "Ultrasonic.h"
#include <LiquidCrystal.h>
LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
Ultrasonic ultrasonic(10,9); //pinos trigger e echo
 long microsec = 0;
 float distanciaCM = 0;

void setup() {
  Serial.begin(9600);
  pinMode(sensor,INPUT);
  pinMode(controle,OUTPUT);
  lcd.begin(20,4);
  }
 
 double
    P = 0,
    I = 0,
    D = 0, 
    PID = 0;
 
 double 
    KP = 285.5,
    KI = 160.5, 
    KD = 10.0;
    
 int    setpoint = 10;
 float  controlePWM = 0;
 float  nivel;
 double erro = 0;
 double lastNivel;
 long   lastProcess = 0; 

 
void loop() {
microsec = ultrasonic.timing(); //Lendo o sensor
distanciaCM = ultrasonic.convert(microsec, Ultrasonic::CM);//Convertendo a distância em CM
 
double distanciaCm = map(distanciaCM, 0, 14.5, 0, 255);

nivel = altTanque - distanciaCM;

if(nivel > 14.5){
  nivel = 14.5;
 }

if(nivel < 00.00){
  nivel = 00.00;
}

erro=setpoint-nivel;
float deltaTime = (millis () - lastProcess)/ 1000.0;
lastProcess = millis();

P = erro * KP;

I = (I + erro *KI) * deltaTime; //ou I += erro * KI;

D = (lastNivel - nivel) * KD / deltaTime; //D = (lastNivel - nivel) * KD ;
lastNivel = nivel;                        //lastNivel =nivel;

PID = P + I + D; 

controlePWM = PID; 

// saida do controle
 
if(controlePWM > 255){
  controlePWM = 255;
  
 }
 if( nivel >= setpoint){
  controlePWM = 0;
 }

  analogWrite(controle,controlePWM);
  lcd.setCursor(2,0);
  lcd.print("IFMA-COELHO NETO");
  lcd.setCursor(2,1);
  lcd.print(nivel);
  lcd.print(" CM DE AGUA ");
  lcd.setCursor(5,2);
  lcd.print(controlePWM);
  lcd.print(" PWM ");
  lcd.setCursor(0,3);
  lcd.print("ALAN,JACINTO E BRITO");
  //delay(100);

  analogWrite(controle,controlePWM); 
  Serial.print(setpoint);
  Serial.print(" ");
  Serial.print(nivel);
  Serial.println(" ");
  //delay(100);

/*
analogWrite(controle,controlePWM);
Serial.print("pwm: ");
Serial.println(controlePWM);
Serial.print("cm de agua: ");
Serial.println( nivel);
Serial.print("erro: ");
Serial.println(erro);
delay(1000);
*/