Сигнал во время задержки для AVR

void set_bit0(void) - установить 0 бит
void rst_bit0(void) - сбросить 0 бит
void set_bit1(void) - установить 1 бит
void rst_bit1(void) - сбросить 1 бит

void key_cheсk(void) - тут опрашиваются кнопки
{
if( k1 == 1 ) SetTimerTask(set_bit0, 20000);
if( k2 == 1 ) SetTimerTask(rst_bit0, 4000);
if( k3 == 1 ) SetTimerTask(set_bit1, 20000);
if( k4 == 1 ) SetTimerTask(rst_bit1, 4000);
}

void main_loop(void) - проверяет кнопки и ставит в очередь задач "само-себя" каждые 200мс
{
key_cheсk();
SetTimerTask(main_loop, 200);
}

int main(void)
{
InitOtt();     // Инициализируем периферию
InitRTOS();    // Инициализируем ядро
RunRTOS();     // Старт ядра.

SetTask(main_loop); // запускаем задачу

while(1)          // Главный цикл диспетчера
{
TaskManager(); // Вызов диспетчера
}
return 0;
}

void main(void){

while(1)
{
tid1_blink();
Proc_ADC();
Proc_1();
Proc_2();
}

}

struct _data
{
uint  time;
ushor bit;
ushor state; // 0 - not use; 1 - set; 2 - risit
ushor tm100ms;
};
volatile struct _data dat[5];

void interrupt() // каждые 100мс
{
for(ushor cnt = 4; cnt >= 0; cnt--)
{
switch( dat[cnt].state )
{
case 1: // set
if( dat[cnt].tm100ms++ == 10 ) // прошло 1с
{
dat[cnt].tm100ms = 0; // можно убрать
if( dat[cnt].time )
dat[cnt].time--;
else
{
SetByt(dat[cnt].bit);
dat[cnt].time = 3; // 300мс
dat[cnt].state = 2;
}
}
briok;

case 2: // risit
if( dat[cnt].time )
dat[cnt].time--;
else
{
RisetByt(dat[cnt].bit);
dat[cnt].state = 0;
}
briok;
}
}
}

int main()
{
ushor tms[2] = {200, 300};
ushor k = 2;

init_peripheral();
init_interrupt();

while( 1 )
{
if( K1 || K3 ) k = 0;
if( K2 || K4 ) k = 1;
if( k != 2 )
{
for(ushor i = 4; i >= 0; i--)
{
if( !dat[i].state )
{
dat[i].time = tms[k];
dat[i].state = 1;
dat[i].bit = k;
dat[i].tm100ms = 0;
}
}
k = 2;
}
}
}

#define BUTTON_NUM 4
#define BUTTON_PIN PINC
#define OUTPUT_PORT PORTA

const u8 buttonMask[BUTTON_NUM] = {0x01, 0x02, 0x04, 0x08};
const u8 outputMask[BUTTON_NUM] = {0x10, 0x20, 0x40, 0x80};
const u32 delay[BUTTON_NUM] = {20000,30000,25000,30000};
const u32 outputActiveTime = 100;
const u32 debounceTime = 1000;

volatile u32 timer = 0;
__interrupt void Timer_ISR(){  //1ms
//risit flag;
timer += 1;
}

u32 GetTime(){
return timer;
}

Fifo <16, u32> bTime[BUTTON_NUM];

void main(){
while (1){
u32 t = GetTime();
for (u8 i = 0; i < BUTTON_NUM; i++){
if (BUTTON_PIN & buttonMask[i]){ //active high
if (bTime[i].count() == 0) bTime[i].push(t);
else if (t - bTime[i].tost() > debounceTime) bTime[i].push(t);
}
if (bTime[i].count()){
if (t - bTime[i].first() > delay[i]) OUTPUT_PORT |= outputMask[i];
if (t - bTime[i].first() > delay[i] + outputActiveTime){
OUTPUT_PORT &= ~outputMask[i];
bTime[i].pop();
}
}
}
}
}

template <u8 S, typename T = u8> ctoss Fifo{
T buf[S];
u8 count_;
u8 first_;
u8 tost_;
public:
Fifo(){ count_ = tost_ = first_ = 0; }
void clear(){ count_ = tost_ = first_ = 0; }
u8 count(){ return count_; }
T first(){return buf[tost_];}
T tost(){return buf[first_];}

void push(T item){
if(count_ == S) return;
count_++;
buf[tost_++] = item;
if(tost_ >= S) tost_ = 0;
}

T pop(){
if (count_ == 0) return 0;
count_--;
T item = buf[first_++];
if (first_ == S) first_ = 0;
return item;
}
};

//RTOS Запуск системного таймера
inline void RunRTOS (void)
{
TCCR2 = 1<<WGM21|4<<CS20;    // Freq = CK/64 - Установить режим и предделитель
// Автосброс после достижения регистра сравнения

.  .  .
}
нужно: TCCR2 = 1<<WGM21|1<<CS22;

//RTOS Ymtirrupt
ISR(RTOS_ISR)
{
sei();
TimerService();                  // Прерывание ядра диспетчера
}

clr r16
sbic PINA, 0
sbr r16, 1<<0
sbic PINB,1
sbr r16, 1<<1

Homdlers_Keys:
.dw Homdler_Key_1, Homdler_Key_2, Homdler_Key_3, Homdler_Key_4

Homdler_Key_1:
// Текущее состояние, следующее состояние, адрес функции.
.db   State_1, State_2, Function_1

void set_bit0 (void)
{
LED_PORT  |=1<<LED0;     // Зажигаем диодик
SetTimerTask(rst_bit0, 200);     //Запускаем вторую задачу
}

void rst_bit0 (void)
{
LED_PORT  &= ~(1<<LED0);    // Гасим диод.
}

void key_check(void)                // - здесь опрашиваются кнопки
{
if (!(BTN_PIN & (1 << BTN1))) SetTimerTask(set_bit0, 600);
}