/*
	sonar.c
	Polaroid 6500 routines for Handy Board / Interactive C 
	by Doug Rinckes (doug.rinckes@iname.com)
	January 2001

	Based on original code by:
	by Fred Martin, fredm@media.mit.edu
	Sat Nov 22 13:57:35 1997

 	echo signal is connected to tic3/pa0;
 	init signal is pd5 (SS);
 	binh signal is pd4 (SCK)

	Freds code modified to allow up to 10 meter measurements.

	The speed of sound (in m/sec) is given by:
		 speed = sqrt(r * R * T)
	where r is specific heat ratio (1.4), R is gas constant (286), and
	T is absolute temperature (i.e. temp in celsius + 273.15)

	So for 17 degrees (pretty normal indoors temperature), we get 340.85 m/s.

	The timer used runs at 2MHz.  So, to work out the distance to the object,
	we need to know how many of these cycles occur per centimeter.

	So, take value above (340.85), divide by 10,000 to give cm/u-sec.  Invert 
	this to give u-sec/cm, and multiply by 2, giving cycles/cm.

	Now, since the sound has to get to the target and back, double this value.

	*phew*  this should give you about 117.  This is the constant used in the
	distance calculation.  The other constant is how far 65536 cycles gives - since
	the counter may have cycled back to zero, and in fact does this about every 32 
	milliseconds.  Using the speed value above, this is 558.  Without this, the 
	furthest difference you can measure is about 2.8 meters.  

	This routine uses the BINH blanking signal, so measurements down to about 17cm
	are possible.

	If you got really carried away you could mount a thermometer on your robot to
	compensate these calculations.
	
*/


void sonar_init() {
	bit_set(0x1009, 0x30);		/* ddrd */
	bit_set(0x1021, 1);			/* at tctl2, */
	bit_clear(0x1021, 2);		/* set tic3 for rising edge */
}


int sonar_sample() {

	/*
		note on variable names
		The first part indicates its use, the second the units.
		st is start time, end is end time, dur is duration, dst is distance.
		ms is millisec, us is micro sec, cm is centimeters.
	*/

	int st_us,end_us;
	long st_ms,end_ms,dur_ms,abort_ms;
	int dst_cm;

	poke(0x1023, 1);

	/* capture the start time */
	st_ms = mseconds();

	/* calc the abort time = 70ms is about 12 meters */
	abort_ms = st_ms + 70L;

	st_us = peekword(0x100e);

	/* trigger the pulse */
	bit_set(0x1008, 0x20);

	/* wait for blanking time - about 10cm worth */
	while ((peekword(0x100e) - st_us) < 1170);

	/* turn on the BINH (blanking inhibit) signal */
	bit_set(0x1008, 0x30);

	/* while the echo has not come in */
	while (!(peek(0x1000) & 0x1)) {
		/* if we've been waiting too long (70ms = 12 meters) */
		if (mseconds() > abort_ms) {
			bit_clear(0x1008, 0x30);
			return(-1);
		}
		defer();
	}
	/* save the end times */
	end_us = peekword(0x1014);
	dur_ms = mseconds() - st_ms;

	/* clear trigger and BINH */
	bit_clear(0x1008, 0x20);
	bit_clear(0x1008, 0x30);

	/*  divide timers by number of cycles required to travel 1cm */
	st_us = st_us / 117;
	end_us = end_us / 117;

	/* find distance difference */
	dst_cm = end_us - st_us;

	/* if distance is less than zero, add distance of 65536 cycles */
	if (dst_cm < 0) dst_cm = dst_cm + 558;

	/* if the millisecond counter indicates that the integer counter has cycled */
	if (dur_ms > 32L) dst_cm = dst_cm + 558;
	if (dur_ms > 64L) dst_cm = dst_cm + 558;

	/* my unit reads about 5cm too high - yours may differ */
	return (dst_cm - 5);
}


void sonar_display()
{
	int distance;
	sonar_init();
	
	while (1) {
		int result;
		result= sonar_sample();
		if (result != -1) printf("%d\n", result);
		else printf("*******\n");
		msleep(300L);
	}
}