/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *\

 *                                                                        *

 * This file is the source for the CMUcam v1.11 firmware, it is           *

 * the code that operates on the CMUcam Vision Board.                     *

 * Contact cmucam@cs.cmu.edu, or see                                      *

 * http://www.cs.cmu.edu/~cmucam/ for more information.                   *

 *                                                                        *

 * Copyright 2001 Charles Rosenberg, Anthony Rowe,                        *

 *                and Carnegie Mellon University. All Rights Reserved.    *

 *                                                                        *

 * This software is made available for academic and research purposes     *

 * only. No commercial license is hereby granted. Copying and other       *

 * reproduction is authorized only for research, education and other      *

 * non-commercial purposes. No warranties, either expressed or implied,   *

 * are made regarding the operation, use or results of the software.      *

 *                                                                        *

 * Redistribution and use in source and binary forms for research,        *

 * education and other non-commercial purposes, with or without           *

 * modification, are permitted provided that the following conditions     *

 * are met:                                                               *

 *                                                                        *

 *  1. Redistributions of source code must retain the above copyright     *

 *  notice, this list of conditions and the preceding disclaimer.         *

 *                                                                        *

 *  2. Redistributions in binary form must reproduce the above            *

 *  copyright notice, this list of conditions and the preceding           *

 *  disclaimer in the documentation and/or other materials provided       *

 *  with the distribution.                                                *

 *                                                                        *

 *  3. The name of the author(s) may not be used to endorse or promote    *

 *  products derived from this software without specific prior written    *

 *  permission.                                                           *

 *                                                                        *

\* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */



#pragma ID ="CMUcam";

#include </dev/sx28ac.h>

#include <port.h>



#define UART_PORT PORTB		// this port shared with I2C

#define UART_RX   2

#define UART_TX   3

#define AUTO_START   4

#define BAUD_SELECT  5

#define SERVO_LINE   6

#define SLAVE_SELECT 7



#define I2C_PORT PORTB		// this port shared with the UART

#define I2C_PORT_DDR_IDLE	IOIIOIII

#define I2C_PORT_DDR_READ_SDA	IOIIOIOI

#define I2C_PORT_DDR_READ_SCL	IOIIOIIO

#define I2C_PORT_DDR_WRITE	IOIIOIOO

#define I2C_SCL   RB1   // I2C clock 

#define I2C_SDA   RB0   // I2C data I/O 

#define I2C_DELAY 1     // I2C delay 

#define CAMERA_ADDR 0xC0



#define CAMERA_PORT PORTA

#define LED1_LINE   0

#define HREF        1

#define VSYNC       2

#define CAM_RESET   3

#define DATA_PORT PORTC



#define OPTION_DEFAULT     RTW|RTS|RTE_D|RTE_ES|PS_1	// detect fall edge

#define OPTION_BOARD_CHECK RTW|RTS|RTE_D|PS_1		// detect rising edge

#define OPTION_TIMING      RTW|RTE_D



#define DDR_AREG(PORT)  MODE(M_DDR); PORT_CONTROL(PORT)

#define WKPND(PORT,VAL) MODE(M_WKPND); AREG=VAL; PORT_CONTROL(PORT)

#define IF_CARRY     #asm ( snc );

#define IF_NOT_CARRY #asm ( sc );

#define IF_ZERO      #asm ( snz );

#define IF_NOT_ZERO  #asm ( sz );

#define IF_BIT(addr,bit) #asm ( snb addr,bit );

#define IF_NOT_BIT(addr,bit) #asm ( sb addr,bit );

#define TEST(addr) #asm ( test addr )



void Camera_Setup(unsigned int8 cnt);

unsigned int8 i2c_send(unsigned int8 num,unsigned int8 near *buffer);

void Send_Frame();

void putc(char c);

void delay_us_4(registera delay_time);

void HiResFrame();

char getc();

unsigned int8 getNum();

void putNum(unsigned int8 val);

void pReturn();

void pVersion();

void pCK(registera c);

unsigned int8 divX16 (unsigned int16 near *numer_ptr, 

		      unsigned int8 near *dterm_ptr);

void delay_i2c();

void setDefaultWindow();

void delay_uart_bit();

void putc_href(registera c);

void set_portb_ddr (registera val);

void putc_delay_uart_bit_entry();

void clrMem(unsigned int8 near *ptr, unsigned int8 cnt);



char bank1 mem1[16];

char bank2 mem2[16];

char bank3 mem3[16];

char bank4 mem4[16];

char bank5 mem5[16];



unsigned int16 redLineSum    @ mem1;

unsigned int16 greenLineSum  @ mem1+2;

unsigned int16 blueLineSum   @ mem1+4;

unsigned int16 redLineDev    @ mem1+6;

unsigned int16 greenLineDev  @ mem1+8;

unsigned int16 blueLineDev   @ mem1+10;

unsigned int8  tmpVal        @ mem1+12;

unsigned int8  redFullMean   @ mem1+13;

unsigned int8  greenFullMean @ mem1+14;

unsigned int8  blueFullMean  @ mem1+15;



unsigned int8 redLineSumLo   @ redLineSum;

unsigned int8 redLineSumHi   @ redLineSum+1;

unsigned int8 greenLineSumLo @ greenLineSum;

unsigned int8 greenLineSumHi @ greenLineSum+1;

unsigned int8 blueLineSumLo  @ blueLineSum;

unsigned int8 blueLineSumHi  @ blueLineSum+1;



unsigned int8 redLineDevLo   @ redLineDev;

unsigned int8 redLineDevHi   @ redLineDev+1;

unsigned int8 greenLineDevLo @ greenLineDev;

unsigned int8 greenLineDevHi @ greenLineDev+1;

unsigned int8 blueLineDevLo  @ blueLineDev;

unsigned int8 blueLineDevHi  @ blueLineDev+1;



unsigned int16 redFullSum    @ mem2;

unsigned int16 greenFullSum  @ mem2+2;

unsigned int16 blueFullSum   @ mem2+4;

unsigned int16 redFullDev    @ mem2+6;

unsigned int16 greenFullDev  @ mem2+8;

unsigned int16 blueFullDev   @ mem2+10;

unsigned int8  meanLine      @ mem2+12;

unsigned int8  redLineMean   @ mem2+13;

