#include <htc.h>

// CONFIG
#pragma config FOSC = INTRCIO   // Oscillator Selection bits (INTOSC oscillator: I/O function on GP4/OSC2/CLKOUT pin, I/O function on GP5/OSC1/CLKIN)
#pragma config WDTE = OFF       // Watchdog Timer Enable bit (WDT disabled)
#pragma config PWRTE = ON       // Power-Up Timer Enable bit (PWRT enabled)
#pragma config MCLRE = ON       // GP3/MCLR pin function select (GP3/MCLR pin function is MCLR)
#pragma config BOREN = ON       // Brown-out Detect Enable bit (BOD enabled)
#pragma config CP = OFF         // Code Protection bit (Program Memory code protection is disabled)
#pragma config CPD = OFF        // Data Code Protection bit (Data memory code protection is disabled)

#define _XTAL_FREQ 4000000
#define servoPin GP2
//#define pulseWidth int


void servoPulse(pulseWidth) //Sends a single pulse to the servo
{
unsigned short Period = 20000; //total time of pulse in microseconds
T1CON=0b00000001; //Configuration for Timer 1. Prescaler = 1
//Send pulse for a duration of pulsewidth microseconds
TMR1 = 65536 - (_XTAL_FREQ/(4*1000000))*pulseWidth;//preload timer1 with this
TMR1IF = 0; //reset Timer 1 flag before proceedin
servoPin = 1;
while (!TMR1IF){} //When Timer1 hits 65536, this flag is set. Loop until then


 //Low voltage for the rest of the 20 ms
unsigned short offTime = Period - pulseWidth;
TMR1 = 65536 - (_XTAL_FREQ/(4*1000000))*offTime; //preload timer1 with this
TMR1IF = 0; //reset Timer 1 flag before proceeding
servoPin = 0;
while (!TMR1IF){}
}


/*Main Program*/
void main()
{
/* Setup - do this once */
//ADCON1  0b00000000;
ADCON0 = 0b00001101;
ANSEL = 0b00001000; //Configures pin 3 of port A as Analog
//GPIO3 = 1;
unsigned char adNumber;
unsigned short pulseWidth; //pulse width in microseconds
/* Loop forever*/
while(1)
{
GO_DONE = 1; /*Sets the GO_DONE bit in ADCON0 (bit 1)
which starts the A to D conversion*/
while (GO_DONE == 1); /*wait until A to D is complete, at which point
GO_DONE will drop to 0*/
adNumber = ADRESH; //reads the high register of the A to D result
// into port C, which displays it on the 7seg
pulseWidth = 1000 + (1000/250)*adNumber; //converts AtoD into a value from 1000 to 2000

servoPulse(pulseWidth);
 }
}