// Working 11/05/2007 @ 15:32 PM
// by Phil Winder || www.philwinder.com

// DESCRIPTION: **************************************************
// This is the first ZBot program that acts like a robot in that
// it responds to external stimuli (IR sensors) and acts 
// accordingly (turns the robot).  At the current time it uses 2
// IR pairs, but this is insufficient for correct operation.
// It uses 2 other files, namely the IR and Motor files which
// hold the functions for operating the IR pairs and the drive, 
// respectively.
// The speed of the bot can be changed by altering the NormalSpeed
// define but in the future it would be nessecary to implement a
// more sophisticated system.
// The PIC runs off the internal clock and has no power 
// optimisations as of yet.
// The program structure is in the form of a state machine which
// the diagram can be seen on my website.
// NOTES: ********************************************************
// # The two IR's are insufficent for operation and the bot seems
// 	 to hit walls dependant on where the IR's are placed.  In the
//	 future it will be nessecary to have at least 3 IR's (left, 
//	 front, right) for a more accurate representation of the
//	 obsticles
// # The motors sound crap due to the internal cogs whirring
//	 and there is not much torque so the bot gets stuck quite 
//	 often.
// # Hence another drawback is the inability to move backwards.
//	 In the future two motor drivers will be used.
// # Although, the primary task is to get the IR circuit on some
//	 sort of stripboard/PCB, but due to not having the components
//	 I will  have to order them in.
// # Finally I will not have a lot of time in the next 4 weeks 
//	 due to exams so I estimate that I can re-start this work on
//	 approximatly 6/06/07 to get the parts for 11/06/07.  But
//	 by then I will be back to work so I may be able to scrounge
//	 a few parts from there.
// VERSION INFORMATION: ******************************************
// V1.0 @ 11/05/07 - First version. Code taken from inital test program
// ***************************************************************

#include <p18f2520.h>
#include <delays.h>
#include <stdlib.h>
#include "motor.c"
#include "IR.c"

// Pre-Processor Directives
#pragma config OSC = INTIO67	// Internal RC with IO
#pragma config FCMEN = OFF 
#pragma config IESO = OFF
#pragma config PWRT = OFF
#pragma config BOREN = OFF 
#pragma config WDT = OFF 
#pragma config MCLRE = ON
#pragma config LPT1OSC = OFF
#pragma config PBADEN = OFF
#pragma config STVREN = OFF 
#pragma config LVP = OFF
#pragma config XINST = OFF 

// Defines
#define NormalSpeed 60

// Function Definitions


// Setup Routines
void Setup_Osc(void)
	{
	OSCCONbits.SCS1 = 0;	// 00 = Normal Clock source
	OSCCONbits.SCS0 = 0;
	OSCCONbits.IRCF0 = 1;	// 111 = 8MHz
	OSCCONbits.IRCF1 = 1;
	OSCCONbits.IRCF2 = 1;
	OSCCONbits.IDLEN = 0;	// Device enters Sleep on SLEEP
	}

void Setup_Ports(void)
	{
	 // A/D converter module disabled
	 ADCON0 = 0xFC;
	 // All pins as Digital I/O
	 ADCON1 = 0xFF;
	
	 // PORTA 0..7 Enabled as Output
	 TRISA = 0x00;
	 PORTA = 0x00;
	 LATA =  0x00;

	 // PORTB 2..7 Enabled as Output, 0,1 as input
	 TRISB = 0x03;
	 PORTB = 0x00;
	 LATB =  0x00;

	 // PORTC RC2 = PWM1, RC3 = PWM2. No direction since on one driver
	 TRISC = 0b11111001;
	 PORTC = 0x00;
	 LATC =  0x00;
	}

// Main Program
void main(void)
	{
	unsigned char StateVar = 0;
	unsigned int MoveCount = 0, TurnCount = 0;
	unsigned char Random;

	while (1)
		{
		Delay1KTCYx(40);		// Represents a Sleep/Idle Function
		switch (StateVar)
			{
			// -= Initial setup =-
			// Only called once at the start of the program, or if something goes wrong
			case 0:
				Setup_Osc(); 	// Setup the Oscillator for 8MHz
				Setup_Ports();	// Setup Ports
				
				Init_PWM();		// Initalise the PWM module.
			
				Stop();			// Make sure the motors are stopped
				StateVar = 1;	// Firs state.
				break;
			// -= Start State =- (For future use)
			case 1:
				StateVar = 2;
			// -= Proximity Detection =-
			case 2:
				switch (Proximity_Detect())	// Remember that the TSOP is active low
					{
					case 0:	// Both IR's Detected
						LATA = 3;	
						StateVar = 23;
						break;
					case 1:	// Right IR detected
						LATA = 2;	
						StateVar = 21;
						break;
					case 2:	// Left IR Detected
						LATA = 1;	
						StateVar = 22;
						break;
					case 3:	// No IR's Detected
						LATA = 0;
						StateVar = 3;
						break;
					default:	// Other Cock up.
						LATA = 0;
						Stop();
						Delay10KTCYx(200);
						break;	
					}
				break;	
			// -= Move forward =-
			// If no IR detect then move forward.	
			case 3:
				Move_Forward(NormalSpeed);
				StateVar = 4;
			// -= Anti-Stuck Counter 1 =-
			case 4:
				MoveCount++;
				TurnCount = 0;
				StateVar = 6;
				break;
			// -= Anti-Stuck Counter 2 =-
			case 5:
				MoveCount = 0;
				TurnCount++;
				StateVar = 6;
			// -= Anti-Stuck State =-
			case 6:
				if ( (MoveCount > 1000) || (TurnCount > 1000) )
					{
					Stop();
					while(1)
						{
						LATA = 1;
						Delay10KTCYx(200);
						LATA = 2;
						Delay10KTCYx(200);
						}
					}
				else
					{
					StateVar = 1;
					}
				break;	
			// -= Move Left =-
			// If Right IR detect
			case 21:
				Move_Left(30);
				StateVar = 5;
				break;	
			// -= Move Right =-
			// If Left IR detect
			case 22:
				Move_Right(30);
				StateVar = 5;
				break;		
			// -= Move Back =-
			// If center IR detect (Future)
			case 23:
				Move_Right(30);	
				StateVar = 5;
				break;
			}

		}
	}