unsigned int8  greenLineMean @ mem2+14;

unsigned int8  blueLineMean  @ mem2+15;



unsigned int8 redFullDevLo   @ redFullDev;

unsigned int8 redFullDevHi   @ redFullDev+1;

unsigned int8 greenFullDevLo @ greenFullDev;

unsigned int8 greenFullDevHi @ greenFullDev+1;

unsigned int8 blueFullDevLo  @ blueFullDev;

unsigned int8 blueFullDevHi  @ blueFullDev+1;



unsigned int8 myPixel_red    @ mem3;

unsigned int8 myPixel_green  @ mem3+1;

unsigned int8 myPixel_blue   @ mem3+2;

unsigned int8 myPixel_green2 @ mem3+3;



unsigned int8 CameraPortDDRTemp @ mem3;



unsigned int8 TrackBlock[6];

unsigned int8 redMin   @ TrackBlock+0;

unsigned int8 redMax   @ TrackBlock+1;

unsigned int8 greenMin @ TrackBlock+2; 

unsigned int8 greenMax @ TrackBlock+3; 

unsigned int8 blueMin  @ TrackBlock+4;

unsigned int8 blueMax  @ TrackBlock+5;



unsigned int8 outStats[11];



unsigned int8 BoxBlock[4];

unsigned int8 xBox  @ BoxBlock+0;

unsigned int8 yBox  @ BoxBlock+1;

unsigned int8 xSize @ BoxBlock+2;

unsigned int8 ySize @ BoxBlock+3;



unsigned int8 servoPos;		// Servo Position

unsigned int8 servoCnt @ mem4;	// Servo Counter



char getc_c;



unsigned int8 ParseBlock[4];

char indata @ ParseBlock;

char state1 @ ParseBlock+1;

char state2 @ ParseBlock+2;

unsigned int8 process @ ParseBlock+3;

unsigned int8 line    @ ParseBlock+3;



bit process1 @ process.1;

bit process2 @ process.2;

bit process4 @ process.4;

bit process7 @ process.7;



unsigned int8 BitFlagsBlock[2];

unsigned int8 BitFlags @ BitFlagsBlock;

bit ArgFlag      @ BitFlags.0;

bit lastHref	 @ BitFlags.0;

bit GetNumOK     @ BitFlags.0;

bit TransMode    @ BitFlags.1;

bit RawMode      @ BitFlags.2;

bit NoiseFilter  @ BitFlags.3;

bit AltMode      @ BitFlags.4;

bit AcquireMode  @ BitFlags.5;

bit MassMode     @ BitFlags.6;

bit LineDumpMode @ BitFlags.7;

unsigned int8 BitFlags2 @ BitFlagsBlock+1;

bit Tack         @ BitFlags2.0;

bit MeanMode     @ BitFlags2.1;

bit putcHrefMode @ BitFlags2.2;

bit divMode24bit @ BitFlags2.3;

bit led1AutoDisable @ BitFlags2.4;

bit vSyncFlag    @ BitFlags2.5;

bit HalfMode     @ BitFlags2.6;

bit autoServoMode    @ BitFlags2.7;



unsigned int8 fx_fx1 @ outStats+2;

unsigned int8 fy_fy1 @ outStats+3;

unsigned int8 fx_fx2 @ outStats+4;

unsigned int8 fy_fy2 @ outStats+5;

unsigned int8 fmisc_fsize @ outStats+6;

unsigned int8 fmisc_fconf @ outStats+7;



unsigned int16 fmisc_fcnt    @ outStats+6;

unsigned int8  fmisc_fcnt_lo @ outStats+6;

unsigned int8  fmisc_fcnt_hi @ outStats+7;



unsigned int16 xSum @ outStats+0;

unsigned int16 ySum @ outStats+8;

unsigned int8  xySum_hi2 @ outStats+10;



unsigned int8  xSum_lo @ xSum;

unsigned int8  xSum_hi @ xSum+1;

unsigned int8  ySum_lo @ ySum;

unsigned int8  ySum_hi @ ySum+1;



unsigned int8  xMean @ outStats+0;

unsigned int8  yMean @ outStats+1;



unsigned int8 tmp8 @ __AImage;		// allows access to the compiler temp

unsigned int8 tmp9 @ __FSRImage;	// allows access to the compiler temp

unsigned int8 tmpC @ 0x0C;		// allows access to the compiler temp

unsigned int8 tmpD @ 0x0D;		// allows access to the compiler temp



// use unused compiler temp

unsigned int8 BitFlags3 @ tmpC;

bit indata_flag  @ BitFlags3.0;

bit RawMode2     @ BitFlags3.1;

bit RawMode3     @ BitFlags3.2;

bit servoLineOut @ BitFlags3.3;

bit autoServoDir @ BitFlags3.4;

bit slaveMode    @ BitFlags3.5;

bit servoPosOut  @ BitFlags3.6;



/* Even values are the camera register address, odd is the value */

#define CAM_SETUP_CNT 8

const unsigned int8 camSetupData[CAM_SETUP_CNT] = {0x14, 0x20,

						   0x13, 0x21,

						   0x12, 0x28,

						   0x11, 0x02};



int __INT(void)

{

  indata=getc();

  indata_flag=1;

  WKPND(UART_PORT,0x00);

  return(0);

}



void pRawMode(registera val)

{

  if(!RawMode)

    putc(val);

}

void pReturn()

{

putc('\r');

}

void pCK(registera c)

{

  if (!RawMode2) {

    putc(c);

    putc('C');

    putc('K');

    pReturn();

  }

}

void pVersion()

{ 

putc('C');

putc('M');

putc('U');

putc('c');

putc('a');

putc('m');

putc(' ');

putc( 'v' );

putc( '1' );

putc( '.' );

putc( '1' );

putc( '1' );

pReturn();

}



// this division code does rounding

//     rounding currently only works for 8 bit values

// divMode24bit should is set to 0 by default and care

//     should be taken to set it back to 0 when finished



unsigned int8 divX16 (unsigned int16 near *numer_ptr, 

		      unsigned int8  near *dterm_ptr)

