#define PORTA 0x378
#define PORTB 0x379
#define PORTC 0x37A

void sonar_init()
{
	bit_set(0x1009, 0x30);
   bit_set(0x1021, 1);
   bit_clear(0x1021, 2);
}

int sonar_sample()
{
	int start_time;
	poke(0x1023, 2);
   start_time = peekword(0x100e);
   bit_set(0x1008, 0x20);

   while(!(peek(0x1000) & 0x1))
   {
   	if((peekword(0x100e) - start_time) < 0)
      {
      	bit_clear(0x1008, 0x20);
         return -1;
      }
   	defer();
	}
   bit_clear(0x1008, 0x20);

   return peekword(0x1014) - start_time;
}

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