/****************************************************************************
Dbt_light_seek_c
Derbot seeks light. PWM applied. Speed is dependent on light difference 
(front to back), so Derbot comes to a halt when light difference is minimal. 
Microswitches used for bump detection.
Files c018i.o and p18f242.lib are included by the Linker Script. 

TJW 12.11.05											Tested 27.11.05
****************************************************************************
Clock is 4MHz. Configuration Word, to be set in MPLAB: crystal oscillator (HS), 
WDT off, power-up timer on, code protect off, all others default*/

#include <p18F242.h>
#include <adc.h>
#include <timers.h>
#include <pwm.h>
#include <delays.h>		

/*function prototypes, reproduced from Header Files for information
	void OpenPWM1 (char);
	void OpenPWM2 (char);
	void OpenTimer2 (unsigned char);
	void Delay10KTCYx (unsigned char);
	void Delay10TCYx (unsigned char);
	void OpenADC (unsigned char, unsigned char);
	void SetChanADC (unsigned char);
	void ConvertADC(void);
	char BusyADC(void);
	int  ReadADC(void);			*/

//User-defined function prototypes
	void leftmot_fwd (void);
	void rtmot_fwd (void);
	void leftmot_rev (void);
	void rtmot_rev (void);
	void rev_left (void);
	void rev_rt (void);
	void fwd_left (void);
	void fwd_rt (void);
	void rotate_rt (void);
	void rotate_left (void);
	void diagnostic (void);

//Declare Variables
	int ldr_rt;   	    //right ldr value
	int ldr_left;   	//left ldr value
	int ldr_rear;   	//rear ldr value
	int ldr_ave;	  	//computed average of front ldrs
	int ldr_diff;  	    //difference between front ldrs, left - right
	int ldr_fwd;	  	//ave fwd speed required
	int fwd_dr_left;	//offset added to left PWM for fwd motion
	int fwd_dr_rt;	  	//offset added to right PWM for fwd motion

//Main Program
	void main (void)
{
//Initialise. Active bits identified. Unused bits set as outputs.
	TRISA = 0b00001011;	//ADC channels set as inputs, 
							//bits 2&5 are motor enables
	TRISB = 0b00110000; //bits 4 & 5 are uswitch inputs
	TRISC = 0b10000000; //bit 7 is mode switch, 1 & 2 are PWM
//Enable Timer 2, with pre- and post-scalers divide-by-1
	OpenTimer2 (TIMER_INT_OFF & T2_PS_1_1 & T2_POST_1_1);
	OpenPWM1 (0xFF);    //Enable PWM1 and set period
	OpenPWM2 (0xFF);    //Enable PWM2 and set period
//Enable ADC. Port A bits 0,1,3 are analog input, internal reference, 
//right justify result
	OpenADC(ADC_FOSC_8 & ADC_RIGHT_JUST & ADC_3ANA_0REF,ADC_CH0 & ADC_INT_OFF); 
//Switch all outputs off
	PORTA = PORTB = PORTC = 0;
//call diagnostic function	
	diagnostic();
//enable motors at idle speed
	CCPR1L = CCPR2L = 0x80; 
	PORTAbits.RA5 = 1;	
	PORTAbits.RA2 = 1;
		
//**********************************************************************
//this is main loop. 
//**********************************************************************
while (1)
  {
//check first for collisions 
	if (PORTBbits.RB4 == 0) //Test right uswitch
	rev_left ();
	if (PORTBbits.RB5 == 0) //Test left uswitch
	rev_rt ();
//Read and store all ldr values
	//left channel 
	SetChanADC (ADC_CH0);	
	Delay10TCYx (2);		//delay for 20us approx acquisition time
	ConvertADC();
	while (BusyADC());		//wait for conversion to complete
	ldr_left = ReadADC()&0x03FF;  // read it, AND out unwanted bits 
	ldr_left = 1024 - ldr_left;   //reverse polarity
	//right channel 
	SetChanADC (ADC_CH1);	
	Delay10KTCYx (2);		//delay for 20us approx acquisition time
	ConvertADC();
	while (BusyADC());
	ldr_rt = ReadADC()&0x03FF;  // read it, AND out unwanted bits
	ldr_rt = 1024 - ldr_rt; //reverse polarity
	//rear channel 
	SetChanADC (ADC_CH3);	
	Delay10KTCYx (2);		//delay for 20us approx acquisition time
	ConvertADC();
	while (BusyADC());
	ldr_rear = ReadADC()&0x03FF;  // read it, AND out unwanted bits ;
	ldr_rear = 1024 - ldr_rear; //reverse polarity
//Compute some intermediate variables
	ldr_diff = (ldr_left - ldr_rt); //difference between ldrs
	ldr_ave = (ldr_left + ldr_rt);  //average front two ldrs
	ldr_ave = (ldr_ave>>1);  		//divide this +ve no. by 2
	ldr_fwd = ldr_ave - ldr_rear;	//front to back differential - to set forward speed
	if (ldr_fwd < 0) ldr_fwd = 0;   //set minimum value
//determine action, by comparing LDR readings
	if (ldr_left > ldr_rt)
	{if (ldr_left > ldr_rear)
		fwd_left();				//ldr_left is brightest, go forward left
	  else rotate_left ();		//rear is brightest, rotate towards light
	}
	else 
	{if (ldr_rt > ldr_rear)
		fwd_rt();				//ldr_rt is brightest, go forward left
		else rotate_rt ();
	}
	Delay10KTCYx (10);
  }		//end of while
}		//end of main