{

  unsigned int8  dterm @ dterm_ptr;

  unsigned int8  result @ numer_ptr;

  unsigned int8 numer_block[3];

  unsigned int16 numer @ numer_block;

  unsigned int8  numer_lo  @ numer_block;

  unsigned int8  numer_hi  @ numer_block+1;

  unsigned int8  numer_hi2 @ numer_block+2;

  unsigned int8 d_block[3];

  unsigned int16 d  @ d_block;

  unsigned int16 d2 @ d_block+1;

  unsigned int8  d_lo  @ d_block;

  unsigned int8  d_hi  @ d_block+1;

  unsigned int8  d_hi2 @ d_block+2;

  numer=*numer_ptr;

  numer_hi2=0;

  d_hi=*dterm_ptr;

  d_lo=0;

  d_hi2=0;

  if (divMode24bit) {

    d_hi2=*(dterm_ptr+1);

    numer_hi2=*(numer_ptr+2);

  }

  result=0;

  if (d2!=0) {

    dterm=d_hi/2;	// 8 bit rounding

    numer_lo+=dterm;	// 8 bit rounding

    if (STATUS.C) {

      numer_hi++;

      if (STATUS.Z) numer_hi2++;

      // numer+=(d2>>1);	// 16 bit rounding currently disabled

    }

    dterm=1<<7;

    do {

      STATUS.C=0;

      RR(d_hi2);

      RR(d_hi);

      RR(d_lo);

      if (!numer_hi2) {

	if(!d_hi2 && numer>=d)

	  {

	    numer-=d;

	    result|=dterm;

	  }

	dterm>>=1;

      }

      else {

	STATUS.C=0;

	RR(numer_hi2);

	RR(numer_hi);

	RR(numer_lo);

	STATUS.C=0;

      }

    }

    while(STATUS.C==0);

  }

  return (result);

}



void putNum(unsigned int8 val)

{

  unsigned int8 digit;

  unsigned int8 cnt;

  unsigned int8 flag;

  flag=0;

  digit=100;



  if(RawMode) {

    val++;		// check if val is 255

    if(STATUS.Z) val--;

    putc(val-1);

    return;

  }  

  do {

    cnt=0;

    while (1) {

      AREG=val-digit;

      if (!STATUS.C)

	break;

      val=AREG;

      cnt++;

    }

    if(cnt | flag) {		// using the bitwise OR saves space 

      flag|=1;

      putc(cnt+'0');

    }

    digit-=90;

  } while (STATUS.C);

  putc(val+'0');

}



unsigned int8 getNum()

{

  bit digit_flag;

  unsigned int8 value=0;

  digit_flag=0;

  GetNumOK = 1;

  

  while(1)

    {

      do {} while(indata_flag==0);

      indata_flag=0;

      AREG=indata-'0';

      tmp9=AREG;

      if (RawMode3) {

	return indata;

      }

      if (STATUS.C && tmp9 <= 9) {

	if(value>=100)break;

	// multiply value by 10

	value<<=1;

	tmp8=value*2;

	AREG+=tmp8;

	AREG+=tmp9;

	value+=AREG;

	digit_flag=1;

      }

      else if (!digit_flag) 

	{

	  if (indata!=' ')

	    break;

	}

      else if (indata==' ' || indata=='\r') 

	{

	return value;

      }

      else 

	{

	break;          

      }

      indata_flag=0;

    }

  GetNumOK=0;

  return 0;

}



void delay_uart_bit()

{

  AREG=256-81;	// 115K baud

  if (!UART_PORT.BAUD_SELECT)

    AREG=256-244;	// 38.4K baud

  RTCC=AREG+RTCC;

  do {} while(RTCC);  

}



char getc()

{

  OPTION(OPTION_TIMING|PS_8);  

  AREG=256-27;	// 115K baud

  if (!UART_PORT.BAUD_SELECT)

    AREG=256-81;	// 38.4K baud

  RTCC=AREG;

  do {} while(RTCC);

  getc_c=128; 

  do

    {

      delay_uart_bit();

      // PORTC.0=1;

      getc_c>>=1;

      if(UART_PORT.UART_RX)getc_c|=128;

      // PORTC.0=0;

    } 

  while (!STATUS.C);

  delay_uart_bit();

 

  OPTION(OPTION_DEFAULT);

  return (getc_c);

}



// delays for delay_time * 20 msecs

void delay_ms_20(unsigned int8 delay_time)

{

  unsigned int8 i;

  do {

    i = 40;

    do {

      delay_us_4(147);	// takes into account delay_us_4 inaccuracy

    } while (--i);

  } while (--delay_time);

}



// delays for delay_time * 4 usecs

// only works for delay_time args up to 254 

// the actual timing is 3.41 useconds

void delay_us_4(registera delay_time)

{

  tmp8 = 0;

  tmp8-= delay_time;

  OPTION(OPTION_TIMING|PS_256);

  RTCC = tmp8;

  do {} while(RTCC);

  OPTION(OPTION_DEFAULT);

}



void putc_delay_uart_bit()

{

  delay_uart_bit();

  putc_delay_uart_bit_entry();

}



void putc_delay_uart_bit_entry()

{

  if (putcHrefMode) {

    if (CAMERA_PORT.HREF) {

      if (!lastHref) {

	lastHref=1;

	if (--line==0) line|=0x01;

      }

    }

    else {

      lastHref=0;

    }

  }

  else if (servoCnt) {

    AREG=1;

    if (!UART_PORT.BAUD_SELECT)

      AREG=3;

    AREG=servoCnt-AREG;

    if (!STATUS.C)

      AREG=0;

    servoCnt=AREG;

    if (servoCnt==0)

      UART_PORT.SERVO_LINE=0;

  }

}



void putc_href(registera c)

{

  putcHrefMode=1;

  putc(c);

}



void putc(char c)

{

  unsigned int8 i;

  i=0;

  OPTION(OPTION_TIMING|PS_8);

  RTCC=0;

  UART_PORT.UART_TX=0;

  putc_delay_uart_bit();

  for(; !(i&8); )

    {

      if(c & 0x01)

        UART_PORT.UART_TX=1;

      else

        UART_PORT.UART_TX=0;

      RR(c);

      i++;

      putc_delay_uart_bit();

    }

  UART_PORT.UART_TX=1;

  putc_delay_uart_bit();

  OPTION(OPTION_DEFAULT);

  putcHrefMode=0;

}



