#include <16F648A.h>
#use delay(clock=4000000)
#fuses XT,PUT,BROWNOUT,MCLR,NOWDT,NOPROTECT,NOLVP
#use rs232(baud=9600,parity=N,xmit=PIN_B2,rcv=PIN_B1,bits=8)
//Servo tester

#include <stdlib.h> 

#byte	PORTB   = 0x06 
#byte	TRISB   = 0x86 
#byte	PR2     = 0x92
#byte	TMR2	= 0x11
#byte	T2CON	= 0x12
#byte	CCPR1L	= 0x15
#byte	CCP1CON	= 0x17
#byte	CMCON   = 0x1F

#bit PINCCP1 = TRISB.3

#define TIMER2START() T2CON=0B01111111;         //Bit2=1
#define TIMER2STOP()  T2CON=0B01111011;         //Bit2=0

#define DUTYMAX  80
#define DUTYMIN  20

#define TESTONLY
//Convert 10 bit PWM duty Cycle Value
//to  8bit CCPR1L and 2 bit CCPR1X,CCPR1Y in  CCP1CON<5:4>
//Long = 16bit
void ConvertPWM(unsigned long  PWMduty,unsigned int  *RCCPR1L,unsigned int  *RCCP1CON54)
{
unsigned int  D;
unsigned long Buff;

    Buff =PWMduty;
    Buff >>=2;
    *RCCPR1L=(unsigned int)buff;

    *RCCP1CON54=0;
    D=PWMduty&0x01 ;
    if(D!=0)
      *RCCP1CON54|=0x10;
    D=PWMduty&0x02;
    if(D!=0)
      *RCCP1CON54|=0x20;

    *RCCP1CON54|=0B00001111;      //PWM mode

}

//Find PWM duty Cycle Value
//Output 10 bit
long  CCPDPWM(int duty)
{
int P;
long  iCCP1;
float DPWM,Tosc,CCPR1;

//DPWM= dxT/100;
    DPWM  = (duty*0.003)/100;       //Pluse =333Hz
    Tosc = 0.00000025;              //XTAL 4MHz
    P=16;                           //Prescaler=16

//CCPR1L:CCP1CON<5:4> = DPWM/Tosc*P;
    CCPR1=DPWM/(Tosc*P);            
    iCCP1=(long)CCPR1; 

 return iCCP1 ;
}

//Covert 10 bit Binary to String
void Bin10ToStr(long PWMduty,char *Str)
{
int i;
long int M,D;
//unsigned int M,D;

    Str[0]=0;
    M=0B1000000000;             //0B1000000000  mark bit 10   0x200;
    for(i=0;i<10;i++)       //Max bit=10;
    {
     D=PWMduty&M;
     if(D==0)
      Str[i]='0';
     else
      Str[i]='1';
     M>>=1;
    }
    Str[i]=0;

}
//Covert 8 bit Binary to String
void Bin8ToStr(unsigned int DAT,char *Str)
{
int i;
long int M,D;
//unsigned int M,D;

    Str[0]=0;
    M=0B10000000;          //Mark bit
    for(i=0;i<8;i++)       //Max bit=8;
    {
     D=DAT&M;
     if(D==0)
      Str[i]='0';
     else
      Str[i]='1';
     M>>=1;
    }
    Str[i]=0;
}

void SetDutyCycle(int duty)
{
unsigned long PWMD;
   PWMD=CCPDPWM(Duty);
   ConvertPWM(PWMD,&CCPR1L,&CCP1CON);
}
void DemoSevo(void)
{
int i;
int cycle[16]={15,20,25,30,35,40,45,50,75,70,65,60,55,50,20,50};
//int time[5]={20,50,80,55,15};
  
  for(i=0;i<16;i++)
  {
   SetDutyCycle(cycle[i]);
   delay_ms(250);
  }
}
void main()
{
char c,buff[15];
//unsigned long PWMD;
//unsigned int i,Duty,RCCPR1L,RCCP1CON54;

int i,Duty,Dat;

    set_tris_b(0B00000010);     //RB1 is RX

    printf("Servo Tester\r\nPress Duty cycle and press Enter\r\n"); 

    PR2     = 186;           //Set TIMER2 frequency 333Hz
    Duty=50;
    SetDutyCycle(Duty);
    TMR2    = 0;            //Clear TMR2 first
    T2CON   = 0B01111111;   //Set prescaler T2CON<0:1>= 11 T2CON<2>=0 stop CCP1

    while(1)
    {
     if(kbhit())            //Check data Receive 
     {   
       c=getc();            //Get data

       switch(c)
       {
        case '\r':
          buff[i]=0;
          i=0;
          Dat=atoi(buff);
          printf("%u\r\n",Dat);
          if(Dat!=0)
          {
           Duty=Dat;
           SetDutyCycle(Duty);
          }
            break;
        case 'd':
            DemoSevo();
            Duty=50;
            SetDutyCycle(Duty);            
            break;
        case '+':
              Duty++;
              SetDutyCycle(Duty);
              printf("%u\r\n",Duty);
            break;
        case '-':
              Duty--;
              SetDutyCycle(Duty);
              printf("%u\r\n",Duty);
            break;

        case 'o':
            i=0;
            PINCCP1=1;              //Tristate input
            printf("Signal OFF\r\n");
            break;
        case 'n':
            i=0;
            PINCCP1=0;              //Tristate output
            printf("Signal ON\r\n");
            break;

        default:
            buff[i]=c;
            i++;
            if(i>=15)
            {
             i=0;
             buff[0]=0;
             printf("buffer full\r\n");
            }

       }

     }
    }

}