#include <hc11.h>
#include <mil.h>
#include <analog.h>
#include <irtj.h>
#include <int32.h>
#include <vectors.h>


#define PERIOD 40000   /*Period = 20ms*/

#pragma interrupt_handler irread2;
void irread2();

int beacon_num=3;               /*BEACON NUMBER*/
int counter=0;
int pulse_count=-1;
int analog_value2[8];
int i;
long t;
int BIT2=0x04;

extern INT32 time2;

void
msleep(t)   /*Created 7/21/94 Chuck McManis */
	int	t; 	/* number of milleseconds to sleep */
{
	unsigned int	i;

	asm(" sei");
        i = LO_WORD(time2) + t;
	asm(" cli");
        while (i > LO_WORD(time2)) ;
	return;
}




/* void init_ir2()
	 Will initialize the interrupt-driven ir readings for TJ.  OC3 will be used
	 for timing control
*/

void init_ir2()
{
	int i;

	INTR_OFF();

	for (i = 0; i < 9; i++)
                analog_value2[i] = 0;

	TOC3 = 10000;
	CLEAR_BIT(TCTL1, 0x30);  /* Interrupt will not affect OC3 pin */
	SET_BIT(TMSK1, 0x20);    /* Turn on OC3 interrupt */
	INTR_ON();

}

/* sonar_pulse() modulates PORTB, bit 0 sonar transducer at 40kHz */
void sonar_pulse() 
{
	asm("ldaa    #1\n"
			"ldy    #255\n"         /* 2*(# of IR pulses) */
			"staa    4100\n");        /* 4100 = Port B */
	asm("loop2 : ldaa 4100\n"     /* 4 cycles - necessary */
			"eora #1\n"               /* 2 cycles - necessary */
			"staa 4100\n"           /* 4 cycles - necessary */
			"nop\n"
			"nop\n"
			"nop\n"               /* 8 cycles */
			"next2 : nop");
	asm("dey\n"                         /* 4 cycles - necessary */
			"bne loop2");                    /* 3 cycles - necessary */
							/* total = 25 cycles = 40 kHz*/
	asm("ldaa 4100");              /* Clear Port B - Turn LEDs off */
	asm("anda #254");
	asm("staa 4100");
} 

/*Calls the modulator and then reads the analog ports */
/* Care should be taken since it also kills interrupts
         for that period of time2*/

/* irread2() is the interrupt handler */


void irread2() 
{   pulse_count=-1;
	TCTL2=0x10;						/*Set IC1 to capture rising edges only*/
    
	
	if (TFLG1 & BIT2){
	   t=TIC1;
	   CLEAR_FLAG(TFLG1, 0x40);		/*Clears IC1 flag*/
	   pulse_count++;
	}/*end if*/ 
		  
	  while (TCNT < t+0x5DC0){  		 /*Look for pulses for the next 24000 cycles (12ms)*/
	    if (TFLG1 & BIT2){
	      CLEAR_FLAG(TFLG1, 0x40);		/*Clears IC1 flag*/
	      pulse_count++;
	    }/*end if*/  
	  }/*end while  
	}/*end if*/
			

}
void main()
{
  irread2();
  if (pulse_count==beacon_num)
	  sonar_pulse();   
}