Переделать скейтч

#include <Arduino.h>
#include <Wire.h> 
#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27,16,2); // для Протеуса адрес должен быть 0x20, для платы Ардуино - 0x3F или 0x27 
 
// интервал давления для поддержания нагревателем 
#define P_LOW     800   // включать ТЭН при нижней границе давления 
#define P_HIGH    830   // выключать ТЭН при верхней границе 
#define P_PODACHA 770   // давление открытия клапана подачи 
#define P_SPUSK   500   // спуск давления 
#define P_VPUSK   500   // спуск вакуума 
#define P_VACUM   700   // порог давления в режиме вакума для включения таймера вакумирования 
 
#define TIMER1_DELAY_SEC  30  // задержка таймера 1 (продувка)
#define TIMER2_DELAY_SEC  30  // задержка таймера 2 (стерилизация)
#define TIMER4_DELAY_SEC  20  // задержка таймера 4 (вакуум)
 
bool timer1_start_flag = false; // флаг запуска таймера 1
bool timer2_start_flag = false; // флаг запуска таймера 2
bool timer3_start_flag = false; // флаг запуска таймера 3
bool timer4_start_flag = false; // флаг запуска таймера 4
bool timer5_start_flag = false; // флаг запуска таймера 5
bool timer6_start_flag = false; // флаг запуска таймера 6
int tmrCnt = 0; // счетчик таймера 
 
