/*******************************************************************************

	AlphaBot

		AlphaBot.c

*******************************************************************************/

#include <stdlib.h>
#include "AlphaBot.h"

#define	RC_LEFT	P2_2
//------------------------------------------------------------------------------

void	Initialize(void)
{
	HeadStop();
	cSpeed[LEFT] = cSpeed[RIGHT] = 0;
	ulMileage[LEFT] = ulMileage[RIGHT] = 0;;
	ulSpeed_PrevTick[LEFT] = ulSpeed_PrevTick[RIGHT] = ulSpeed_CurTick[LEFT] = ulSpeed_CurTick[RIGHT] = 0;
	uiSpeed_PrevCount[LEFT] = uiSpeed_PrevCount[RIGHT] = uiSpeed_CurCount[LEFT] = uiSpeed_CurCount[RIGHT] = 0;
	uiSpeed_StalledSheelCounter[LEFT] = uiSpeed_StalledSheelCounter[RIGHT] = 0;

	TMOD = 0x12;	//-- initalize	Timer 0 in Mode 2 (8-bit auto-reload)
			//--	and 	Timer 1 in Mode 1 (16-bit)
	EA = 1;		// Enable interrupts

	// ***** Initiallize PWM (uses PCA Modules0-3 and Timer0) *****

		//-- Set timer0
//	TMOD = 0x02;	//-- initalize Timer0 in Mode 2 (8-bit auto-reload) (-already done)
	TH0 = 128;	//-- Set reload value / frequency (1-256)
	TL0 = 0x00;	//-- Set start value
	TR0  = 1;	//-- Start timer0
		//-- Set PCA
//	CMOD = 0x00;	//-- Initialize PCA to use 1/6 osc frq
	CMOD = 0x02;	//-- Initialize PCA to use 1/2 osc frq
//	CMOD = 0x04;	//-- Initialize PCA to use Timer0 overload as input frequency
	CL = 0x00;
	CH = 0x00;
		//-- Initialize Module0 (left drive - CW)
	CCAPM0 = 0x42;	//-- Module 0 in PWM mode
	CCAP0L = 0x00;
	CCAP0H = 0;	//- Initial speed = 0 [0...255]
		//-- Initialize Module1 (left drive - CCW)
	CCAPM1 = 0x42;	//-- Module 1 in PWM mode
	CCAP1L = 0x00;
	CCAP1H = 0;	//- Initial speed = 0 [0...255]
		//-- Initialize Module2 (righr drive - CW)
	CCAPM2 = 0x42;	//-- Module 2 in PWM mode
	CCAP2L = 0x00;
	CCAP2H = 0;	//- Initial speed = 0 [0...255]
		//-- Initialize Module3 (right drive - CCW)
	CCAPM3 = 0x42;	//-- Module 3 in PWM mode
	CCAP3L = 0x00;
	CCAP3H = 0;	//- Initial speed = 0 [0...255]
		//-- Start PCA counter
	CR = 1;

	// ***** Set Clock ***** (uses Timer1 and Interrupt3 )
/*
	Setup Timer 1 to interrupt every 10 Msecs 

	For Philips chip @ 11.0592 MHz, one machine cycle == 0.5425 usec
		36866 x 0.5425 usec = 1,9999 usec ( 20 msec )
		65,536 - 36,866 = 28,670 =>	0x6FFE	- 20ms
						0xB7FE	- 10ms
						0x????	- 10ms anjusted
*/
	TIMER_RELOAD;

//	TMOD = 0x10;	// Set Timer1 to 16-bit mode (-already done)
   	PT1  = 1;	// select high priority for TIMER1 interrupt
   	ET1  = 1;	// enable Timer 1 interrupts (after EA is set to 1 )
	TR1  = 1;	// Start Timer1


	// ***** Set Sound ***** (uses Timer2)
/*
	Program P1.0 (T2) as frequency generator in Clock-Out mode

	Output a 50% duty cycle clock:
		61 Hz - 4 MHz (12-clock mode) or 122 Hz - 8 MHz (6-clock mode)
		(at a 16 MHz operating frequency)

				Oscillator Frequency
	Clock-Out frequency = ------------------------------
				n * (65536 - RCAP2H,RCAP2L)

	where:	n =	2 in 6-clock mode
			4 in 12-clock mode
		(RCAP2H,RCAP2L) = 	the content of RCAP2H and RCAP2L
					taken as a 16-bit unsigned integer.

	I.e. to generate 1KHz @ 11.0592 MHz & 6-clock:
		RCAP2H,RCAP2L	= 65536 - (5529600/Hz)
				= 65536 - (5529600/1000) = 60007 = EA67h,
		or
					Oscillator Frequency / n
		RCAP2H,RCAP2L = 65536 - ------------------------ =
						F
			      = 65536 - 1843200 / F
*/
	CT2 = 0;		//-- Setup Timer2 in FreqGen mode
	T2MOD = 0x02;

	RCAP2H = 0xEA; 		//-- Specify default/initial frequency (1 KHz)
	RCAP2L = 0x67;

//	TR2 = 1;		// Start timer - when needed


	// ***** Set External Interrupts (wheel speed meters) *****
		//-- Left wheel
	IT0 = 1;	// set falling edge interrupt
	EX0 = 1;	// enable external INT0
		//-- Right wheel
	IT1 = 1;	// set falling edge interrupt
	EX1 = 1;	// enable external INT1

} // Initialize
//-----------------------------------------------------------------------------------------

void	main(void)
{
	Initialize();	//-- before doing anything

	Show(S_START);

	Diagnostic(TRUE);	//-- Start-up self-diagnostic

	srand(ulTickCount);	// after (varilable time of) diagnostic

	FOREVER		//-- Main Loop
	{
		//-- AI Behavior - read sensors, make desicion, set tasks
/*		if( IS_DARK )
			LED_HEART=LED_ON;
		else
			LED_HEART=LED_OFF;
*/
		Behavior();


		//-- Propcess routine taksks

		Drive(); 	//-- (has built-in over-do check)

		Diagnostic(FALSE);	//-- Quick Diagnostics task


		HeartBeat();		//-- Heart Beat LED

	} // end of Main Loop

} // main
//------------------------------------------------------------------------------