/*
Chris @ http://www.pyroelectro.com
Date: 10/31/2009

Simple Motor Optical Encoder

Program Description: This program takes ADC data from an IR detector circuit
and uses that as feedback for the position of a stage controlled by a 12v DC motor.
This program outputs two PWM signals to control the speed of that motor.

**NOTE**
I didn't clean up this code so it might be possible that there are unused variables.

*/

#include <p18f4520.h>
#include <pwm.h>
#include <timers.h>
#include <adc.h>

#define SPEED 90 		//INBETWEEN 0-100 

int movement = 1;
int direction = 0;

int i = 0;
int count = 0;
int result = 0;
int current = 0;
long int total=0;

void set_speed(int);

void move_center(void);

void main(void)
{

TRISD = 0x00;
PORTD = 0xFF;

/****Find Initial Location***/

total = 0;

for(i=0;i<20;i++){
OpenADC(ADC_FOSC_32 & ADC_RIGHT_JUST & ADC_20_TAD, ADC_CH1 & 
ADC_INT_OFF & ADC_VREFPLUS_VDD & ADC_VREFMINUS_VSS, 0);

Delay10TCYx( 5 ); // Delay for 50TCY
ConvertADC(); // Start conversion
while( BusyADC() ); // Wait for completion
total += ReadADC(); // Read result
CloseADC(); // Disable A/D converter
}

result = total/20;

if(result > 0x03E3)
	current = 0; //BLACK
else if(result < 0x03DB)
	current = 1; //WHITE

/**Setup Motors**/

OpenTimer2(TIMER_INT_OFF & T2_PS_1_1);
	//PWM period =[(period ) + 1] x 4 x TOSC x TMR2 prescaler
	// Fosc = 40 MHz
	// TMR2 prescaler = 1

OpenPWM1(0xFF);
OpenPWM2(0xFF);

SetOutputPWM1(SINGLE_OUT, PWM_MODE_3);
	//PWM_MODE_3 -> PWM2 Inverted Signal
move_center();


if(direction)
	set_speed(0x3EF-SPEED);
else
	set_speed(SPEED);


count = 0;

while(1){

total = 0;

for(i=0;i<20;i++){
OpenADC(ADC_FOSC_32 & ADC_RIGHT_JUST & ADC_20_TAD, ADC_CH1 & 
ADC_INT_OFF & ADC_VREFPLUS_VDD & ADC_VREFMINUS_VSS, 0);

Delay10TCYx( 5 ); // Delay for 50TCY
ConvertADC(); // Start conversion
while( BusyADC() ); // Wait for completion
total += ReadADC(); // Read result
CloseADC(); // Disable A/D converter
}

result = total/20;

if(result > 0x03E3){
	if(current == 1)
		count++;
	PORTD = 0xFF;
	current = 0; //BLACK
}
if(result < 0x03DB){
	if(current == 0)
		count++;
	PORTD = 0x00;
	current = 1; //WHITE
}

if(count == movement){
set_speed(0x1FF);
Delay10KTCYx(10);

count = 0;
movement++;

if(movement == 14){
	Delay10KTCYx(250);
	Delay10KTCYx(250);
	
	move_center();
	movement = 1;
	count = 0;
}

if(direction == 1){
	direction = 0;
	set_speed(SPEED);
}
else if(direction == 0){
	direction = 1;
	set_speed(0x3EF-SPEED);
}

}


}

}

void set_speed(int speedy){

SetDCPWM1(speedy);  // set the duty cycle (provides volume)
SetDCPWM2(speedy);  // set the duty cycle (provides volume)

}

void move_center(void){

//Move To The Left Hand Side
set_speed(0x380);

Delay10KTCYx(250);
Delay10KTCYx(250);
Delay10KTCYx(250);
Delay10KTCYx(250);


//Set Original Speed
set_speed(SPEED);


//Begin Move To Center
while(1){

total = 0;

for(i=0;i<20;i++){
OpenADC(ADC_FOSC_32 & ADC_RIGHT_JUST & ADC_20_TAD, ADC_CH1 & 
ADC_INT_OFF & ADC_VREFPLUS_VDD & ADC_VREFMINUS_VSS, 0);

Delay10TCYx( 5 ); // Delay for 50TCY
ConvertADC(); // Start conversion
while( BusyADC() ); // Wait for completion
total += ReadADC(); // Read result
CloseADC(); // Disable A/D converter
}

result = total/20;

	if(result > 0x03E3){
		if(current == 1)
			count++;
		PORTD = 0xFF;
		current = 0; //BLACK
	}
	if(result < 0x03DB){
		if(current == 0)
			count++;
		PORTD = 0x00;
		current = 1; //WHITE
	}

	if(count == 14){
	set_speed(0x1FF);
	return 0;	
	}
}

}