Оптимизация для повышения частоты ШИМ

#include <avr/io.h>
#include <avr/eeprom.h>
#include <avr/interrupt.h>
#include <stdbool.h>
 
#define F_CPU 9600000U
#include <util/delay.h>
 
#define PWM_RES 16 - 1
 
uint8_t adc;
//uint8_t adcs[4]={46,94,115,115}, values[20]={1,2,3,4,10,10,10,10,20,20,20,20,40,40,40,40,60,60,60,60}, freq=0;
uint8_t adcs[4], values[20], freq;
uint8_t pwm1=0,pwm2=0,pwm3=0,pwm4=0,flag=0,oldflag=0;
uint8_t port_out_table[16];
uint8_t cnt=0;
 
EEMEM uint8_t adcs_store[4] ;
EEMEM uint8_t values_store[20] ;
EEMEM uint8_t freq_store ;
 
//uint8_t flag_uart_error = 0;
 
 
uint8_t UART_receive(void)
// Receive one byte on hardcoded 9600 bps
{
    const uint8_t p = 49, P = p*2;
    volatile uint8_t usart_r=0;
    volatile uint8_t inbit = 0;
    
    while(PINB&(1<<4));
    _delay_us(P);
    for(uint8_t a=0; a<8; a++)
    {
        _delay_us(p);
        volatile int pinb = PINB;
        inbit = 1<<4;
        inbit = pinb & inbit;
        inbit = inbit>>4;
        usart_r |= (inbit << a);
        _delay_us(p);
    }
    _delay_us(P);
    //if( !(PINB&(1<<4)) ) flag_uart_error++;
    return usart_r;
}
 
 
ISR(TIM0_COMPA_vect)
// Timer interrupt handler
{
    PORTB = port_out_table[cnt];
    cnt == PWM_RES ? cnt = 0 : cnt++;
}
 
ISR(ADC_vect)
{
    adc=255-ADCH;
}
 
void boost(void)
// Blip fans before increasing speeds
{
    cli();
    PORTB = 0b00001111;
    _delay_ms(255);
    PORTB = 0;
    sei();
}
 
void beep_hi(uint16_t b)
{
    cli();
    while(b--)
    {
        _delay_us(450);
        PORTB = 0b00001111;
        _delay_us(50);
        PORTB = 0;
    }
    sei();
}
 
void beep_lo(uint16_t b)
{
    cli();
    while(b--)
    {
        _delay_us(900);
        PORTB = 0b00001111;
        _delay_us(100);
        PORTB = 0;
    }
    sei();
}
 
void newdelay(uint8_t in)
{
    while(in--) _delay_us(1);
}
 
void bitpeep()
// Beeps byte for debug reasons
{
    for(int t =0;t<4;t++)
    {
        uint8_t tmp = adcs[t];//UART_receiv();
        for(uint8_t i = 0; i<8; i++)
        {
            
            if(tmp&(1<<i)) beep_hi(500);
            else beep_lo(500);
            //tmp = tmp>>y;
            _delay_ms(500);
        }
    }
}
 
void read_settings_from_eep()
{
    for(uint8_t i = 0; i<4; i++)
    adcs[i] = eeprom_read_byte(&adcs_store[i]);
    for(uint8_t i = 0; i<20; i++)
    values[i] = eeprom_read_byte(&values_store[i]);
    freq = eeprom_read_byte(&freq_store);
}
 
void receive_settings_from_uart()
{
    cli();
    ADCSRA = 0;
    
    //Required input: 'saaaabbb...bbbce' - save 4a and 20b
    //If input: 'saaaabbb...bbbcn' - only load, not save
    
    if(UART_receive() == 's') // start block
    {
        for(uint8_t i = 0; i<4; i++)
        adcs[i]=UART_receive();
        for(uint8_t i = 0; i<20; i++)
        values[i]=UART_receive();
        freq = UART_receive();
        
        if(UART_receive() == 'e') // 'e' = write to eeprom
        {
            for(uint8_t i = 0; i<4; i++)
            eeprom_write_byte(&adcs_store[i], adcs[i]);
            for(uint8_t i = 0; i<20; i++)
            eeprom_write_byte(&values_store[i], values[i]);
            eeprom_write_byte(&freq_store, freq);
            //for(uint8_t h=3;h>0;h--)
            //{
            //beep_hi(200);
            //beep_lo(200);
            //_delay_ms(200);
            //}
        }
        
    }
    
    ADCSRA = 0xEA;
    sei();
}
 
