ТЕРМОПАРНЫЙ ПИД терморегулятор

#include <Wire.h> 
#include <LiquidCrystal_I2C.h>
#include <EEPROM.h>
#include <OneWire.h>           // http://rcl-radio.ru/wp-content/uploads/2018/07/OneWire.zip
#include <Adafruit_ADS1015.h>  // https://github.com/adafruit/Adafruit_ADS1X15/archive/master.zip
LiquidCrystal_I2C lcd(0x27,16,2);  // Устанавливаем дисплей
OneWire  ds(8); // Вход датчика 18b20
Adafruit_ADS1115 ads; 

int nagrev,set,up,dw,tip;
float t,temper,t_reg,kpp = 1,kii = 0,kdd = 0;
unsigned long times,times1,times2;
bool w,w1=1;
int16_t results;
float multiplier = 0.0078125F; 

//K
const float a[]{0,2.5173462*10,-1.1662878,-1.08336338,-8.9773540/10,-3.7342377/10,-8.6632643/100,-1.0450598/100,-5.1920577/10000};
const float a1[]{0,2.508355*10,7.860106/100,-2.503131/10,8.315270/100,-1.228034/100,9.804036/10000,-4.413030/100000,1.057734/1000000,-1.052755*pow(10,-8)};
const float a2[]{-1.318058*100,4.830222*10,-1.646031,5.464731/100,-9.6550715/10000,8.802193/1000000,-3.110810/100000000};
//L
const float b[]{1.1573067/10000,1.5884573*10,4.0458554/100,0.3170064,0.1666128,5.146958/100,9.5288883/1000,1.0301283/1000,6.0654431/100000,1.5131878/1000000};
const float b1[]{7.2069422/1000,1.5775525*10,-0.2261183,9.4286756/1000,-3.5394655/10000,1.0050886/100000,-1.9323678/10000000,2.3816891/1000000000,-1.7130654*pow(10,-11),5.4857331*pow(10,-14)};
//S
const float c[]={0,1.84949460*100,-8.00504062*10,1.02237430*100,-1.52248592*100,1.88821343*100,-1.59085941*100,8.23027880*10,-2.34181944*10,2.79786260};
const float c1[]={1.291507177*10,1.466298863*100,-1.534713402*10,3.145945973,-4.163257839/10,3.187963771/100,-1.291637500/1000,2.183475087/100000,-1.447379511/10000000,8.211272125/1000000000};
const float c2[]={-8.087801117*10,1.621573104*100,-8.536869453,4.719686976/10,-1.441693666/100,2.081618890/10000};
const float c3[]={5.333875126*10000,-1.235892298*10000,1.092657613*1000,-4.265693686*10,6.247205420/10};


void setup() {
  lcd.init();lcd.backlight();lcd.clear();
  Serial.begin(9600);
  ads.setGain(GAIN_SIXTEEN); 
  ads.begin();
  pinMode(9,OUTPUT);
  cli();TCCR1A = 0;TCCR1B = 0;TCNT1 = 0;
  TCCR1A = 1 << COM1A1 |1 << WGM11;
  TCCR1B = 1 << WGM13 |1 << WGM12 |1 << CS12 | 1 << CS10;
  TIMSK1 |= (1 << OCIE1A);
  ICR1 = 10240; 
  OCR1A = 0; sei();
  pinMode(2,INPUT_PULLUP);// UP
  pinMode(3,INPUT_PULLUP);// DW
  pinMode(4,INPUT_PULLUP);// SET
  pinMode(5,INPUT_PULLUP);// TIP
  if(EEPROM.read(100)!=0){for(int i=0;i<101;i++){EEPROM.update(i,0);}}
  EEPROM.get(1, t_reg);
  tip=EEPROM.read(30);
  kpp = EEPROM.read(0);if(kpp==0){kpp=1;}
  EEPROM.get(10, kii);
  EEPROM.get(20, kdd);
  temper = dsRead(0);
  delay(200);
}

