/****************************************************************************
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. 18F2420 mod. applied (App.3). 
Files c018i.o and p18f2420.lib are included by the Linker Script. 

TJW 12.11.05, rev. 31.5.09				Retested 31.5.09
****************************************************************************
Clock is 4MHz. Configuration Word, to be set in MPLAB: crystal oscillator (HS), 
WDT off, power-up timer on, low voltage program disabled, code protect off, 
all others default*/

#include <p18F2420.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 config,unsigned char config2,unsigned char portconfig);
	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 rotate_rt (void);
	void rotate_left (void);
	void diagnostic (void);
	void fwd_to_light (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 = 0b00001111;	//4 ADC channels set as inputs, tho ch 2 is unused
							//bit 5 is motor enable
	TRISB = 0b00110000; 	//bits 4 & 5 are uswitch inputs, bit 2 rt motor enable
	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. oscillator divided by 4, right justify result, ac time is 2TAD,
//interrupt off, internal reference, Port A bits 0-3 are analog input
	OpenADC(ADC_FOSC_4&ADC_RIGHT_JUST&ADC_2_TAD,ADC_CH0&ADC_INT_OFF&ADC_VREFPLUS_VDD&ADC_VREFMINUS_VSS,11); 
//Switch all outputs off
	PORTA = PORTB = PORTC = 0;
//call diagnostic function	
	diagnostic();
//enable motors at idle speed
	CCPR1L = CCPR2L = 0x80; 
	PORTAbits.RA5 = 1;	
	PORTBbits.RB2 = 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);	
	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);	
	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);	
	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_to_light();			//ldr_left is brightest, go forward left
     else rotate_left ();		//rear is brightest, rotate towards light
	}
	else 
	{if (ldr_rt > ldr_rear)
	fwd_to_light();			//ldr_rt is brightest, go forward right
	else rotate_rt ();		//rear is brightest, rotate towards light
	}
	Delay10KTCYx (10);
  }					//end of while
}					//end of main

/**************************************************************
Movement Functions
One of these functions selected every loop iteration
***************************************************************/
/*light is somewhere in front, hence move towards it. 
A high reading on the right LDR leads to a strong drive to left 
motor, and vice versa. Algorithm is:
fwd drive left = ldr_fwd - ldr_diff, fwd drive right = ldr_fwd + ldr_diff*/	
void fwd_to_light (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
	CCPR2L = 0x80 + fwd_dr_left; 		//set left motor
	}

//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 = 188;
		PORTAbits.RA5 = 1;		/*enable motor*/
	}

	void rtmot_fwd (void)		/*sets right motor running forward*/
	{	CCPR1L = 188;
		PORTBbits.RB2 = 1;		/*enable motor*/
	}

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

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

	void rev_rt (void)		//reverses and then turns to right
{	PORTCbits.RC6 = 1;		//set right led
	PORTAbits.RA5 = 0;		//stop motors
	PORTBbits.RB2 = 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
	PORTBbits.RB2 = 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: flashes leds in half second bursts (Tcy = 1us)*/
void diagnostic (void)
{	PORTCbits.RC6 = 1;
	Delay10KTCYx (50);
	PORTCbits.RC6 = 0;
	PORTCbits.RC5 = 1;
	Delay10KTCYx (50);
	PORTCbits.RC6 = 1;
	PORTCbits.RC5 = 1;
	Delay10KTCYx (50);
	PORTCbits.RC6 = 0;
	PORTCbits.RC5 = 0;
}