#include	<pic.h>

__CONFIG(WDTDIS & XT & UNPROTECT & LVPDIS);

#define PORTBIT(adr, bit) ((unsigned)(&adr)*8+(bit))

static bit ra0 @ PORTBIT(PORTA, 0);
static bit ra1 @ PORTBIT(PORTA, 1);
static bit ra2 @ PORTBIT(PORTA, 2);
static bit ra3 @ PORTBIT(PORTA, 3);


#ifndef	XTAL_FREQ
#define	XTAL_FREQ	4MHZ		/* Crystal frequency in MHz */
#endif

#define	MHZ	*1000L			/* number of kHz in a MHz */
#define	KHZ	*1			/* number of kHz in a kHz */

#if	XTAL_FREQ >= 12MHZ
	#define	DelayUs(x)	{ unsigned char _dcnt; \
				  _dcnt = (x)*((XTAL_FREQ)/(12MHZ)); \
				  while(--_dcnt != 0) \
				  continue; }
#else
	#define	DelayUs(x)	{ unsigned char _dcnt; \
				  _dcnt = (x)/((12MHZ)/(XTAL_FREQ))|1; \
				  while(--_dcnt != 0) \
					  continue; }
#endif

void DelayMs(unsigned char cnt);

#define DELAY	(200)

#define MOTOR1    (1)
#define MOTOR2    (2)
#define FOWARD    (1)
#define REVERSE   (2)
#define HALFSPEED (127/2)
#define FULLSPEED (127)


void motor(int motor, int direction, int speed)
{
    int bits = 0;

    while(TXIF == 0);

    TXREG = 0x80;

    while(TXIF == 0);

    TXREG = 0x00;

    while(TXIF == 0);

    if (motor == MOTOR2)	// set to 1 the second bit
	bits += 2;

    if (direction == FOWARD)	// set to 1 the first bit
	bits += 1;

    TXREG = bits;

    while(TXIF == 0);

    TXREG = speed;
}

void main()
{

	TRISB  = 0b11000000;
	CMCON  = 0b00000111;	// disable comparator on ra0,1,2
	TRISA  = 0b11111111;
	

	
	//*****************************************************************
	// Following the datasheet in the section of
	// UNIVERSAL SYNCHRONOUS/ASYNCHRONOUS RECEIVER/TRANSMITTER (USART)
	//
	// these lines set the TXSTA register
	//
	CSRC = 0;	// in async. mode don't used
	TX9  = 0;	// 0: set 8 bit transmission
	TXEN = 1;	// 1: transmit enable
	SYNC = 0;	// 0: asynchronous mode
	BRGH = 1;	// 1: async. high speed
	TRMT = 0;	// 0: transmit shift register full
	TX9D = 0;	// 9th bit of transmit data
	//
	// Is also possible set all the bits of TXSTA control register
	// with a unique command like the following:
	// TXSTA = 0b00100100;

	//*****************************************************************
	// these lines set the RCSTA register
	//
	SPEN = 1;	// 1: serial port enable
	RX9  = 0;	// 0: set 8 bit reception
	SREN = 0;	// in async. mode don't used
	CREN = 1;	// 1: enable continuous receive
	ADEN = 0;	// in 8 bit mode don't used
	FERR = 0;	// framing error bit ? 
	OERR = 0;	// 0: no overrum error
	RX9D = 0;	// 9th bit of received data
	//
	// Is also possible set all the bits of RCSTA control register
	// with a unique command like the following:
	// RCSTA = 0b10010000;

	//*****************************************************************
	// these lines set the UART Baud Rate Generator
	//
	SPBRG = 51;
	//
	// with the calculation formula is not very easy to find this value
	// but the tables in the datasheet show the decimal value to set for
	// many baud rate and many clock frequency
	// If we use want a baud rate of 19200 (19.2 K) and the cpu quartz is
	// 4 MHz the value is 51.


	// Now reset the Motor controller with these following command because I have
	// connected the reset pin of the controller to the RB3 pin
	RB3 = 0;
	DelayMs(200);
	RB3 = 1;
	DelayMs(200);

	// This function will be usefull to control the 2 motors 
	motor(MOTOR1, FOWARD, HALFSPEED);
	motor(MOTOR2, FOWARD, HALFSPEED);

	// switch-on the 2 led
	RB4 = 1;
	RB5 = 1;

	while(1)
	{
	    // Will be easy to control the motors with the four buttons

	    if (ra0 == 1)
	    {
		motor(MOTOR1, FOWARD, 0);
		RB4 = 0;
	    }
	    if (ra1 == 1)
	    {
		motor(MOTOR1, FOWARD, HALFSPEED);
		RB4 = 1;
	    }
	    if (ra2 == 1)
	    {
		motor(MOTOR2, FOWARD, 0);
		RB5 = 0;
	    }
	    if (ra3 == 1)
	    {
		motor(MOTOR2, FOWARD, HALFSPEED);
		RB5 = 1;
	    }
	}
	
}


void DelayMs(unsigned char cnt)
{
#if	XTAL_FREQ <= 2MHZ
	do {
		DelayUs(996);
	} while(--cnt);
#endif

#if    XTAL_FREQ > 2MHZ	
	unsigned char	i;
	do {
		i = 4;
		do {
			DelayUs(DELAY);

			if (ra0 == 1 ||
			    ra2 == 1 ||
			    ra2 == 1 ||
			    ra3 == 1)
				return;
		} while(--i);
	} while(--cnt);
#endif
}