/****************************************************************
Dbt_blind_Nav_18C
Derbot moves by “blind” navigation. 
Moves forward, and reverses and turns on bump. 
Files c018i.o and p18f2420.lib are included by the Linker Script.
Fixed rate PWM applied to set reasonable speeds.
18F2420 mod. applied (App.3)
TJW 22.10.05, rev. 24.5.09	  		retested 24.5.09 
*****************************************************************
Clock is 4MHz
Configuration Word all default, except: crystal oscillator (HS), 
power-up timer on,brown-out detect off, WDT off, LV Program disabled*/

#include <p18F2420.h>
#include <delays.h>		//header file for delays
#include <timers.h>		//header file for Timers
#include <pwm.h>		//header file for PWM

/*function prototypes, reproduced from Header Files for information
	void OpenPWM1 (char);
	void OpenPWM2 (char);
	void OpenTimer2 (unsigned char);
	void Delay10KTCYx (unsigned char); */

//User-defined function prototypes
	void diagnostic (void);
	void leftmot_fwd (void);
	void rtmot_fwd (void);
	void rev_left (void);
	void rev_rt (void);

void main (void)
{
/*Initialises SFRs, and sets initial outputs. Assumes hardware is "Build Stage 2",
with 18F2420 mod. applied.All unused port bits set to output.*/
	TRISA = 0b00000000;	//All bits output, 5 is L motor enable. 
	TRISB = 0b00110000;	//Bits 5,4 are microswitches, 2 is R motor enable.
	TRISC = 0b10000000;	//All bits output except 7 (mode switch), 
							//1 & 2 used for PWM
	ADCON1 = 0b00001111; //Set Port A for digital i/o

//Switch all outputs off
	PORTA = 0;
	PORTB = 0;
	PORTC = 0;
//call diagnostic function	
	diagnostic();	
//Enable PWM
	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

	while (1) 
	{
	//start motors
	leftmot_fwd ();
	rtmot_fwd ();
	
//test for bumps - reverse and turn if either microswitch closes
	if (PORTBbits.RB4 == 0)  //Test right uswitch
	rev_left ();
	if (PORTBbits.RB5 == 0)  //Test left uswitch
	rev_rt ();
	Delay10KTCYx (10);
	}
}

/**************************************************************
Motor Drive Functions
***************************************************************/
	void leftmot_fwd (void)	//sets left motor running forward
{	CCPR2L = 196;
	PORTAbits.RA5 = 1;	//enable motor
}

	void rtmot_fwd (void)	//sets right motor running forward
{	CCPR1L = 196;
	PORTBbits.RB2 = 1;	//enable motor. Use PORTAbits.RA2 if circuit board not modified
}

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

	void rtmot_rev (void)	//sets right motor running in reverse
{	CCPR1L = 60;
	PORTBbits.RB2 = 1;	//enable motor. Use PORTAbits.RA2 if circuit board not modified
}
	
	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: switches leds on for 1s (Tcy = 1us)
void diagnostic (void)
{	PORTCbits.RC6 = 1;
	PORTCbits.RC5 = 1;
	Delay10KTCYx (100);
	PORTCbits.RC6 = 0;
	PORTCbits.RC5 = 0;
	Delay10KTCYx (100);
}