Измеритель емкости на LGT8F328

#define CALL_UF   6.10
#define CALL_NF   6.58

#include <Wire.h> 
#include <U8glib.h>            // https://github.com/olikraus/u8glib/
//U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_NONE|U8G_I2C_OPT_DEV_0);
U8GLIB_SSD1306_128X32 u8g(U8G_I2C_OPT_NONE);  // I2C / TWI 

volatile  int x;
float null_c;
bool mk;
unsigned long c_f,times0,times1;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  PORTB |=(1<<PB5);
// TIMER_1 /////////////////////////  
  TCNT1 = 0;TCCR1A = 0;TCCR1C = 0;TCCR1B = 2;TIMSK1 |= (1 << TOIE1);
// COMPARATOR_0 ////////////////////  
  C0SR = 0;C0XR=0;ADMUX=0;ADCSRB=0;
  ADCSRB = 0b01000000;// ADMUX
  ADMUX = 0b00000000; // A0 A1 A2
  C0SR &=~(1<<C0BG);C0XR |=(1<<C0PS0);// ACXP
// ADC //////////////////////////// 
  ADCSRD |= (1<<REFS2); // 4.096 ИОН
  ADCSRA |= 1 << ADEN | 1 << ADSC | 1 << ADATE | 0b111;
  delay(200);
  PORTB |=(1<<PB0);DDRB &=~ (1<<PB0);
  DDRD |=(1<<PD6);PORTD &=~ (1<<PD6);delay(300);
  DDRB &=~ (1<<PB0);PORTB &=~(1<<PB0);
  times1=millis();
  while(analogRead(A0)>10);
  delay(100); 
  TCNT1=0;x=0;PORTD |= (1<<PD6);
  while(((C0SR >> 5) & 1)==1);null_c = (x * 65535 + TCNT1)/CALL_NF;
  u8g.firstPage(); do {u8g.setFont(u8g_font_profont17r);u8g.drawStr(20,11,"CALL_0");
  u8g.setPrintPos(20,28);u8g.print(null_c,1);u8g.drawStr(80,28,"pF");
  } while( u8g.nextPage() );
  delay(2000);
}

void loop() {
///// uF ////////////////////////////////////////////////  
  PORTD &=~(1<<PD6);DDRD &=~(1<<PD6);
  DDRB |= (1<<PB0);PORTB &=~ (1<<PB0);delay(10);
  while(analogRead(A0)>10);
  TCNT1=0;x=0;PORTB |= (1<<PB0);
  while(((C0SR >> 5) & 1)==1){c_f = x * 65535 + TCNT1;PORTB|=(1<<PB5);}
  PORTB&=~(1<<PB5);
/////////////////////////////////////////////////////////
  if((c_f/CALL_UF)/1000.0<0.3){mk=1;}else{mk=0;}  
///// nF //////////////////////////////////////////////// 
if(mk==1){  
  PORTB |=(1<<PB0);DDRB &=~ (1<<PB0);
  DDRD |=(1<<PD6);PORTD &=~ (1<<PD6);delay(300);
  DDRB &=~ (1<<PB0);PORTB &=~(1<<PB0);
  times1=millis();
  while(analogRead(A0)>10){if(millis()-times1>2000){mk=0;break;}};
  delay(100); 
  TCNT1=0;x=0;PORTD |= (1<<PD6);
  while(((C0SR >> 5) & 1)==1){PORTB|=(1<<PB5);if(x>50){mk=0;break;}}
  PORTB&=~(1<<PB5);c_f = x * 65535 + TCNT1;
}

/////////////////////////////////////////////////////////
  u8g.firstPage(); do {
  u8g.setFont(u8g_font_profont29r);u8g.setPrintPos(0,25);
  if(mk==1){
    if((c_f/CALL_NF-null_c)<9999){
      u8g.print(abs(c_f/CALL_NF-null_c),0);u8g.drawStr(100,25,"pF");
      }else{
    u8g.print((c_f/CALL_NF-null_c)/1000.0,2);u8g.drawStr(100,25,"nF");}
  }
  if(mk==0){
    if((c_f/CALL_UF)/1000.0<9.99){
      u8g.print((c_f/CALL_UF)/1000.0,2);u8g.drawStr(100,25,"uF");
      }else{
    u8g.print((c_f/CALL_UF)/1000.0,1);u8g.drawStr(100,25,"uF");
      }
    }

    } while( u8g.nextPage() );
Serial.println((c_f/CALL_UF)/1000.0);
}

ISR(TIMER1_OVF_vect){x++;}