/****************************************************************************
Light_seek_&_disp
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.
Indicative value of light reading on each LDR is displayed. 
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 16.6.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 <i2c.h>
	#include <delays.h>	
	#include <stdlib.h>			//for itoa function
	#include <string.h>			//for strlen function
	#define slave_addr1 0xA4 	//Adress of Derbot Hand Controller I2C node.	

//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_to_light (void);
	void rotate_rt (void);
	void rotate_left (void);
	void diagnostic (void);
	void disp_int (int op_int);		//Converts an integer to string, and sends to lcd via I2C 
	void send_space (char space_no);	//Sends space to lcd via I2C

//Declare Variables
	char loop_cntr;		//counts iterations of super-loop
	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
	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 = 0b10011000; 	//Port C bits. I2C bits are both set as ip
	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 (same as for PWM1)
	loop_cntr = 0;

//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
//60Switch all outputs off
	PORTA = PORTB = PORTC = 0;
//enable motors at idle speed
	CCPR1L = CCPR2L = 0x80; 
	PORTAbits.RA5 = 1;	
	PORTBbits.RB2 = 1;
//Initialise I2C
	OpenI2C (MASTER, SLEW_OFF);
	SSPADD = 0x07; 		//set up 125kHz baud rate
	diagnostic();				//call diagnostic function
/**************************************************
Program from here is structured as "super-loop" 
**************************************************/
while (1)
  {
	loop_cntr++;
//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

//Display ldr values every 10 loops
	if (loop_cntr == 10)
	{
	disp_int (ldr_left);		//display left ldr value on lcd
	disp_int (ldr_rt);		//display right ldr value on lcd
	send_space (2);			//centre rear display
	disp_int (ldr_rear);		//display rear ldr value on lcd
	send_space (2);			//fills second line, forcing line feed
	loop_cntr = 0;
	}

//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 left
		else rotate_rt ();
	}
	Delay10KTCYx (5);		//50ms delay (4MHz clock)		
  }				//end of superloop while
}				//end of main

/*****************************************************
Display Functions
******************************************************/
//Converts an integer to string, and sends to lcd via I2C, filling with spaces up to 4 digits 
	void disp_int (int op_int)
{	
	char disp_val[5];	 	//will hold the string representation of any value
	char *disp_val_ptr; 	//pointer to disp_val[]
	char space_no;			//number of spaces to be inserted
	disp_val_ptr = &disp_val[0];
	itoa (op_int, disp_val_ptr);//first convert to a BCD string
	space_no = 4 - strlen(disp_val_ptr); //find how many spaces needed
//Now send the message
	StartI2C();			   		//send start condition
	WriteI2C (slave_addr1); 	//send address word	
	while (space_no)			//fill up with leading spaces
	{WriteI2C (' ');
	Delay1KTCYx(5);				//delay needed for hand controller
	space_no--;
	}
//send the string
	while (*disp_val_ptr)
	{
	WriteI2C (*disp_val_ptr);
	disp_val_ptr++;
	Delay1KTCYx(5);				//delay needed for hand controller
	}	
	StopI2C();				//send stop condition
}

//Sends space to lcd via I2C
	void send_space (char space_no)
{	
	StartI2C();			   	//send start condition
	WriteI2C (slave_addr1); 	//send address word, function waits until write is complete
//send space
	while (space_no)
	{
	WriteI2C (' ');
	Delay1KTCYx(5);				//delay needed for hand controller
	space_no--;
 	}
	StopI2C();					//send stop condition
}

/**************************************************************
Movement Functions
One of these four 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 = 192;
		PORTAbits.RA5 = 1;		/*enable motor*/
	}

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

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

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

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

	void rev_left (void)	/*reverses and then turns to left*/
	{PORTCbits.RC5 = 1;		//set led
	PORTAbits.RA5 = 0;		/*stop motors*/
	PORTBbits.RB2 = 0;		
	PORTBbits.RB1 = 1;		//small bleep from sounder
	Delay10KTCYx (100);
	PORTBbits.RB1 = 0;	
	leftmot_rev (); 		//reverse both motors
	rtmot_rev ();
	Delay10KTCYx (200);
	rtmot_fwd (); 			//right motor forward to turn
	Delay10KTCYx (200);
	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);
}