/************************************ Uart Send Frame ****************************/



void Send_Frame()

{

unsigned int8 last;

unsigned int8 i,g;



last=0;

putc(1);



for( i=xBox+xSize; --i>=xBox; ) 

    {

      if (!HalfMode || (i&0x01)) {

      while(!CAMERA_PORT.VSYNC);

      // while(CAMERA_PORT.VSYNC); 

      while(!CAMERA_PORT.HREF);

      while(CAMERA_PORT.HREF);

      line=143;

      do

        {

            last=i;

            while(!CAMERA_PORT.HREF);

	    RTCC=0;

            do {

	      while(RTCC.0==1);

	      myPixel_blue=DATA_PORT; 

	      while(RTCC.0==0);

	      // myPixel_green=DATA_PORT; 

	      while(RTCC.0==1);

	      myPixel_red=DATA_PORT; 

	      while(RTCC.0==0);

	      myPixel_green2=DATA_PORT;

            } while (--last);	// Inner Column Loop

          

            if(line-yBox>=0 && line<yBox+ySize ) 

	      {

		lastHref=1;

		putc_href(myPixel_red);

		putc_href(myPixel_green2);

		putc_href(myPixel_blue);

		// putc_href(myPixel_green);

		if(indata_flag) return;

	      }

	    // wait for the line end

	    while(CAMERA_PORT.HREF);

	} while (--line);	// Row Loop

	delay_us_4(100);

      putc(2);

      }	// Half Mode     

    }	// Outer Column Loop

}



unsigned int8 minSet (registera mean)

{

  tmp8=AREG;

  AREG=30;

  AREG=tmp8-AREG;

  if (!STATUS.C)

    AREG=1;

  return AREG;

}



unsigned int8 maxSet ()

{

  unsigned int8 mean @ tmp8;

  AREG=30;

  AREG=mean+AREG;

  if (STATUS.C)

    AREG=254;

  return AREG;

}



void div3 (unsigned int8 near *ans, unsigned int8 near *numer, 

	   unsigned int8 near *denom) {

  unsigned int8 i;

  i = 3;

  do {

    *ans=divX16(numer,denom);

    ans++;

    numer+=2;

  } while (--i);

}





void shrink_window_size ()

{

  xSize>>=1;

  xSize++;

  xBox+=xSize/2;

  ySize>>=1;

  ySize++;

  yBox+=ySize/2;

}





/************************************ High Res Frame Grab ******************/



void HiResFrame()

