// Robot Module (Sharp GP2D02 Interface)
// (c) 1999 Erik Hougaard & Lau Noergaard

// 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 <pic.h>

// PIC Includes
#include "delay.h"

// Robot Includes

#define I2CADDR                     20  // I2C Address for this feature
#define I2Cbuffersize                5  // Size of I2C Buffer
#define XTAL                  10000000  // PIC Speed
#define BRATE                    76800  // Serial Port Speed

#include "util.c"

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

static bit  Vin1           @ (unsigned)&PORTB*8+1;
static bit  Vout1          @ (unsigned)&PORTB*8+0;
static bit  Vin2           @ (unsigned)&PORTB*8+3;
static bit  Vout2          @ (unsigned)&PORTB*8+2;
static bit  Vin3           @ (unsigned)&PORTB*8+5;
static bit  Vout3          @ (unsigned)&PORTB*8+4;
static bit  Vin4           @ (unsigned)&PORTB*8+7;
static bit  Vout4          @ (unsigned)&PORTB*8+6;
static bit  Vin5           @ (unsigned)&PORTA*8+2;
static bit  Vout5          @ (unsigned)&PORTA*8+1;


void Led1()
{
	unsigned char d;
	int x = 0;

  Vin1 = 0;
  while (Vout1);
  while (!Vout1)
    CheckIO();
  for (d = 0; d < 8; d++)
  {
    x = x << 1;
    Vin1 = 1;
    DelayUs(2);
    Vin1 = 0;
    DelayUs(2);
    if (Vout1 == 1)
      x++;
  }
	Vin1 = 1;
	I2Cbuffer[0] = x;
}

void Led2()
{
	unsigned char d;
	int x = 0;

  Vin2 = 0;
  while (Vout2);
  while (!Vout2)
    CheckIO();
  for (d = 0; d < 8; d++)
  {
    x = x << 1;
    Vin2 = 1;
    DelayUs(2);
    Vin2 = 0;
    DelayUs(2);
    if (Vout2 == 1)
      x++;
  }
	Vin2 = 1;
	I2Cbuffer[1] = x;
}


void Led3()
{
	unsigned char d;
	unsigned char x = 0;

  Vin3 = 0;
  while (Vout3);
  while (!Vout3)
    CheckIO();
  for (d = 0; d < 8; d++)
  {
    x = x << 1;
    Vin3 = 1;
    DelayUs(2);
    Vin3 = 0;
    DelayUs(2);
    if (Vout3 == 1)
      x++;
  }
	Vin3 = 1;
	I2Cbuffer[2] = x;
}

void Led4()
{
	unsigned char d;
	unsigned char x = 0;

  Vin4 = 0;
  while (Vout4);
  while (!Vout4)
    CheckIO();
  for (d = 0; d < 8; d++)
  {
    x = x << 1;
    Vin4 = 1;
    DelayUs(2);
    Vin4 = 0;
    DelayUs(2);
    if (Vout4 == 1)
      x++;
  }
	Vin4 = 1;
	I2Cbuffer[3] = x;
}

void Led5()
{
	unsigned char d;
	unsigned char x = 0;

  Vin5 = 0;
  while (Vout5);
  while (!Vout5)
    CheckIO();
  for (d = 0; d < 8; d++)
  {
    x = x << 1;
    Vin5 = 1;
    DelayUs(2);
    Vin5 = 0;
    DelayUs(2);
    if (Vout5 == 1)
      x++;
  }
	Vin5 = 1;
	I2Cbuffer[4] = x;
}


void main()
{
	// int i = 0;
	init(); // Initialization of PORTA and I2C Buffer

  TRISB = 0b01010101;
  // TRISA = 0b00011010; // Overwrite the init value 
  Vin1 = 1;
  Vin2 = 1;
  Vin3 = 1;
  Vin4 = 1;
	
	for(;;)
	{
		CheckIO();
		if (Active)
		{
  		Led1();
	  	Led2();
		  Led3();
  		Led4();
  		//Led5();

		//i++;
	  //if (i == 0)
		}
 	  LED = !LED;
	}
}