Realizar contador en un período de tiempo. PIC16F886 en C

Hola a todos y bienvenidos!

Hace mucho que no entro en este foro debido a cuestiones de tiempo. Me surje una duda y me gustaría que alguien me ayudara lo más rápido posible. El tema es el siguiente:

Estoy haciendo un proyecto con un pic16f886 en C, lo que deseo hacer son unos impulsos a través de un interruptor y que en el primer impulso empiece a contar (con un máximo de 4) y además se active el temporizador. Este contador estará limitado por un temporizador, que también será de no más de 4 segundos. La duda que me surje es ¿cómo puedo hacer esto? Os adjunto el código que llevo hecho:

Código:

#include <16F886.h>
//#include <16F876A.h>
#use delay(clock=4000000)
#use fast_io(C)
#use fast_io(B)
#use rs232(baud=9600,parity=N,xmit=PIN_C6,rcv=PIN_C7,errors)//Errors posa el error a RS232_ERRORS i els reestableix(reset) que es el que ens interesa.
						//En Hexa si val 90H crec que vol dir que no hi ha error. Si acava en 2 crec que hem omplert massa el buffer
						//El tema es que el buffer de recepció es de 2 bytes, si n'arriven 3 avans de llegir, peta a no ser que posem 'errors'
						//Els altres es perden.
//#use fast_io(C)
//#use fast_io(B)
#byte port_a = 5
#byte port_b = 6
#byte port_c = 7

char  dadas;

/*#INT_RDA                //rutina de interrupcio al rebre dades, no la usem
void rda_int()
{
 dadas=getchar();
 printf("interrupcio %C \n\r",dadas);
}*/


/*void parpadeo (int a);  //això no ho usem a l'exemple
void envchar(char a);*/

void main ()
{
   setup_oscillator(OSC_4MHZ,OSC_STATE_STABLE); // Configurem el quartz intern a 4MHZ i estabilizat per al 16F886
   setup_adc_ports(NO_ANALOGS);
   setup_adc(ADC_OFF);
   setup_spi(SPI_SS_DISABLED);
   setup_spi(FALSE);
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_256);//depres el trec
   setup_timer_1(T1_DISABLED);
   setup_timer_2(T2_DISABLED,0,1);
   setup_comparator(NC_NC_NC_NC);
   setup_vref(FALSE);
 //enable_interrupts(INT_RDA);  //HABILITAR INTERRUPCIO AL REBRE DADES
 //enable_interrupts(GLOBAL);  //habilita totes les interrupcions
	
	int canvi;
	int cont;
	int tiempo;

	set_tris_a(0b00001111);	//Port A. RA0 a RA3 com entrades, la resta són sortides
	set_tris_c(0b11001110);	//Port C. RC2, RC3, RC6 com entrades, la resta són sortides
	while (1)
	{
		cont = 0;
		if(input(PIN_A0))	
			bit_set (port_a, 4); //s'encen el led del pin_A4 quan pin_A0 estigui pulsat
		else
			bit_clear(port_a,4); //neteja el led quan es deixa de pulsar
			
		if(input(PIN_A2))
			bit_set (port_a, 5); //s'encen el led del pin_A5 quan pin_A2 estigui pulsat
		else
			bit_clear(port_a, 5); //neteja el led quan es deixa de pulsar
		//-------------------------------------------------------------
		
		do{
			if (input(PIN_A0)) // si pin_A0 està pulsat
		   	{
		       	delay_ms(10); // delay antirebot
		       	if (input(PIN_A0)) // si encara segueix a 1, no ha sigut un rebote.
		    	{
					if(cont == 0)
					{
						set_timer0 (0);
						cont++;
						while (input(PIN_A0))
						{
							delay_ms(200); // cicle tancat fins que no es deixi pin_A0
							contarTimer(tiempo);
						}
					}
					else
					{
						cont++;
		       			while (input(PIN_A0))
						{
							delay_ms(200); // cicle tancat fins que no es deixi pin_A0
							contarTimer(tiempo);
						}
					}
			    }
				if (cont >= 4)	//si contador es 4 o més
					cont = 4;	//entendrem que vol sortir del programa
		   	}
		}while (tiempo < 61);
		//s'ha d'enviar cont al port serie
		printf ("%d", cont);

		//-------------------------------------------------------------
			switch (cont)
			{
				case 1:
					output_bit(PIN_C2,1);  //dos intermitencies de 0.5 segon
			      	delay_ms(250); //
					output_bit(PIN_C2,0);  //dos intermitencies de 0.5 segon
			      	delay_ms(250); //
				case 2:
					output_bit(PIN_C2,1);  //dos intermitencies de 0.5 segon
			      	delay_ms(250); //
					output_bit(PIN_C2,0);  //dos intermitencies de 0.5 segon
			      	delay_ms(250); //
					output_bit(PIN_C2,1);  //dos intermitencies de 0.5 segon
			      	delay_ms(250); //
					output_bit(PIN_C2,0);  //dos intermitencies de 0.5 segon
			      	delay_ms(250); //
				case 3:
					output_bit(PIN_C2,1);  //dos intermitencies de 0.5 segon
			      	delay_ms(250); //
					output_bit(PIN_C2,0);  //dos intermitencies de 0.5 segon
			      	delay_ms(250); //
					output_bit(PIN_C2,1);  //dos intermitencies de 0.5 segon
			      	delay_ms(250); //
					output_bit(PIN_C2,0);  //dos intermitencies de 0.5 segon
			      	delay_ms(250); //
					output_bit(PIN_C2,1);  //dos intermitencies de 0.5 segon
			      	delay_ms(250); //
					output_bit(PIN_C2,0);  //dos intermitencies de 0.5 segon
			      	delay_ms(250); //
				case 4:
					output_bit(PIN_C2,1);  //dos intermitencies de 0.5 segon
			      	delay_ms(250); //
					output_bit(PIN_C2,0);  //dos intermitencies de 0.5 segon
			      	delay_ms(250); //
					output_bit(PIN_C2,1);  //dos intermitencies de 0.5 segon
			      	delay_ms(250); //
					output_bit(PIN_C2,0);  //dos intermitencies de 0.5 segon
			      	delay_ms(250); //
					output_bit(PIN_C2,1);  //dos intermitencies de 0.5 segon
			      	delay_ms(250); //
					output_bit(PIN_C2,0);  //dos intermitencies de 0.5 segon
			      	delay_ms(250); //
					output_bit(PIN_C2,1);  //dos intermitencies de 0.5 segon
			      	delay_ms(250); //
					output_bit(PIN_C2,0);  //dos intermitencies de 0.5 segon
			      	delay_ms(250); //
			}			
}

		int contarTimer (int *tiempo)
		{
			if (get_timer0() > 200)
			{
				get_timer0(0);
				tiempo++;
			}
			return tiempo;
		}

Lo veis bien, mal... Qué cambiaríais??

Gracias