/*
	sonar.c
	Polaroid 6500 routines for Handy Board / Interactive C
	by Fred Martin, fredm@media.mit.edu
	Sat Nov 22 13:57:35 1997

 	echo signal is connected to tic2/pa1 en entree;
 	init signal is pd5 (SS) en sortie;
 	binh signal is pd4 (SCK) en sortie
*/

void sonar_init() {
	bit_set(0x1009, 0x30);		/* Direction du portD 0011 0000 */
	bit_set(0x1021, 3);			/* Registre de controle pour capture IC2 */
	bit_clear(0x1021, 4);		/* Configuration en "rising edge" */
   printf("Go\n");				/* Clear LCD */
}


int sonar_sample() {
	int start_time;

	poke(0x1023, 2);				/* Met a zero le flag du tic2 */

	start_time = peekword(0x100e);	/* Lecture du compteur 16 bits */
   printf("st = %d", start_time);
	bit_set(0x1008, 0x20);			/* INIT = 1 */

   printf(" echo = %d", peekword(0x1000));

	while (!(peek(0x1000) & 0x2)) {	/* wait until receive echo */
		if ((peekword(0x100e) - start_time) < 0) {
			/* if too much time has elapsed, abort */
			bit_clear(0x1008, 0x20);
			return -1;
		}
		defer();					/* let others run while waiting */
	}

	bit_clear(0x1008, 0x20);		/* clear pulse trigger INIT = 0 */
	return peekword(0x1012) - start_time;	/* tic3 has time of echo */
}

int sonar_closeup() {
	int start_time;

	poke(0x1023, 2);				/* clear tic2 flag */
	start_time= peekword(0x100e);
	poke(0x1008, 0x20);

	while ((peekword(0x100e) - start_time) < 1000);

	bit_set(0x1008, 0x30);			/* turn on BINH */

	while (!(peek(0x1000) & 0x02)) {
		if ((peekword(0x100e) - start_time) < 0) {
			/* if too much time has elapsed, abort */
			bit_clear(0x1008, 0x30);
			return -1;
		}
		defer();
	}

	bit_clear(0x1008, 0x30);

	return peekword(0x1012) - start_time;	/* 0x1014 is tic3 */
}

void sonar_display()
{
	int result;
   int i = 0;
	sonar_init();
   /*while(i < 10)
   {*/
		result = sonar_sample();
	/*	if (result != -1)
      printf("dist %d:%d\n", i, result);
		else printf("*******\n");
		sleep(5.0);
      i = i + 1;
   }*/
   printf(" fin");
}