{

  unsigned int8 k;

  

  if (!MeanMode) {

    if (AcquireMode) {

      if (Tack) {

	AcquireMode=0;    

	if (!AltMode)

	  Tack=0;

	setDefaultWindow(); 

	redMin=minSet(fx_fx1);

	redMax=maxSet();

	greenMin=minSet(fy_fy1);

	greenMax=maxSet();

	blueMin=minSet(fx_fx2);

	blueMax=maxSet();

      }

      else {

	Tack=1;

	shrink_window_size();

	goto go_shrink_window_size;

      }

    }

    if (AltMode) {

      setDefaultWindow();

      Tack=!Tack;

      if (Tack) {

	if (fmisc_fsize==0) {

	  Tack=0;

	}

	else {

	  xBox=fx_fx1;

	  yBox=fy_fy1;

	  xSize=fx_fx2-fx_fx1;

	  ySize=fy_fy2-fy_fy1;

	go_shrink_window_size:

	  shrink_window_size();

	}

      }

    }

  }

    

  if(Tack)

    {

      clrMem(&redFullSum,13);

      // redFullSum=0;

      // greenFullSum=0;

      // blueFullSum=0;

      // redFullDev=0;

      // greenFullDev=0;

      // blueFullDev=0;

      // meanLine=0;

      AREG=0xFE;

    }

  else

    {

      clrMem(&xSum,11);

      fx_fx1=254; 

      fy_fy1=254; 

      // fx_fx2=0; 

      // fy_fy2=0; 

      // fmisc_fcnt=0;

      // xSum=0;

      // ySum=0;

      AREG=0xAA;

    }  



  if (LineDumpMode) {

    putc(AREG);

  }



  if (!vSyncFlag) {

    while(!CAMERA_PORT.VSYNC);

    // while(CAMERA_PORT.VSYNC);

  }

  while(!CAMERA_PORT.HREF);

  while(CAMERA_PORT.HREF);

  line=143;

  do

    {



      if (Tack)	// Get Mean

	{

	  clrMem(&redLineSum,13);

	  // redLineSum = 0;

	  // greenLineSum = 0;

	  // blueLineSum = 0;

	  // redLineDev = 0;

	  // greenLineDev = 0;

	  // blueLineDev = 0;

	  // tmpVal = 0;

	  while(!CAMERA_PORT.HREF);

	  RTCC=0;

	  k=xBox-1;

	  k<<=1;

	  while (k) {

	    while(RTCC.0==1);

	    while(RTCC.0==0);

	    k--;

	  } 

	  k=xSize;

	  // tmpVal must be zero throughout this loop

	  do

	    {

	      while(RTCC.0==1);

	      AREG=DATA_PORT;		// load DATA_PORT into W first

	      blueLineSumLo+=AREG;

	      if (STATUS.C) blueLineSumHi++;

	      AREG=blueFullMean-AREG;

	      if (!STATUS.C) {

		AREG=tmpVal-AREG;

	      }

	      blueLineDevLo+=AREG;

	      if (STATUS.C) blueLineDevHi++;

	      while(RTCC.0==0);

	      while(RTCC.0==1);

	      AREG=DATA_PORT;		// load DATA_PORT into W first

	      redLineSumLo+=AREG;

	      if (STATUS.C) redLineSumHi++;

	      AREG=redFullMean-AREG;

	      if (!STATUS.C) {

		AREG=tmpVal-AREG;

	      }

	      redLineDevLo+=AREG;

	      if (STATUS.C) redLineDevHi++;

	      while(RTCC.0==0);

	      AREG=DATA_PORT;		// load DATA_PORT into W first

	      greenLineSumLo+=AREG;

	      if (STATUS.C) greenLineSumHi++;

	      AREG=greenFullMean-AREG;

	      if (!STATUS.C) {

		AREG=tmpVal-AREG;

	      }

	      greenLineDevLo+=AREG;

	      if (STATUS.C) greenLineDevHi++;

	    }

	  while (--k);

	}

      else	// Track Color

	{

	  unsigned int8 *ptrTmp;

      while(!CAMERA_PORT.HREF);

      RTCC=0;

      ptrTmp=mem1-1;

      k=80;

	do

	{ 

	  while(RTCC.0==1);

	  tmp9=DATA_PORT;

	  tmp8=0;

	  AREG=blueMax-AREG;

	  if (STATUS.C)

	    if (tmp9-blueMin>=0)

	      tmp8--;

	  while(RTCC.0==0);

	  ptrTmp++; 

	  ptrTmp|=0x10;

	  while(RTCC.0==1);

	  tmp9=DATA_PORT;

	  AREG=redMax-AREG;

	  if (!STATUS.C)

	    tmp8=0;

	  if (tmp9-redMin<0)

	    tmp8=0;

	  while(RTCC.0==0);

	  tmp8&=DATA_PORT;

	  *ptrTmp=tmp8;

	} while (--k); // for k

	}

      if(indata_flag) return; 



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

      //

      // Track Color

      //

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





      if(line-yBox>=0 && line<yBox+ySize ) 

	{

	  lastHref=1;

	  if (Tack)	// Get Mean

	    {

	      div3(&redLineMean,&redLineSum,&xSize);

	      // redLineMean=divX16(&redLineSum,&xSize);

	      // greenLineMean=divX16(&greenLineSum,&xSize);

	      // blueLineMean=divX16(&blueLineSum,&xSize);

	      redFullSum+=redLineMean; 

	      greenFullSum+=greenLineMean; 

	      blueFullSum+=blueLineMean; 

	      if (LineDumpMode) {

		putc_href(redLineMean);

		putc_href(greenLineMean);

		putc_href(blueLineMean);

	      }

	      if (meanLine) {

		div3(&redLineMean,&redLineDev,&xSize);

		// redLineMean=divX16(&redLineDev,&xSize);

		// greenLineMean=divX16(&greenLineDev,&xSize);

		// blueLineMean=divX16(&blueLineDev,&xSize);

		redFullDev+=redLineMean; 

		greenFullDev+=greenLineMean; 

		blueFullDev+=blueLineMean; 

	      }

	      meanLine++;

	      div3(&redFullMean,&redFullSum,&meanLine);

	      // redFullMean=divX16(&redFullSum,&meanLine);

	      // greenFullMean=divX16(&greenFullSum,&meanLine);

	      // blueFullMean=divX16(&blueFullSum,&meanLine);

	      putcHrefMode=1;

	      putc_delay_uart_bit_entry();

	      putcHrefMode=0;

	    }



	  else		// Track Color

	    {

	      unsigned int8 *loc, col, flags, out_bits;

	      

	      // bit flags are defined in this way to avoid a compiler bug

	      bit colorOK @ flags.0;	

	      bit newColorOK @ flags.1;

	      bit outColorOK @ flags.2;

	      

	      flags=0;

  	      loc=mem1-1;

	      out_bits=1;

	      for(col=1; col<81; col++ )

		{

		  newColorOK=0;

		  outColorOK=0;

		  loc++;

		  loc|=0x10;

		  if(col-xBox>=0 && col<xBox+xSize )

		    {

		      tmp8=*loc;	// green pixel value

		      if(tmp8!=0 && greenMax-tmp8>=0 && tmp8-greenMin>=0)

			{

			  newColorOK=1;

			  if(!NoiseFilter || colorOK ) {

			    outColorOK=1;

			    tmp8=col;

			    tmp9=line;

			    fmisc_fcnt++;

			    if(tmp8-fx_fx2>=0) fx_fx2=tmp8;

			    if(tmp8-fx_fx1<0)  fx_fx1=tmp8;

			    if(tmp9-fy_fy1<0)  fy_fy1=tmp9;

			    if(tmp9-fy_fy2>=0) fy_fy2=tmp9;

			    xSum_lo+=tmp8;

			    if (STATUS.C) {

			      xSum_hi++;

			      if (STATUS.Z)

				xySum_hi2+=0x10;

			    }

			    ySum_lo+=tmp9;

			    if (STATUS.C) {

			      ySum_hi++;

			      if (STATUS.Z)

				xySum_hi2++;

			    }

			  } // noise flag check

			} // color check

		      if (LineDumpMode && (!HalfMode || (col&0x01))) {

			STATUS.C=0;

			if (outColorOK)

			  STATUS.C=1;

			RL(out_bits);			

			if (STATUS.C) {

			dump_last_bitmap:

			  if (out_bits==0xAA)

			    RL(out_bits); // change it to 0x55, carry is set

			  putc_href(out_bits);

			  out_bits=1;

			}

		      } // Bitmap check

		    } // x box check

		  colorOK=0;

		  if (newColorOK) colorOK=1;

		} // col loop

	      if (out_bits!=1) {

		do {

		  STATUS.C=0;

		  RL (out_bits);

		} while (!STATUS.C);

		goto dump_last_bitmap;

	      }

	    } // Tack flag check

	} // y box check

      // wait for the line end

      while(CAMERA_PORT.HREF);



    } while (--line); 	// Row Loop





      if(Tack)

      {

	fx_fx1=redFullMean;

	fy_fy1=greenFullMean;

	fx_fx2=blueFullMean;

	tmpVal=meanLine-1;	

	div3(&fy_fy2,&redFullDev,&tmpVal);

	// fy_fy2=divX16(&redFullDev,&tmpVal);

	// fmisc_fsize=divX16(&greenFullDev,&tmpVal);

	// fmisc_fconf=divX16(&blueFullDev,&tmpVal);



     }

     else

       {

	 if(fx_fx2==0) {

	   fx_fx1=0;

	   fy_fy1=0;

	 }

	 else {

	     {

	       unsigned int8 hi2_tmp, mean_tmp;

	       divMode24bit=1;

	       hi2_tmp=xySum_hi2;

	       xySum_hi2&=0x0F;

	       mean_tmp=divX16(&ySum,&fmisc_fcnt);

	       SWAP(hi2_tmp);

	       xySum_hi2=hi2_tmp&0x0F;

	       ySum=xSum;

	       xMean=divX16(&ySum,&fmisc_fcnt);

	       yMean=mean_tmp;

	       divMode24bit=0;

	     }

     {

       unsigned int8 d1,d2,tmp;

       d1=fx_fx2-fx_fx1;

       d1++;

       d2=fy_fy2-fy_fy1;

       d2++;

       if (d1-d2>=0){

	 unsigned int8 d3;

         d3=d1;

         d1=d2;

         d2=d3;

       }

       {

         unsigned int16 tmp2;

         tmp2=(unsigned int16)divX16(&fmisc_fcnt,&d1)<<8;

         tmp=divX16(&tmp2,&d2);

       }

       fmisc_fcnt+=4;

       fmisc_fcnt>>=1;

       fmisc_fcnt>>=1;

       fmisc_fcnt>>=1;

       if (fmisc_fcnt_hi) fmisc_fcnt_lo=255;

       fmisc_fsize=fmisc_fcnt_lo;      

       fmisc_fconf=tmp;

       if (fmisc_fsize==0) fmisc_fconf=0;

      }

    }

    }



  vSyncFlag=0;

}



