/****************************************************************
Dbt_blind_Nav_PWM_C
Derbot moves by “blind” navigation. 
Moves forward, and reverses and turns on bump. 
Files c018i.o and p18f242.lib are included by the Linker Script.
Fixed rate PWM applied to set reasonable speeds.

TJW 3.11.05								Tested 7.11.05
*****************************************************************
Clock is 4MHz
Configuration Word: WDT off, power-up timer on, 
			   code protect off, crystal oscillator (HS)*/

#include <p18F242.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". 
All unused port bits set to output. Used bits are identified.*/
	TRISA = 0b00000000;	//All bits output, 2 & 5 used for motor enables. 
	TRISB = 0b00110000;	//Bits 5 and 4 (microswitches) only are input, 
	TRISC = 0b10000000;	//All bits output except 7 (mode switch), 
							//1 & 2 used for PWM
	ADCON1 = 0b00000110; //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;
	PORTAbits.RA2 = 1;	//enable motor
}

	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;
	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)
{	PORTCbits.RC6 = 1;
	PORTCbits.RC5 = 1;
	Delay10KTCYx (100);
	PORTCbits.RC6 = 0;
	PORTCbits.RC5 = 0;
	Delay10KTCYx (100);
}