/**************************************************************
Movement Functions
One of these four functions selected every loop iteration
***************************************************************/
/*light is front left, hence move forward left. Algorithm is:
fwd drive left = ldr_fwd - ldr_diff, fwd drive right = ldr_diff + ldr_fwd*/
	void fwd_left (void)	
	{
	fwd_dr_left = ldr_fwd - ldr_diff;
	if (fwd_dr_left < 0) fwd_dr_left = 0; //set to zero if -ve
	fwd_dr_left = fwd_dr_left>>1;         //rotate right to scale down drive value
	if (fwd_dr_left > 127) fwd_dr_left = 127; //limit maximum value
	fwd_dr_rt = ldr_fwd + ldr_diff;
	if (fwd_dr_rt < 0) fwd_dr_rt = 0;  		//set to zero if -ve
	fwd_dr_rt = fwd_dr_rt>>1;         		//rotate right to scale down drive value
	if (fwd_dr_rt >127) fwd_dr_rt = 127;  	//limit maximum value
	CCPR1L = 0x80 + fwd_dr_rt;  	//set right motor, which is greater
	CCPR2L = 0x80 + fwd_dr_left; 	//set left motor, which is less
	}
/*light is front right, hence move forward right. Algorithm is:
fwd drive right = ldr_fwd + ldr_diff, fwd drive left = ldr_fwd - ldr_diff 
(noting "polarity" of ldr_diff*/
	void fwd_rt (void)
	{
	fwd_dr_rt = ldr_fwd + ldr_diff;	
	if (fwd_dr_rt < 0) fwd_dr_rt = 0; 		//set to zero if -ve
	fwd_dr_rt = fwd_dr_rt>>1;         	//rotate right to scale down drive value
	if (fwd_dr_rt > 127) fwd_dr_rt = 127; 	//limit maximum value
	fwd_dr_left = ldr_fwd - ldr_diff;
	if (fwd_dr_left < 0) fwd_dr_left = 0;  	//set to zero if -ve
	fwd_dr_left = fwd_dr_left>>1;         	//rotate right to scale down drive value
	if (fwd_dr_left >127) fwd_dr_left = 127;  //limit maximum value
	CCPR1L = 0x80 + fwd_dr_rt;  	//set right motor, which is less
	CCPR2L = 0x80 + fwd_dr_left; 	//set left motor, which is greater
	}

//fixed speed left rotation (light is at rear left)
	void rotate_left (void)
	{rtmot_fwd ();
	leftmot_rev ();
	}

//fixed speed right rotation (light is at rear right)
	void rotate_rt (void)	
	{leftmot_fwd ();
	rtmot_rev ();
	}
/**************************************************************
Motor Drive Functions
***************************************************************/
	void leftmot_fwd (void)		/*sets left motor running forward*/
	{	CCPR2L = 176;
		PORTAbits.RA5 = 1;		/*enable motor*/
	}

	void rtmot_fwd (void)		/*sets right motor running forward*/
	{	CCPR1L = 176;
		PORTAbits.RA2 = 1;		/*enable motor*/
	}

	void leftmot_rev (void)		/*sets left motor running in reverse*/
	{	CCPR2L = 80;
		PORTAbits.RA5 = 1;		/*enable motor*/
	}

	void rtmot_rev (void)		/*80 sets right motor running in reverse*/
	{	CCPR1L = 80;
		PORTAbits.RA2 = 1;		/*enable motor*/
	}

	void rev_rt (void)		//reverses and then turns to right
{	PORTCbits.RC6 = 1;		//set right led
	PORTAbits.RA5 = 0;		//stop motors
	PORTAbits.RA2 = 0;		
	PORTBbits.RB1 = 1;		//small bleep from sounder
	Delay10KTCYx (50);
	PORTBbits.RB1 = 0;		//clear sounder
	leftmot_rev (); 		//reverse both motors
	rtmot_rev ();
	Delay10KTCYx (200);
	leftmot_fwd (); 		//left motor forward to turn
	Delay10KTCYx (100);
	PORTCbits.RC6 = 0;		//clear led
}

	void rev_left (void)	//reverses and then turns to left
{	PORTCbits.RC5 = 1;		//set left led
	PORTAbits.RA5 = 0;		//stop motors
	PORTAbits.RA2 = 0;		
	PORTBbits.RB1 = 1;		//small bleep from sounder
	Delay10KTCYx (50);
	PORTBbits.RB1 = 0;	
	leftmot_rev (); 		//reverse both motors
	rtmot_rev ();
	Delay10KTCYx (200);
	rtmot_fwd (); 			//right motor forward to turn
	Delay10KTCYx (100);
	PORTCbits.RC5 = 0;		//clear led
}

/*Diagnostic: switches leds on for 1s (Tcy = 1us)*/
void diagnostic (void)
{	Delay10KTCYx (100);
	PORTCbits.RC6 = 1;
	PORTCbits.RC5 = 1;
	Delay10KTCYx (100);
	PORTCbits.RC6 = 0;
	PORTCbits.RC5 = 0;
	Delay10KTCYx (100);
}