void loop() {
  if(digitalRead(5)==LOW){tip++;if(tip>2){tip=0;}w=1;w1=1;times1=millis();}
  if(digitalRead(4)==LOW){set++;if(set>3){set=0;}w1=1;}
  if(digitalRead(2)==LOW && set==0){if(up>10){t_reg+=5;}else{t_reg++;}up++;w=1;w1=1;times1=millis();}
  if(digitalRead(3)==LOW && set==0){if(dw>10){t_reg-=5;}else{t_reg--;}dw++;w=1;w1=1;times1=millis();}
  if(digitalRead(2)==HIGH){up=0;}if(digitalRead(3)==HIGH){dw=0;}
  if(tip==0 && t_reg<0){t_reg=0;}if(tip==0 && t_reg>1372){t_reg=1372;}
  if(tip==1 && t_reg<0){t_reg=0;}if(tip==1 && t_reg>800){t_reg=800;}
  if(tip==2 && t_reg<0){t_reg=0;}if(tip==2 && t_reg>1768){t_reg=1768;}

  if(digitalRead(2)==LOW && set==1){kpp++;;w=1;w1=1;times1=millis();}
  if(digitalRead(3)==LOW && set==1){kpp--;if(kpp<1){kpp=1;};w1=1;w=1;times1=millis();}

  if(digitalRead(2)==LOW && set==2){kii+=0.01;;w=1;w1=1;times1=millis();}
  if(digitalRead(3)==LOW && set==2){kii-=0.01;if(kii<0){kii=0;};w=1;w1=1;times1=millis();}
  
  if(digitalRead(2)==LOW && set==3){kdd++;;w=1;w1=1;times1=millis();}
  if(digitalRead(3)==LOW && set==3){kdd--;if(kdd<0){kdd=0;};w=1;w1=1;times1=millis();}
  
  if(w1==1){w1=0;
  results = ads.readADC_Differential_0_1();
  temper = dsRead(0); delay(100);
  lcd.setCursor(0, 0);
  if(t==10000){lcd.print("  HIGH  ");}
  else if(t==-1000){lcd.print("  LOW   ");}
  else{lcd.print("T=");lcd.print(t,0);lcd.print((char)223);lcd.print("C  ");}
  lcd.setCursor(10, 0);lcd.print("P=");lcd.print(100.00/512.00*float(nagrev),0);lcd.print("%  ");
  lcd.setCursor(0, 1);lcd.print("t=");lcd.print(t_reg,0);lcd.print((char)223);lcd.print("C  ");  
  
  lcd.setCursor(9, 1);
  if(set==0){ 
    switch(tip){
    case 0: lcd.print(" type K");break;
    case 1: lcd.print(" type L");break;
    case 2: lcd.print(" type S");break;
    }}
  if(set==1){lcd.print("kp=");lcd.print(kpp,0);lcd.print("   ");}
  if(set==2){lcd.print("ki=");lcd.print(kii,2);lcd.print("   ");}
  if(set==3){lcd.print("kd=");lcd.print(kdd,0);lcd.print("   ");}
  }
  if(millis()-times1>10000 && w==1){w=0;EEPROM.update(0,kpp);EEPROM.put(1, t_reg);EEPROM.update(30,tip);EEPROM.put(10, kii);EEPROM.put(20, kdd);
  lcd.setCursor(9, 1);lcd.print("  SAVE  ");delay(300);
  }
}

ISR(TIMER1_COMPA_vect){w1=1;delay(50);
  switch(tip){
    case 0: txa();break;
    case 1: txk();break;
    case 2: tpp_s();break;
    }
    nagrev = computePID(t, t_reg, kpp, kii, kdd, 0.6, 0, 511);
    OCR1A = nagrev*20;
    
    }

void txk(){float u=results * multiplier+temper*0.06555;// окружающая температура 0.06555 мВ на 1 гр.Цельсия от 0 до 40 гр.Цельсия для TXK(L)
if(u<0){t=(b[0]*pow(u,0))+(b[1]*pow(u,1))+(b[2]*pow(u,2))+(b[3]*pow(u,3))+(b[4]*pow(u,4))+(b[5]*pow(u,5))+(b[6]*pow(u,6))+(b[7]*pow(u,7))+(b[8]*pow(u,8))+(b[9]*pow(u,9));}
if(u>=0){t=(b1[0]*pow(u,0))+(b1[1]*pow(u,1))+(b1[2]*pow(u,2))+(b1[3]*pow(u,3))+(b1[4]*pow(u,4))+(b1[5]*pow(u,5))+(b1[6]*pow(u,6))+(b1[7]*pow(u,7))+(b1[8]*pow(u,8))+(b1[9]*pow(u,9));}
if(results * multiplier<-9.49){t=-1000;} if(results * multiplier>66.47){t=10000;}
}  