int main(void)
{
    //register int cnt=0; // ???
    
    //DDRB&=~(1<<4); //input
    //DDRB|=(1<<0);  //outputs
    //DDRB|=(1<<1);
    //DDRB|=(1<<2);
    //DDRB|=(1<<3);
    DDRB=0b00001111;
    TCCR0A=(1<<WGM01); // clear at compare match
    TIMSK0=(1<<OCIE0A); // interrupt on compare match
    TCCR0B=(1<<CS00);
    OCR0A=60;
    
    //for(uint8_t i = 0; i<4; i++)
    //{
    //eeprom_write_byte(&adcs_store[i], adcs[i]);
    //}
    //for(uint8_t i = 0; i<20; i++)
    //eeprom_write_byte(&values_store[i], values[i]);
    //beep_hi(255);
    
    read_settings_from_eep();
    
    ADMUX = 0b00100010; // опорное напряжение - VCC
    ADCSRA = (1<<ADEN)|(1<<ADSC)|(1<<ADATE)|(1<<ADIE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0);// 0b11101111;
    // АЦП включен, запуск преобразования, режим автоизмерения, прерывание по окончанию преобразования, частота CLK/128
    ADCSRB = 0x00; // режим автоизмерения: постоянно запущено
    //beep_lo(255);
    //boost();
    //boost();
    //boost();
    //beep_hi(255);
    
    asm("sei");
    
    while(1)
    {       
        // HIGH on input means uart connected
        if(0)//adc<=30) // DEBUG
        {
            receive_settings_from_uart();
        }
        
        newdelay(freq);
        
        oldflag=flag;
        
        // Setting the dependense of the settings on temperature
        // adc[0], 1, 2, 3 are thresholds
        if(adc<adcs[0])
        {
            pwm1=values[0];
            pwm2=values[1];
            pwm3=values[2];
            pwm4=values[3];
            flag=0;
        }
        else if(adc>adcs[0] && adc<adcs[1])
        {
            pwm1=values[4];
            pwm2=values[5];
            pwm3=values[6];
            pwm4=values[7];
            flag=1;
        }
        else if(adc>adcs[1] && adc<adcs[2])
        {
            pwm1=values[8];
            pwm2=values[9];
            pwm3=values[10];
            pwm4=values[11];
            flag=2;
        }
        else if(adc>adcs[2] && adc<adcs[3])
        {
            pwm1=values[12];
            pwm2=values[13];
            pwm3=values[14];
            pwm4=values[15];
            flag=3;
        }
        else if(adc>adcs[3])
        {
            pwm1=values[16];
            pwm2=values[17];
            pwm3=values[18];
            pwm4=values[19];
            flag=4;
        }
        if(flag>oldflag) boost();
        
        // PWM table setting. 
        volatile uint8_t u = 0; // костыль, чтоб цикл не выкидывался
        for(; u < PWM_RES; u++)
        {
            uint8_t tbl_tmp=0;
            if(u<pwm1) tbl_tmp |= (1<<0);
            else tbl_tmp &= ~(1<<0);
            if(u<pwm2) tbl_tmp |= (1<<1);
            else tbl_tmp &= ~(1<<1);
            if(u<pwm3) tbl_tmp |= (1<<2);
            else tbl_tmp &= ~(1<<2);
            if(u<pwm4) tbl_tmp |= (1<<3);
            else tbl_tmp &= ~(1<<3);
            
            port_out_table[u] = tbl_tmp;
            
        }       
    }
}
 
 
//}