MainIO Module Pseudocode

/* Subroutines */

ReadADC: takes nothing, returns CurrADValue
If we need to get a new A/D value (GoDoneSetFlag = 0)
	Set GODONE
	Set GoDoneSetFlag
If the new A/D value is ready (GODONE = 1)
	Store ADRESH in CurrADvalue
	Clear GoDoneSetFlag
Return CurrADValue

SetServo: takes desired Position, returns nothing
set the ServoHighTime to the desired Position (in ms)

SetDuty: takes LeftDuty and RightDuty, returns nothing
Bound LeftDuty at 100
Set SentLeftDuty to LeftDuty*255/100
Bound RightDuty at 100
Set SentRightDuty to LeftDuty*255/100
Set the CCPR registers to the SentLeftDuty and SentRightDuty values
	

OutputCompareISR: takes nothing, returns nothing
Clear flag
If we need to set our high time
	Set the currHighTime to the ServoHighTime
	Load the currHighTime into CCPR3L and CCPR3H
	Set the lastServoState to HI
	Set output line high
Else if we need to set our low time
	Set the currLowTime to PERIOD - ServoHighTime
	Load the currLowTime into CCPR3L and CCPR3H
	Set the lastServoState to LO
	Set output line low
Clear the timer


/* Initializations */

InitPWM: takes nothing, returns nothing
Set CCP2 to RB3
Set PWM period = (PR2 + 1)*4*Tosc*TMR2Prescale = (255+1)*4/(20e6)*4 = 0.2048ms => 4.88 kHz	
Set PWM initial duty cycle
Make CCP1 and CCP2 an output
Use timer 2 frequency: 19.53 kHz
Set TMR2 prescale to 1 and enable TMR2
Configure CCP1 & CCP2 for PWM

InitServo: takes nothing, returns nothing
Configure CCP3 to output compare with software interrupt
Clock source: fosc/4 (20MHz/4)
Prescale: 1:4 (timer runs at 5MHz/4 = 1250kHz)
Don't use timer 1 as the system clock (default)
Don't turn on Timer1 interrupt
We'll be using the OC interrupt instead
Set the output line initially low
Make sure at least our pin (AN13) is not analog
Set initial flag to NO_FLAG
Schedule how long we wait before we start our OC
Clear the timer before we time our first OC
Turn on timer1

InitADC: takes nothing, returns nothing
Disable TRISB0
Configure AN0 as Analog (See Page 155 in DataSheet)
Select ADC Conversion Clock (32 Tosc, shortest for 20 Mhz Clock)
Configure Voltage Reference (Vref connected to Vdd/Vss)
Select ADC Acquisition Time
Turn On ADC Module
Select ADC Input Channel (AN0)
Select Result Format
Left Justified
Wait 

/* Interrupts */

IO_ISR: takes nothing, returns nothing
If the output compare has fired
	Call OutputCompareISR