void clrMem(unsigned int8 near *ptr @ tmp9, unsigned int8 cnt @ tmp8)

{

  do {

    *ptr=0;

    ptr++;

  } while (--cnt);

}



/************************************ I2C Send ****************************/



void delay_i2c() 

{

  delay_us_4(I2C_DELAY);

}



void set_portb_ddr_i2c_idle()

{

  set_portb_ddr(I2C_PORT_DDR_IDLE);

}



void set_portb_ddr_i2c_write()

{

  set_portb_ddr(I2C_PORT_DDR_WRITE);

}



void set_portb_ddr(registera val)

{

  if (!servoLineOut)	// set input direction for servo line

    AREG|=0x40;

  DDR_AREG(PORTB);

  delay_i2c();

}



unsigned int8 i2c_send(unsigned int8 num,unsigned int8 near *buffer)

{

  unsigned int8 ack,i,k;

  unsigned int8 data; 



  // Send Start Bit

  I2C_PORT.I2C_SDA=0;			// needed because values can be reset by read-modify cycle

  set_portb_ddr(I2C_PORT_DDR_READ_SCL);	// SDA=0 SCL=1

  I2C_PORT.I2C_SCL=0;			// needed because values can be reset by read-modify cycle

  set_portb_ddr_i2c_write();		// SDA=0 SCL=0



  // Send the Byte    

  for( k=0; k!=num; k++ )

    { 

      data=buffer[k]; // To avoid shifting array problems   

      for( i=0; !(i&8); i++ ) // Write data    

	{

	  if( data & 0x80 ) { 

	    set_portb_ddr(I2C_PORT_DDR_READ_SDA);  // SDA=1 SCL=0

	    set_portb_ddr_i2c_idle();              // SDA=1 SCL=1

	  }

	  else {

	    set_portb_ddr_i2c_write();             // SDA=0 SCL=0

	    set_portb_ddr(I2C_PORT_DDR_READ_SCL);  // SDA=0 SCL=1 

	  }

	  while(!I2C_PORT.I2C_SCL);

    

	  if( data & 0x80 ) { 

	    set_portb_ddr(I2C_PORT_DDR_READ_SDA);  // SDA=1 SCL=0

	  }  

	  else {

	    set_portb_ddr_i2c_write();             // SDA=0 SCL=0

	  }



	  data<<=1;

	}   // END OF 8 BIT FOR LOOP



      // Check ACK  <*************************************

      set_portb_ddr(I2C_PORT_DDR_READ_SDA);  	// SDA=1 SCL=0



      set_portb_ddr_i2c_idle();      		// SDA=1 SCL=1

      ack=0;



      if(I2C_PORT.I2C_SDA)           		// sample SDA

	{

	  ack|=1;

	  break;

	}



      set_portb_ddr_i2c_write();  	// SDA=0 SCL=0

    

    }

     

  // Send Stop Bit 

  set_portb_ddr(I2C_PORT_DDR_READ_SCL);  // SDA=0 SCL=1

  set_portb_ddr_i2c_idle();  		 // SDA=1 SCL=1

 

return ack;



}



// cnt is the number of args * 2 

void Camera_Setup(unsigned int8 cnt)

{

  unsigned int8 i;

  unsigned int8 cameraData[3];

  unsigned int8 *ptrTmp;

  unsigned int8 end_cnt;

  if (slaveMode)

    return;

  AREG=cnt;

  if (STATUS.Z) {

    delay_us_4(25);

    CAMERA_PORT.CAM_RESET=1;

    delay_us_4(25);

    CAMERA_PORT.CAM_RESET=0; 

    delay_us_4(50);

    AREG=CAM_SETUP_CNT;

  }

  end_cnt=AREG;



 cameraData[0]=CAMERA_ADDR;

 ptrTmp=mem1-1;

 i=0;

 do

   {

     ptrTmp++;

     ptrTmp|=0x10;

     AREG=*ptrTmp;

     if (cnt==0) {

       AREG=camSetupData[i];

     }

     cameraData[2]=AREG;

     if (i&0x01) {

       do {} while(i2c_send(3,cameraData)); 

     }

     cameraData[1]=cameraData[2];

     i++;

   } while (--end_cnt);

}



// to save space anything set to all ones was commented out

// because the power up default is the same value

void ports_init()

{



  if (UART_PORT.SLAVE_SELECT==0)

    slaveMode=1;

  if (UART_PORT.AUTO_START==0)

    slaveMode=0;



  // OPTION(OPTION_DEFAULT);        // Setup interrupts

  // LVL(DATA_PORT,0b11111111);     // Set Data port values

  // DDR(DATA_PORT,  IIIIIIII);

  I2C_PORT=    0b00001000;

  set_portb_ddr_i2c_idle();

  PLP(I2C_PORT,0b00001011);      // Setup Camera I2C

  ST (I2C_PORT,0b11111000);      // Setup schmitt trigger

  // LVL(I2C_PORT,0b11111111);

  CAMERA_PORT=0b11111110;        // set the port latch value 

  AREG=          IIIIOIII;

  if (slaveMode)

    AREG=        IIIIIIII;  

  CameraPortDDRTemp=AREG;

  DDR_AREG(CAMERA_PORT);         // set the data direction 

  PLP(CAMERA_PORT,0b00001111);   // disable the pull up resistors 

  //  UART_PORT=0b00001011;

  // OPTION(OPTION_DEFAULT);



  // Setup the interrupt on change handler for UART

  

  //  AREG=          IOIIOIII;

  //  set_portb_ddr(AREG);

  WKED (UART_PORT,0x04);

  WKPND(UART_PORT,0x00);

  WKEN (UART_PORT,0xFB);

}



