// Ejemplo con un servo FUTABA S3003
// Alimentacin y pulsos a 5V

// Cuadro de Tiempos :

//    Periodo 20 ms (Frecuencia 50 Hz)
//    Ancho Pulso minimo 0.5 ms
//    Ancho pulso medio  1.5 ms
//    Ancho pulso maximo 2.5 ms

//    TMR0 a 1:16 -> 1 RTCC cada 4.096 ms
//                -> 1 Tick cada 0.096 / 256 = 0.016 ms
//                -> 20 ms = (4 x RTCC completas) + (1 * RTCC - 30 ticks)

//    Ancho Pulso minimo 0.5 ms ->  31 ticks de TMR0
//    Ancho pulso medio  1.5 ms ->  93 ticks de TMR0
//    Ancho pulso maximo 2.5 ms -> 155 ticks de TMR0



#include <16f84A.h>
#fuses XT,NOWDT
#use delay(clock=4000000)
#byte PORTA=5
#byte PORTB=6
#byte PWRTE=1
#byte PORT_B_PULLUPS(1)

#define PIN_SERVOD PIN_A1  		  //servo derecha
#define PIN_SERVOI PIN_A2         //servo izquierda

const int RTCC         =30;
const int Pulsominimo  =31;
const int Pulsomedio   =93;
const int Pulsomaximo =155;

int1 flagRTCC    =0;
int  contRTCC    =0;
int1 flagSERVOD  =0;
int1 flagSERVOI  =0;
int  tSERVOD     =Pulsomedio;
int  tSERVOI     =Pulsomedio;



void main ()
{
  int ValTIMER0;
  set_tris_A=0;
  set_tris_B=1;
  PortA=0;
  PortB=0;
  
 do 
 {
     // DISPARO DEL PULSO PWM

      if(flagRTCC==1)
	   {
         flagRTCC=0;
         output_high(PIN_SERVOD);
         output_high(PIN_SERVOI);
         flagSERVOD=1;  
         flagSERVOI=1;   
       }

      // CONTROL DE ANCHO DEL PULSO PWM
      
       if(flagSERVOD==1)
	    {
             valTIMER0 = get_TIMER0();
          if(valTIMER0>tSERVOD)
	        {
             flagSERVOD=0;
             output_low(PIN_SERVOD);
            }   
         }
  
        if(flagSERVOI==1)
	     {
             valTIMER0 = get_TIMER0();
           if(valTIMER0>tSERVOI)
	         {
             flagSERVOI=0;
             output_low(PIN_SERVOI);
            }   
         }
  
  if((bit_test(PORTB,2))||(bit_test(PORTB,4))==0)
   { 
     tSERVOD=Pulsominimo;
     tSERVOI=Pulsominimo;
     delay_ms(20);
     tSERVOD=Pulsomedio;
     tSERVOI=Pulsomedio;
   }
  if((bit_test(PORTB,3))||(bit_test(PORTB,5))==0)
  { 
    tSERVOD=Pulsomaximo;
    tSERVOI=Pulsomaximo;
    delay_ms(20);
    tSERVOD=Pulsomedio;
    tSERVOI=Pulsomedio;
  }
 }
}