тахометр

#include <Encoder.h>  // http://rcl-radio.ru/wp-content/uploads/2019/05/Encoder.zip
#include <MsTimer2.h> // http://rcl-radio.ru/wp-content/uploads/2018/11/MsTimer2.zip
#include <LiquidCrystal_I2C.h> //Библиотека -  http://forum.rcl-radio.ru/misc.php?action=pan_download&item=45&download=1
   Encoder myEnc(5, 6);//CLK, DT
   LiquidCrystal_I2C lcd(0x27,16,2);  // Устанавливаем дисплей 


float f;
long en=10000;
unsigned long oldPosition  = -999,newPosition;
bool w;
byte hag;
int mn;

void setup(){
  Serial.begin(9600);
  MsTimer2::set(3, to_Timer);MsTimer2::start();
  Wire.begin();lcd.init();lcd.backlight();
 DDRB |= (1 << 1); // ВЫХОД ГЕНЕРАТОРА D9
  pinMode(7,INPUT);  // КНОПКА ЭНКОДЕРА D7
  setupFreq(f/60);
}

void loop(){
   if(digitalRead(7)==LOW){hag++;if(hag>3){hag=0;}delay(100);}
   switch(hag){
    case 0: mn=1;break;
    case 1: mn=10;break;
    case 2: mn=100;break;
    case 3: mn=1000;break;
    }
  
   if (newPosition != oldPosition){oldPosition = newPosition;
     en=en+newPosition*mn;myEnc.write(0);newPosition=0;if(en<100){en=100;}if(en>250000){en=250000;}w=1;f=(float)en;setupFreq(f/600);} 
  Serial.println(f,2);
  lcd.setCursor(1,0);lcd.print(f/10,1);lcd.print("  ");lcd.setCursor(10,0);lcd.print("rpm");
  lcd.setCursor(9,1);lcd.print("x");lcd.print((float)mn/10,1);lcd.print("   ");lcd.setCursor(1,1);lcd.print(f/600,3);lcd.print(" ");
  delay(100);
  }

void setupFreq(float freq){
 int prescallers[] = {0, 1, 8, 64, 256, 1024};
 unsigned long ocr;
  byte prescallerBits = 1; 
  int prescaller = prescallers[prescallerBits]; 
  if (freq) {  do  {
          ocr = 8000000 / freq  / prescaller - 1; 
          if (ocr < 65535){break;}
          prescallerBits += 1; prescaller = prescallers[prescallerBits]; 
      }
      while (prescallerBits < 6);                        
      if (ocr < 65535) {
            cli();                   
          TCCR1A = 0;             
          TCCR1B = 0;            
          TCCR1B = prescallerBits;
          OCR1A = ocr; 
          TCCR1B |= (1 << WGM12); 
           TIMSK1 |= (1 << OCIE1A);
           sei();   
          float freq = 8000000.00 / prescaller / (ocr + 1); 
    }
  } 
}

ISR(TIMER1_COMPA_vect){PORTB |= (1 << 1);delay(1);PORTB &= ~(1 << 1);}


void to_Timer(){newPosition = myEnc.read()/4;}