#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()
{
	int motor1, motor2;

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

	
	TXSTA = 0b00100100;
	RCSTA = 0b10010000;
	SPBRG = 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);

	// Just to save the status
	motor1 = FOWARD;
	motor2 = FOWARD;

	// 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)
	    {
		if (motor1 == FOWARD)
		{
		    motor(MOTOR1, REVERSE, HALFSPEED);
		    motor1 = REVERSE;
		}
		else
		{
		    motor(MOTOR1, FOWARD, HALFSPEED);
		    motor1 = FOWARD;
		}
		RB4 = 1;
	    }
	    if (ra2 == 1)
	    {
		motor(MOTOR2, FOWARD, 0);
		RB5 = 0;
	    }
	    if (ra3 == 1)
	    {
		if (motor2 == FOWARD)
		{
		    motor(MOTOR2, REVERSE, HALFSPEED);
		    motor2 = REVERSE;
		}
		else
		{
		    motor(MOTOR2, FOWARD, HALFSPEED);
		    motor2 = FOWARD;
		}
		RB5 = 1;
	    }
	    DelayMs(200);
	}
	
}


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
}