//#use "srf04_sonar.ic"

/*********************************************/
/* HANDY BOARD EXAMPLE FOR */
/* DEVANTECH SRF04 ULTRASONIC RANGING MODULE */
/* */
/* This code is based heavily on the */
/* Polaroid Sonar Ranging code at: */
/* http://handyboard.com/software/sonar.html */
/* developed by Fred Martin */
/* fredm@media.mit.edu */
/*********************************************/

void sonar_init() {
    bit_set(0x1026, 0x80); /* set Digital Input #9 as output */
    bit_clear(0x1021, 1); /* at TCTL2, */
    bit_set(0x1021, 2); /* set TIC3 for falling edge */
}

int sonar_sample() {


    int start_time;
    int dist;

    poke(0x1023, 1); /* clear tic3 flag */

    start_time= peekword(0x100e); /* capture start time */

    bit_set(0x1000, 0x80); /* send init pulse */
    bit_clear(0x1000, 0x80);

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

    msleep(10L); /* give unit time to reset */

    dist= peekword(0x1014)-start_time;

    return dist; /* tic3 has time of echo */
}