void sonar_init() {
    bit_set(0x1009, 0x30);         /* ddrd */
    bit_set(0x1021, 0x10);         /* at tctl2, */
    bit_clear(0x1021, 0x20);       /* set tic1 for rising edge */
}


int sonar_sample() {
    int start_time;
 
    poke(0x1023, 4);                /* clear tic1 flag */
 
    start_time= peekword(0x100e);   /* capture start time */
    bit_set(0x1008, 0x20);          /* trigger pulse */
 
    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 */
    
    return peekword(0x1010) - start_time;   /* tic3 has time of echo */
}

void main() {
    sonar_init();
    
    while(1) {
        printf("%f\n", (float)sonar_sample()*1.1/4000.);
        sleep(.1);
    }
}