void txa(){float u=results * multiplier+temper*0.0403;// окружающая температура 0.0403 мВ на 1 гр.Цельсия от 0 до 40 гр.Цельсия для TXA(K)
if(u<0){t=(a[0]*pow(u,0))+(a[1]*pow(u,1))+(a[2]*pow(u,2))+(a[3]*pow(u,3))+(a[4]*pow(u,4))+(a[5]*pow(u,5))+(a[6]*pow(u,6))+(a[7]*pow(u,7))+(a[8]*pow(u,8));}
if(u>=0&&u<=20.64){t=(a1[0]*pow(u,0))+(a1[1]*pow(u,1))+(a1[2]*pow(u,2))+(a1[3]*pow(u,3))+(a1[4]*pow(u,4))+(a1[5]*pow(u,5))+(a1[6]*pow(u,6))+(a1[7]*pow(u,7))+(a1[8]*pow(u,8))+(a1[9]*pow(u,9));}
if(u>20.64){t=(a2[0]*pow(u,0))+(a2[1]*pow(u,1))+(a2[2]*pow(u,2))+(a2[3]*pow(u,3))+(a2[4]*pow(u,4))+(a2[5]*pow(u,5))+(a2[6]*pow(u,6));}
if(results * multiplier<-6.35){t=-1000;} if(results * multiplier>54.9){t=10000;}
}

void tpp_s(){float u=results * multiplier+temper*0.005875;// окружающая температура 0.005875 мВ на 1 гр.Цельсия от 0 до 40 гр.Цельсия для TPPs(S)
if(u<1.874){t=(c[0]*pow(u,0))+(c[1]*pow(u,1))+(c[2]*pow(u,2))+(c[3]*pow(u,3))+(c[4]*pow(u,4))+(c[5]*pow(u,5))+(c[6]*pow(u,6))+(c[7]*pow(u,7))+(c[8]*pow(u,8))+(c[9]*pow(u,9));}
if(u>=1.874&&u<10.332){t=(c1[0]*pow(u,0))+(c1[1]*pow(u,1))+(c1[2]*pow(u,2))+(c1[3]*pow(u,3))+(c1[4]*pow(u,4))+(c1[5]*pow(u,5))+(c1[6]*pow(u,6))+(c1[7]*pow(u,7))+(c1[8]*pow(u,8))+(c1[9]*pow(u,9));}
if(u>=10.332&&u<17.536){t=(c2[0]*pow(u,0))+(c2[1]*pow(u,1))+(c2[2]*pow(u,2))+(c2[3]*pow(u,3))+(c2[4]*pow(u,4))+(c2[5]*pow(u,5));}
if(u>=17.536&&u<=18.694){t=(c3[0]*pow(u,0))+(c3[1]*pow(u,1))+(c3[2]*pow(u,2))+(c3[3]*pow(u,3))+(c3[4]*pow(u,4));}
if(results * multiplier<-0.236){t=-1000;}  if(results * multiplier>18.70){t=10000;}
}


int computePID(float currentpoint, float setpoint, float kp, float ki, float kd, float dt, int minOut, int maxOut) {
  static float last_error;
  float P;
  float D;
  static float I;
  long PID;
  P = (setpoint - currentpoint);
  I = (I + (setpoint - currentpoint) * dt);
  D = (((setpoint - currentpoint) - last_error) / dt);
  if(I>511){I=511;}if(I<-511){I=-511;}
  last_error = setpoint - currentpoint;
  PID = (kp * P) + (ki * I) + (kd * D);
  return constrain(PID, minOut, maxOut);
}

float dsRead(byte x) {
  byte data[2], addr[8][8], kol = 0;
  while (ds.search(addr[kol])) {  // поиск датчиков, определение адреса и кол-ва датчиков
    kol++;
  } 
  ds.reset_search();  // Сброс поиска датчика
  ds.reset();         // Инициализация, выполняется сброс шины
  ds.select(addr[x]); // Обращение к датчику по адресу
  ds.write(0x44, 0);  // Измерение температуры с переносом данных в память
  ds.reset();         // Инициализация, выполняется сброс шины
  ds.select(addr[x]); // Обращение к датчику по адресу
  ds.write(0xBE);     // Обращение памяти
  data[0] = ds.read();// Чтение памяти byte low
  data[1] = ds.read();// Чтение памяти byte high
  float value = ((data[1] << 8) | data[0]) / 16.0; return (float)value; // Расчет температуры и вывод
}