//SERVO TESTER / CONTROLLER

//Program to move a servo with a pot
//Or more technically: Generate a repeating pulse 1-2ms high and 20ms low
//Written for PIC12F683 / CC5X compiler

//Personal and non-commercial use encouraged
//Otherwise Copyright 2007 - David Theunissen - See www.flyelectric.ukgateway.net

//PREPROCESSOR section ========================================================

#include "12F683.H"				//PIC chip file
#include "delay_ms.h"			//Delay function used below

#pragma config = 0b.11.1100.0100//Core device configuration

								
//VARIABLES section ===========================================================

#define LED 	GPIO.4			//LED output defined (pin3)
#define SERVO 	GPIO.5			//Servo output defined (pin2)

uns16 POT;						//Variable to represent the ADC value of the servo control pot
uns16 PULSE;					//Variable to calculate pulse width
uns16 p;						//Variable used in timers/counters


//FUNCTIONS section ===========================================================
/*	
void write_data()				//Function to write to EEPROM flash memory (used for setup)
{
	GIE = 0;					//Disable all interrupts if not done elsewhere
    PIR1.7 = 0;					//EEIF: Clear this bit which gets set after each write
	
	EECON2 = 0x55;				//Required before writing to memory
	EECON2 = 0xAA;				//Required before writing to memory
	EECON1.1 = 1;				//WR: Writes data EEDAT to address EEADR

	while(EECON1.1==1);			//Wait for write to complete (becomes 0 when complete)

	EEADR++;					//Increment the address by 1 for next write (8 bit 0-255)

	//GIE = 1;					//Enable interrupts if appropriate
}
*/
//-----------------------------------------------------------------------------

void main()
{
//Main port settings
	TRISIO2 = 1;				//Sets GP2/AN2 (pin5) to be Input for pot
	TRISIO4 = 0;				//Sets GP4/AN3 (pin3) to be Output for LED
	TRISIO5 = 0;				//Sets GP5 (pin2) to be Output for Servo
    
    ANS2 = 1;					//Enables ADC port AN2 for pot control
    ANS3 = 0;					//Disables ADC port AN3 (make digital) for LED
    
    CMCON0 = 0b.111;			//Switches all 3 comparator ports off

//ADC settings (ADC needed for pot input)
    ADCON0.6 = 0;				//VCFG: Use Vdd for ADC voltage reference
    ANSEL.4 = 1;				//ADCS: Set AD Conversion Clock to Fosc/16
    ANSEL.5 = 0;				//part of above
    ANSEL.6 = 1;				//part of above
    ADCON0.7 = 1;				//ADFM: Right-justify ADC register
	
//Activate ADC
    ADCON0.0 = 1;				//ADON: Turn AD module ON
    ADCON0.3 = 1;				//CHS0: Select input channel to ANS2 only
    ADCON0.2 = 0;				//part of above
    ADIE = 0;					//ADC interrupts disabled

//Static Timer2 settings (use to generate high pulses)
	T2CON.1=1;					//Sets prescaler to 16

/*	
//Static EEPROM data logging settings (used for initial setup and calibration)
    EECON1.3 = 0;				//WRERR: clear on startup in case previously set
	EECON1.2 = 1;				//WREN: enable writing to memory
    EEADR = 0;					//Address to be used for first write
*/

//Start low
	SERVO = 0;
	    
//Flash LED three times on startup	
	for (p=0; p<3; p++)
	{
		LED=1;
		delay_ms(250);
		LED=0;
		delay_ms(250);
	}
	LED=1;						//Leave LED on to indicate powered up
		

    while(1)						//Continuous loop to monitor pot and instruct servo to move
    { 
		
	//Run ADC to convert pot position into digital
    	delay_ms(1);				//Delay for acquisition capacitor to charge
		ADIF = 0;					//Clear the ADC Complete flag
		ADCON0.1 = 1;				//GO: Start ADC acquisition
		while(ADIF==0);				//Wait for conversion to complete (become 1)

	//Determine ADC value (this is a 10bit value stored in two registers that need merging)
		POT = ADRESH;				//Copies upper 2 bits from 8bit register to 16bit
		POT = POT<<8;				//<<8 moves all bits 8 places left (hence the need for 16bits)
		POT = POT|ADRESL;			//| merges Lower 8 bits of ADRESL with upper 2

//-----------------------------------------------------------------------------
/*

Used for determining pot values		

	EEDAT=PULSE.high8;
	write_data();
	
	EEDAT=PULSE.low8;
	write_data();
		
Flash LED three times when done then wait 1sec
	for (p=0; p<3; p++)
	{
		LED=1;
		delay_ms(250);
		LED=0;
		delay_ms(250);
	}
	delay_ms(1000);
	    
*/
//-----------------------------------------------------------------------------

	//The calculation that follows creates a pulse width parameter 'PULSE' based on the pot position
	//With the prototype's pot, the ADC values range from 322 to 629 (measured by logging in EEPROM)
	//Goal is to make these represent 1 to 2ms (standard servo pulse widths)
	//Timer2 requires 63 for 1ms and 126 for 2ms
	
		PULSE = POT;				//Pick up ADC POT value
		PULSE -= 322;				//Deduct POT's lowest ADC value (bottom end)
		PULSE *= 63;				//Multiply by difference between timer values for 2 and 1ms (126-63=63)
		PULSE /= 307;				//Divide by difference between Pot upper and lower values (629-322=307)
		PULSE += 63;				//Add timer value for 1ms

		
	//Instruct servo to move!
		PR2 = PULSE.low8;			//Timer 2 counts to the value set in PR2 ('period register')
									//Timer 2 can only handle 8bits; required value is within this limit
		TMR2=0;						//Initialise Timer2
		TMR2IF = 0;					//Reset interrupt flag
		
		SERVO = 1;					//Go high
	    TMR2ON = 1;					//Start Timer2 (timer increments to PR2 value)
		while(TMR2IF==0);			//TMR2IF flag is set to 1 when Timer2 reaches PR2
		TMR2ON = 0;					//Stop Timer2
		
		SERVO = 0;					//Go low
	    delay_ms(20);				//for 20ms
	    
	    
	}							//end while
}								//end main