#include <18f2550.h>
#fuses HSPLL,PLL5,NOMCLR,PUT,NOWDT,NOPROTECT,BROWNOUT,NOLVP,NOCPD,NODEBUG,NOWRT //HS,PUT,NOWDT,NOPROTECT,BROWNOUT,NOLVP,NOCPD,NODEBUG,NOWRT
#use delay(clock=48000000)
#use rs232(baud=19200, xmit=PIN_C6, rcv=PIN_C7,errors)
#use fast_io(A)
#use fast_io(B)
#use fast_io(c)

#priority rda,timer1,CCP2




//#define tx     PIN_A3

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 = 7500; // PWM High for Center Position = 1.5 ms 
const int16 Ticks4Maximum = 11500; // PWM High for Maximum Position = 2.3 ms 

char command;
static int16 Servo_PWM[8]={Ticks4Center,Ticks4Center,Ticks4Center,Ticks4Center,0,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 SERVO5_ON=0;
static int1 SERVO6_ON=0;
static int1 SERVO7_ON=0;
static int1 SERVO8_ON=0;
static int1 flag_Phase;
static int16 Ticks4NextInterrupt=53036;


int Servo_nro=0;
int   Paso=0;       //maquina de estados para saber que hacer
int   indice=0;
int   rxBuffer[8];
int16 Servo_POS[16]={7500,7580,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500,7500};
int1  rxOK=0;
int1  fFase=0;
int1  fServo1=0;//servo1 menor que servo2
int1  iguales=0;
int16  Port_Copy=0;
INT16  Tics2_5ms=0;

#int_rda
void serial_isr(void){
command=0;
   
  if(kbhit()){
    command=getc();
    rxBuffer[indice]=command; 
    INDICE++;
    if(indice>7)indice=0,rxOK=1;
  }
}

#int_timer1
void timer1_isr(void){
int16 tservo;
int16 tservo1;
tservo=Servo_Pos[Servo_nro];
tservo1=Servo_Pos[Servo_nro+1];
//    fServo1=0;
    iguales=0;
    bit_set(Port_Copy,Servo_nro);
    bit_set(Port_Copy,Servo_nro+1);
 
    output_b((int8)Port_Copy & 0x00ff);
    //output_D((int8)(Port_Copy>>8)& 0x00ff);
    if(tservo>tservo1){
            fServo1=1;
            Ticks4NextInterrupt=tservo;
            ccp_2=tservo1;
            Tics2_5ms=65535 - Ticks4Window + tservo;
    }        
    else{
            fServo1=0;
            Ticks4NextInterrupt=tservo1;    
            ccp_2=tservo;
            Tics2_5ms=65535 - Ticks4Window + tservo1;
    }
    if(tservo1==tservo)flag_phase=1;
//     #ASM bcf  0xd,0
//          bcf  0xc,2
//     #ENDASM   
    set_timer1(0);
    enable_interrupts(int_ccp2);
}

///////// INTERRUPCION IGUALACION DE CCP_2 Y TIMER1 ////////////

#int_ccp2
void ccp2_int(){
int16 temp=0;
   if(flag_phase==0){//baja primer servo
      if(fServo1)     bit_clear(Port_Copy,Servo_nro);
      if(!fServo1)    bit_clear(Port_Copy,Servo_nro+1);
      CCP_2=Ticks4NextInterrupt;
      flag_phase=1;
   }      
   ELSE{
      OUTPUT_TOGGLE(PIN_A2);
      Port_Copy=0;
      set_timer1(Tics2_5ms);
      if((Servo_nro+=2)>15)Servo_Nro=0;
      disable_interrupts(int_ccp2); 
      flag_phase=0;
   }
   
   output_b((int8)Port_Copy & 0x00ff);
   // output_D((int8)(Port_Copy>>8)& 0x00ff);
}



void pres_menu(void){

  printf("\r\n[?] This menu");
  printf("\r\n[I] All to Minimum");
  printf("\r\n[C] All to Center");
  printf("\r\n[X] All to Maximum");
  printf("\r\n[+] Step to front");
  printf("\r\n[-] Step to back");
  printf("\r\n\n>");

}

void main(void) {

  int1 valid_command;
  int8 i;
  float temp=0;
  int16 temp_pos=0;

  disable_interrupts(global);
  setup_adc_ports(NO_ANALOGS);
  setup_adc(ADC_OFF);
  setup_counters(RTCC_INTERNAL,RTCC_DIV_2);
  //setup_timer_0(RTCC_OFF);
  setup_timer_1(T1_INTERNAL | T1_DIV_BY_2);
  setup_timer_2(T2_DISABLED,0,1);
  setup_ccp2(CCP_COMPARE_INT);    // Configura CCP2 en modo COMPARE interrumpe cuando son iguales
  setup_timer_3(T3_DISABLED);
  port_b_pullups(FALSE);

  set_tris_a(0b00000000);
  set_tris_b(0b00000000);
  set_tris_c(0b11000000);
  //set_tris_D(0b00000000);

  delay_ms(1000);
  command='\0';
  enable_interrupts(int_rda);
  set_timer1(Ticks4NextInterrupt);
  enable_interrupts(int_timer1);
  enable_interrupts(global);


  do {
      if (rxOK==1){
         rxOK=0;
         output_high(PIN_A3);
         DELAY_MS(1);
         printf ("@");
         for (i=0;i<=7;++i){
            temp_pos=rxBuffer[i];
            Servo_Pos[i]=(temp_pos*80)+3500;
            printf ("%U",rxBuffer[i]);//,Servo_Pos[i]);
            rxBuffer[i]=0;
            indice=0;
         }
         output_low(PIN_A3);
      }
  } while (TRUE);
}