#define VCC 4.95    // !!!ОБЯЗАТЕЛЬНО!!! задать точное значение питания 
// Pt1000 коэффициенты 
#define A 0.0039083
#define B -0.0000005775
#define R0 1000
 
 
//*********************************************************************
// вычисление напряжения Ut(в Вольтах) на датчике температуры по прочитанному значению ADC (АЦП) 
double ADC_to_Ut (int adc_val){
 
    return VCC / 1024 * adc_val;
}
//*********************************************************************
// вычисление сопротивления Rt датчика температуры по напряжению Ut на нем 
double Ut_to_Rt (double ut){
 
    return ut * 1000 / (VCC - ut);
}
//*********************************************************************
// вычисление температуры t(в Целсиях) по сопротивлению датчика Rt 
int Rt_to_t (double rt){
 
    double d = A * A - 4 * B * (1 - rt / 1000);
    return (-A + sqrt(d)) / (2 * B);
}
//*********************************************************************
// поддержка температуры между заданными t1 и t2, (current - текущее значение) 
void set_pressure (int p1, int p2, int current){
 
    if (current < p1){ // включаем ТЭН, поднимаем температуру 
        PORTB |= (1<<1);    // тэн 
        //PORTC &= ~(1<<2);
        //PORTC &= ~(1<<3);
        //PORTB &= ~(1<<2);
    }
    else if (current >= p2){ // выключаем ТЭН 
        PORTB &= ~(1<<1);
    }
}
//*********************************************************************
// управление клапаном подачи пара 
void set_podacha (bool f){
 
    if (f){
        PORTC |= (1<<2);  // вкл. пар в камеру 
    }
    else {
        PORTC &= ~(1<<2); // выкл. пар в камеру 
    }
}
//*********************************************************************
// управление продувкой 
void set_produvka (bool f){
 
    if (f){
        PORTC |= (1<<3);  // вкл продувка 
    }
    else {
        PORTC &= ~(1<<3); // выкл продувка 
    }
}
//*********************************************************************
// управление вакуумом 
void set_vakum (bool f){
 
    if (f){
        PORTB |= (1<<2);  // вкл вакуум 
    }
    else {
        PORTB &= ~(1<<2); // выкл вакуум 
    }
}
//*********************************************************************
// управление впуском после вакуума 
void set_vpusk (bool f){
 
    if (f){
        PORTB|= (1<<0);   // вкл впуск 
    }
    else {
        PORTB &= ~(1<<0); // выкл впуск 
    }
}
//*********************************************************************
void setup (){
 
    DDRC |= (1<<DDC2) | (1<<DDC3);              // светодиоды: PC2-PODACHA, PC3-PRODUV 
    DDRB |= (1<<DDB0) | (1<<DDB1) | (1<<DDB2);  // светодиод PB0-VPUSK, PB1-TEN, PB2-VAKUM 
    
    DDRC &= ~((1<<DDC1) | (1<<DDC0));           // аналоговые входы PC1/ADC1 и PC0/ADC0 
    ADCSRA |= (1<<ADEN);                        // разрешаем работу АЦП 
    ADCSRA |= (1<<ADPS2) | (1<<ADPS1) | (1<<ADPS0);  // частота дискретизации 125 кГц 
 
    ADMUX |= (1<<REFS0);    // внешний источник 5 В 
    ADMUX &= ~(1<<REFS1);   // 
    ADMUX &= ~(1<<ADLAR);   // правостороннее выравнивание 
    ADMUX &= ~(1<<MUX3) | (1<<MUX2)| (1<<MUX1); // очистить биты выбора канала 
    ADMUX |= (1<<MUX0);     // задействуем ADC1 
    ADCSRA |= (1<<ADSC);    // запускаем АЦП 
    
  // Инициализация дисплея I2C 1602 
  lcd.init();                 // инициализация дисплея 
  lcd.backlight();            // включить подсветку дисплея 
    
    // Вывод HELLO и очистка 
  lcd.setCursor(5,2);
  lcd.print("Hello");
  lcd.setCursor(5,1);
  lcd.print("1602");
 
    delay(2000);
  lcd.clear();
}
//*********************************************************************
void loop (){
char t_adc[5];
 
  lcd.clear();
  
  // вычисляем температуру в одну строку от считанного ADC 
  int t =   Rt_to_t(Ut_to_Rt(ADC_to_Ut(ADC)));
  // конвертируем числовое значение t температуры в строку для вывода на LCD 
  itoa(t, t_adc, 10);
  lcd.print(t_adc);
  lcd.setCursor(0, 1);
 
  // переключаем канал на АЦП0 для датчика давления 
  ADCSRA &= ~(1<<ADEN);   // stop АЦП 
  ADMUX &= 0xF0;          // выбрать АЦП0 
  ADCSRA |= (1<<ADEN);    // разрешаем работу АЦП 
  ADCSRA |= (1<<ADSC);    // запускаем АЦП 
  delay(1);               // пропускаем первые несколько конверсий 
  
  int pressure = ADC;     // Cчитываем ADC 0 канал - давление 
  itoa(pressure, t_adc, 10);
  lcd.print(t_adc);
 
  if (!timer1_start_flag){ // если таймер 1 не запущен 
    set_pressure(P_LOW, P_HIGH, pressure);        // авторегулировка давленияв заданном интервале 
    if (pressure >= P_PODACHA) set_podacha(true); // подача пара 
    if (pressure >= P_HIGH){
      set_produvka(true);
      timer1_start_flag = true; // запуск таймера 1 
    }
  }
  else if (!timer2_start_flag){ // если таймер 1 запущен, а таймер 2 еще нет 
    set_pressure(P_LOW, P_HIGH, pressure);  // авторегулировка давленияв заданном интервале 
    if (tmrCnt >= TIMER1_DELAY_SEC){ // таймер 1 истек 
      set_produvka(false);
      timer2_start_flag = true; // запуск таймера 2 
      tmrCnt = 0;
    }
    else {
      tmrCnt++;
      itoa(tmrCnt, t_adc, 10);  // отображаем таймер на дисплей в секундах 
      lcd.print(" Produv ");
      lcd.print(t_adc);
    }
  }
  
  if (timer2_start_flag && (!timer3_start_flag)){ // если таймер 2 запущен, а таймер 3 еще нет 
    set_pressure(P_LOW, P_HIGH, pressure);  // авторегулировка давленияв заданном интервале 
    if (tmrCnt >= TIMER2_DELAY_SEC){
      PORTB &= ~(1<<1);   // выкл ТЭН 
      set_podacha(false); // выкл клапан подачи пара 
      set_produvka(true); // открыть продувку 
      tmrCnt = 0;
      timer3_start_flag = true;
    }
    else {
      itoa(tmrCnt, t_adc, 10);  // отображаем таймер на дисплей в секундах 
      lcd.print(" Steril ");
      lcd.print(t_adc);
      tmrCnt++;
    }
  }
  
  if (timer3_start_flag && (!timer4_start_flag)){ // если таймер 3 запущен, а таймер 4 еще нет (на самом деле это не совсем таймер, а ожидание выравнивания давления) 
    if (pressure > P_SPUSK){
      lcd.print(" Spusk ");
    }
    else {
      set_produvka(false);  // выкл. продувку 
      set_vakum(true);      // вкл. вакуум 
      tmrCnt = 0;
      timer4_start_flag = true;
    }
  }
  
  if (timer4_start_flag && (!timer5_start_flag)){ // если таймер 4 запущен, а таймер 5 еще нет 
    if (tmrCnt >= TIMER4_DELAY_SEC){
      set_vakum(false);
      set_vpusk(true);
      tmrCnt = 0;
      timer5_start_flag = true;
    }
    else if (pressure > P_VACUM){
      itoa(tmrCnt, t_adc, 10);  // отображаем таймер на дисплей в секундах 
      lcd.print(" Vacum ");
      lcd.print(t_adc);
      tmrCnt++;
    }
  }
  
  if (timer5_start_flag){ // если последний таймер 5 запущен (на самом деле это не совсем таймер, а ожидание выравнивания давления) 
    if (pressure > P_VPUSK){
      lcd.print(" Vpusk ");
    }
    else {
      set_vpusk(false);
      lcd.clear();
      lcd.setCursor(3, 0);
      lcd.print("Okoncheno");
      delay(250);
      while(1);
    }
  }
 
  // переключаем канал  на АЦП1 
  ADCSRA &= ~(1<<ADEN);   // stop АЦП 
  ADMUX &= ~((1<<MUX3) | (1<<MUX2) | (1<<MUX1));
  ADMUX |= (1<<MUX0);     // задействуем ADC1 
  ADCSRA |= (1<<ADEN);    // разрешаем работу АЦП 
  ADCSRA |= (1<<ADSC);    // запускаем АЦП 
  delay(970);             // задержка на каждой итерации, чтобы с учетом остального кода была одна секунда на каждый круг, тогда один тик таймера это одна секунда 
}
//*********************************************************************