Programação no MikroC for PIC

[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;
  }
}