Lince Negra

Preciso de um programa, no pic 16f628a, q aciona, ao pressionar de um botão de trava, duas portas em nivel alto e quando pressionar o botão novamente uma das portas desliga e a outra dá uma especie de delay e depois (de uns 2 minutos) desliga. E não estou conseguindo fazer a ultima parte de desligar após o tempo estimado, alguém poderia me auxiliar ? Como tá a programação por enquanto: CMCON = 7; TRISB = 3; RB0_bit = 0; RB1_bit = 0; RB2_bit = 0; RB3_bit = 0; RB4_bit = 0; void main(){ int cont = 0, cont1 = 4; while(1) { if((RB0_bit == 1)&&(RB1_bit == 0)) { RB2_bit = 1; RB4_bit = 1; RB3_bit = 0; } else if((RB1_bit == 1)&&(RB0_bit == 0)) { RB2_bit = 0; RB4_bit = 0; RB3_bit = 1; } else if(RB0_bit == 0) { cont ++; if ((cont != 0)&&(RB1_bit == 0)) { cont1 --; RB2_bit = 1; RB4_bit = 0; RB3_bit = 0; } else if ((cont1 == 0)&&(RB1_bit == 0)) { RB2_bit = 0; } } else { RB2_bit = 0; RB4_bit = 0; RB3_bit = 0; } }