// Robot Module
// (c) 1999 Erik Hougaard & Lau Nørgaard

// PIC Layout

// PORTA Communication / Service Port

// BIT 0 - Activity LED 
// BIT 1 - RS232 / RxData
// BIT 2 - RS232 / TxData
// BIT 3 - I2C Bus Clock
// BIT 4 - I2C Bus Data

// PORTB Feature Port

// ANSI C Includes
#include <stdio.h>
#include <string.h>
#include <pic.h>

// PIC Includes
#include "delay.h"

// Robot Includes

#define I2CADDR                     80  // I2C Address for this feature
#define I2Cbuffersize                6  // Size of I2C Buffer
#define XTAL                  10000000  // PIC Speed
#define BRATE                    76800

#include "util.c"
// Function list in "util.c"
// I2C (I2C Communication handler)
// I2Cbuffer[] (Data segment for I2C)
// putch (RS232)
// getch (RS232)

static bit  Motor1OnOff     @ (unsigned)&PORTB*8+2;
static bit  Motor2OnOff     @ (unsigned)&PORTB*8+5;
static bit  Motor1Brake     @ (unsigned)&PORTB*8+1;
static bit  Motor2Brake     @ (unsigned)&PORTB*8+4;
static bit  Motor1Direction @ (unsigned)&PORTB*8+0;   
static bit  Motor2Direction @ (unsigned)&PORTB*8+3;   

unsigned char MotorTrigger1;
unsigned char MotorTrigger2;

unsigned int  Puls1;
unsigned int  Puls2;

void UpdateDirection()
{
  Motor1Direction = I2Cbuffer[0];
  Motor2Direction = I2Cbuffer[3];
  memcpy(&Puls1,&I2Cbuffer[1],sizeof(int));
  memcpy(&Puls2,&I2Cbuffer[4],sizeof(int));
}

void PWMbit1()
{
	if (Puls1 & 0b0000000000000001)
	{
		Motor1OnOff = 1;
		Puls1 = Puls1 >> 1;
		Puls1 |= 32768;
	}
	else
	{
		Motor1OnOff = 0;
		Puls1 = Puls1 >> 1;
	}
}

void PWMbit2()
{
	if (Puls2 & 0b0000000000000001)
	{
		Motor2OnOff = 1;
		Puls2 = Puls2 >> 1;
		Puls2 |= 32768;
	}
	else
	{
		Motor2OnOff = 0;
		Puls2 = Puls2 >> 1;
	}
}

void main()
{
	unsigned char i;
	unsigned char i2;
	init(); // Initialization of PORTA and I2C Buffer
	PORTB = 0;
	TRISB = 0x00;
  Motor1Brake = 0; // Support for brake not included
  Motor2Brake = 0;

	for(;;)
	{
		if (CheckIO())
		{
		  UpdateDirection();
		}
		PWMbit1();
		if (CheckIO())
		{
		  UpdateDirection();
		}
		PWMbit2();
		i++;
		if (i == 0)
		{
			i2++;
			if (i2 == 0)
    		LED = !LED;
		}
	}
}