void setDefaultWindow()

{

  xBox=1;

  yBox=1;

  xSize=80;

  ySize=143;

}



// can only handle up to 16 args, can't copy across banks

void argCopy (unsigned int8 near *dest, unsigned int8 cnt @ tmp8)

{

  while (cnt) {

    cnt--;

    tmp9=mem1[cnt];

    dest[cnt]=tmp9;

  }

}



void main(void)

{

SetDefaults:

BitFlags=0;

BitFlags2=0;

BitFlags3=0;	// set first so that servo line ddr is set correctly

servoPos=0;

ports_init();

Camera_Setup(0);

setDefaultWindow();

state1=0;

state2=0;

// TransMode=0;

// RawMode=0;

NoiseFilter=1;

MassMode=1;

// divMode24bit=0;

// led1AutoDisable=0;

// lastProcess=0;

process=0;

pReturn();

pVersion();



// Check for version 1.1 board and enable LED

OPTION(OPTION_BOARD_CHECK);

RTCC=0;

while(RTCC.0==0);

if (CAMERA_PORT.LED1_LINE==0) {

  CameraPortDDRTemp&=0xFE;

  DDR(CAMERA_PORT,CameraPortDDRTemp);

}

OPTION(OPTION_DEFAULT);



// Check for stand alone mode

 if (UART_PORT.AUTO_START==0) {

   delay_ms_20(250);

   mem1[0]=18;

   mem1[1]=32;

   mem1[2]=19;

   mem1[3]=32;

   Camera_Setup(4);

   if (UART_PORT.SLAVE_SELECT==0)

     autoServoDir=1;

   autoServoMode=1;

   servoLineOut=1;

   set_portb_ddr_i2c_idle();

   AcquireMode=1;

   goto track_window_start;

 }



while(1)

    {



      // output the prompt 

      putc(':');

          

      while (1) {

	while(indata_flag==0);

	indata_flag=0;

	if(indata>=97) indata&=0xDF;	// convert to upper case

	if(indata=='\r') {

	  goto uiAck_handle;

	}

	if (process7) break;

	state1=indata;

	if (state1!=' ' && state1!=10) {

	  state1=indata;

	  process7=1;

	}

      }

      state2=indata;

	  

      {

	unsigned int8 *ptr;

	unsigned int8 rawcnt;

	unsigned int8 argcnt @ tmpD;



	    // the arguments are read in and stored in mem1 and mem2



	ptr = mem1-1;

	argcnt=0;

	AREG=255;

	if (RawMode3) 

	  AREG=getNum();

	rawcnt=AREG;

	do

	  {

	    if (argcnt==rawcnt)

	      goto args_OK;

	    tmp8 = getNum();

	    if (GetNumOK) {

	      if (argcnt & 32) {

		break;

	      }

	      argcnt++;

	      ptr++;

	      ptr|=0x10;

	      *ptr = tmp8;

	    }

	    if (!RawMode3 && indata=='\r') {

	      goto args_OK;

	    }

	  } while (GetNumOK);

	goto uiAck_handle;



      args_OK:



	if(state1=='C' && state2=='R' && (!(argcnt & 0x01)))

	  {

	    Camera_Setup(argcnt);

	    goto handle_uiAck3;

	  }

	if( argcnt==1 )

	  {

	    ArgFlag=0;

	    tmp8=mem1[0];

	    if(!STATUS.Z)ArgFlag=1;

	    if (state2=='M'  )

	      {

		if(state1=='P'  )

		  {

		    TransMode=0;

		    if(ArgFlag) TransMode=1;

		    goto handle_uiAck3;

		  }

		if(state1=='R'  )

		  {

		    RawMode=0;

		    RawMode2=0;

		    RawMode3=0;

		    if(tmp8&0x01) RawMode=1;

		    if(tmp8&0x02) RawMode2=1;

		    if(tmp8&0x04) RawMode3=1;

		    goto handle_uiAck3;

		  }

		if(state1=='S'  )

		  {

		    AltMode=0;

		    if(ArgFlag) AltMode=1;

		    goto handle_uiAck3;

		  }

		if(state1=='M'  )

		  {

		    MassMode=0;

		    if(ArgFlag) MassMode=1;

		    autoServoDir=0;

		    if(tmp8&0x04) 

		      autoServoDir=1;

		    servoPosOut=0;

		    if(tmp8&0x08) 

		      servoPosOut=1;

		    autoServoMode=0;

		    if(tmp8&0x02) {

		      autoServoMode=1;

		      goto servo_config;

		    }

		    goto handle_uiAck3;

		  }

		if(state1=='L'  )

		  {

		    LineDumpMode=0;

		    if(ArgFlag) LineDumpMode=1;

		    goto handle_uiAck3;

		  }

		if(state1=='H'  )

		  {	

		    HalfMode=0;

		    if(ArgFlag) HalfMode=1;

		    goto handle_uiAck3;

		  }

	      }

	    if(state1=='N' && state2=='F'  )

	      {

		NoiseFilter=0;

		if(ArgFlag) NoiseFilter=1;

		goto handle_uiAck3;

	      }

	    if (state2=='1') {

	      if(state1=='L')

		{

		  led1AutoDisable=1;

		  if(tmp8&0x02)

		    led1AutoDisable=0;

		  if(tmp8&0x01)

		    CAMERA_PORT.LED1_LINE=1;

		  else

		    CAMERA_PORT.LED1_LINE=0;

		  goto handle_uiAck3;

		}

	      if(state1=='S')

		{

		  autoServoMode=0;

		  servoPos=tmp8;

		  if (servoPos&0x80) {

		    UART_PORT.SERVO_LINE=1;

		    servoPos=0;

		  }

		  else {

		  servo_config:

		    UART_PORT.SERVO_LINE=0;

		  }

		  servoLineOut=1;

		  set_portb_ddr_i2c_idle();

		  goto handle_uiAck3;



		}

	    }

	  }

	if(state1=='S' && state2=='W') 

	  { 

	    if (argcnt==4 )

	      {

		unsigned int8 xBox_new @ mem1+0;

		unsigned int8 yBox_new @ mem1+1;

		unsigned int8 xSize_new @ mem1+2;

		unsigned int8 ySize_new @ mem1+3;

		if (xBox_new!=0 && yBox_new!=0 &&

		    xSize_new<=80 && ySize_new<=143 ) 

		  {

		    xSize_new=xSize_new-xBox_new;

		    if (STATUS.C) {

		      ySize_new=ySize_new-yBox_new;

		      if (STATUS.C) {

			xSize_new++;

			ySize_new++;

			argCopy(&BoxBlock,4);

			goto handle_uiAck3;

		      }

		    }

		  }

	      }

	    if (argcnt==0)

	      {

		setDefaultWindow();

		goto handle_uiAck3;

	      }		

	  }

        if(state1=='T' && (state2=='C'||state2=='W')

	   && (argcnt==0||argcnt==6))

	  {

	    argCopy(&TrackBlock,argcnt);

	    /*

              if (argcnt!=0)

	      {

	      redMin  =mem1[0];

	      redMax  =mem1[1];

	      greenMin=mem1[2];

	      greenMax=mem1[3];

	      blueMin =mem1[4];

	      blueMax =mem1[5];

	      }

	    */

	    AcquireMode=0;

	  track_window_start:

	    Tack=0;

	    process2=1;

	    vSyncFlag=0;

	    MeanMode=0;

	    if(state2=='W') {

	      AcquireMode=1;

	    }

	    else if(AltMode) {

	      Tack=1;

	    }

	  }               

	if ( argcnt==0 )

	  {

	    if(state1=='G') {

	      if (state2=='V'  )

		{

		  pCK('A');

		  pVersion();

		  goto handle_uiAck7;

		}

	      if(state2=='M'  )

		{

		  Tack=1;

		  MeanMode=1;

		  process2=1;

		  vSyncFlag=0;

		}

	    }

	    if(state1=='D' && state2=='F'  )

	      {

		process1=1;

	      }

	    if(state1=='R' && state2=='S'  )

	      {

		process4=1;

	      }

	    if(state1=='I' && state2=='1' )

	      {

		servoPos=0;

		servoLineOut=0;

		autoServoMode=0;

		set_portb_ddr_i2c_idle();

		pCK('A');

		AREG=0;

		IF_BIT(UART_PORT,SERVO_LINE)

		  AREG=1;

		putNum(AREG);

		pReturn();

		goto handle_uiAck7;

	      }

	  }

      }



    uiAck_handle:



	if (process==0x80) {

	  if (!RawMode3) {

	    do {} while(indata!='\r');

	    indata_flag=0;

	  }

	uiAck_NCK:

	  AREG='N';

	} 

	else {

	handle_uiAck3:

	  AREG='A';

	}

	pCK(AREG);

      handle_uiAck7:

	state1=0;

	state2=0;

	process7=0;



	while (process) {



	  if(process4)

	    goto SetDefaults;

    

	  if(process1)

	    {

	      Send_Frame();

	      process=0;	// must be cleared because overwritten by line

	      putc(3);

	      if (indata_flag) {

		if(indata!='\r' )

		  process1=1;

		indata_flag=0;

	      }

	    }

	  if(process2)

	    {

	      HiResFrame();

	      process=0;	// must be cleared because overwritten by line

	      servoCnt=0;

	      if (LineDumpMode) {

		if (Tack) {

		  AREG=0xFD;

		}

		else {

		  putc(0xAA);

		  AREG=0xAA;

		}

		putc(AREG);

	      }

	      if(indata_flag) {

		if(indata=='\r' ) {

		  if (!MeanMode && AltMode)

		    setDefaultWindow();

		}

		else {

		  process2=1;

		}

		indata_flag=0;

	      }

	      else {



		unsigned int8 tmp;

		if (!TransMode || AcquireMode) process2=1;

		if (RawMode) putc(255);

		tmp=2;

		AREG='S';

		if (!Tack) {

		  if (!led1AutoDisable) {

		    if (fmisc_fsize >= 2) 

		      CAMERA_PORT.LED1_LINE=1;

		    else

		      CAMERA_PORT.LED1_LINE=0;

		  }

		  AREG='C';

		  if (MassMode) {

		    if (autoServoMode) {

		      if (servoPos==0)

			servoPos=64;

		      if (fmisc_fsize >= 2) {

			tmp8=40;

			AREG=xMean-AREG;

			if (autoServoDir) {

			  AREG=tmp8-xMean;

			}

			tmp8=0;

			IF_NOT_CARRY AREG=tmp8-AREG;

			if (AREG & 0xF8) {	// is the delta more than 8?

			  if (AREG & 0xF0)   // is the delta more than 16?

			    AREG=6;

			  else

			    AREG=3;

			  if (STATUS.C) {

			    AREG=servoPos-AREG;

			    IF_NOT_CARRY AREG=1;

			    servoPos=AREG;

			  }

			  else {

			    servoPos+=AREG;

			  }

			}

		      }

		    }

		    tmp=0;

		    AREG='M';

		    IF_BIT(BitFlags3,servoPosOut)

		      AREG='N';

		  }

		}

		putc (AREG);

		// servo timing is based on 950-2050 usec

		// with 150 usec as the center

		if (servoPos!=0) {

		  UART_PORT.SERVO_LINE=1;

		  if (servoPos & 0x80)

		    servoPos=127;

		  servoCnt=servoPos+109;

		}

		if (servoPosOut) {

		  pRawMode(' ');

		  putNum(servoPos); 

		}

		do {

		  pRawMode(' ');

		  putNum(outStats[tmp]); 

		  if(CAMERA_PORT.VSYNC)

		    vSyncFlag=1;	     

		  tmp++;

		} while (!(tmp&8));

		pRawMode('\r');

		OPTION(OPTION_TIMING|PS_8);

		while (servoCnt) {

		  putc_delay_uart_bit();

		} 

		OPTION(OPTION_DEFAULT);

	      }

	    }

	}

      }

}