#include <18f4525.h>


#fuses HS,NOMCLR,PUT,NOWDT,NOPROTECT,BROWNOUT,NOLVP,NOCPD,NODEBUG,NOWRT
#use delay(clock=20000000)

#include <flexlcd.h>

#use fast_io(A)
#use fast_io(B)

#priority timer1                     //prioridad de interrupciones timer1 sobre interrupcion serial rda

#define SERVO1 PIN_B0
#define SERVO2 PIN_B1
#define SERVO3 PIN_B2
#define SERVO4 PIN_B4


const int16  Ticks4Window  = 12500;  // PWM Window for servo = 2.5 ms x 8 = 20 ms
const int16  Ticks4Minimum =  3500;  // PWM High for Minimum Position = 0.7 ms
const int16  Ticks4Center  =  7415;  // PWM High for Center  Position = 1.483 ms
const int16  Ticks4Maximum = 11277;  // PWM High for Maximum Position = 2.3 ms

static float Servo_PWM[4]={Ticks4Center,0,0,0};

static int8  Servo_Idx=0;

static int1  SERVO1_ON=1;
static int1  SERVO2_ON=1;
static int1  SERVO3_ON=1;
static int1  SERVO4_ON=1;

static int1  flag_Phase;
static int16 Ticks4NextInterrupt=53036;

#int_timer1

void timer1_isr(void){

 if(flag_Phase==0){

     if(Servo_Idx==0 && SERVO1_ON) output_high(SERVO1);
     if(Servo_Idx==1 && SERVO2_ON) output_high(SERVO2);
     if(Servo_Idx==2 && SERVO3_ON) output_high(SERVO3);
     if(Servo_Idx==3 && SERVO4_ON) output_high(SERVO4);

     Ticks4NextInterrupt = 65535 - Servo_PWM[Servo_Idx];
     set_timer1(Ticks4NextInterrupt);

     }

     if(flag_Phase==1){

     if(Servo_Idx==0 && SERVO1_ON) output_low(SERVO1);
     if(Servo_Idx==1 && SERVO2_ON) output_low(SERVO2);
     if(Servo_Idx==2 && SERVO3_ON) output_low(SERVO3);
     if(Servo_Idx==3 && SERVO4_ON) output_low(SERVO4);

     Ticks4NextInterrupt = 65535 - Ticks4Window + Servo_PWM[Servo_Idx];
     set_timer1(Ticks4NextInterrupt);

     if(++Servo_Idx>4) Servo_Idx=0;
     }
     ++flag_Phase;
}


void main(void){
int8 i;
int8 p;
float ticknumber;


enable_interrupts(global);                  //Desactivar interrupciones globales
setup_adc_ports(NO_ANALOGS);                 //Desactivar puertos analogos
setup_adc(ADC_OFF);                          //Apagando modulo ADC
setup_counters(RTCC_INTERNAL,RTCC_DIV_2);    //Configurando contadores internos
setup_timer_0(RTCC_OFF);                     //Desactivando el timer0
setup_timer_1(T1_INTERNAL | T1_DIV_BY_1);    //Activando el timer1, configurandolo 1:1
setup_timer_2(T2_DISABLED,0,1);              //Desactivando el timer2
setup_timer_3(T3_DISABLED);                  //Desactivando el timer3
port_b_pullups(FALSE);                       //Desactivando resistencias pullups

set_tris_a(0b00000000);                      //configurando puertos
set_tris_b(0b00000000);


output_low(SERVO1);
output_low(SERVO2);
output_low(SERVO3);
output_low(SERVO4);


delay_ms(1000);

enable_interrupts(int_rda);
set_timer1(Ticks4NextInterrupt);             //precargando el timer1
enable_interrupts(int_timer1);               //activando la interrupcion del timer1
enable_interrupts(global);                   //activando interrupciones globales

ticknumber=(44,44)*10;
ticknumber=11500-ticknumber;


         while (p<1000){
         Servo_PWM[0]=Ticknumber;
         Servo_PWM[1]=Ticks4Maximum;
         p++;
         }

}