/* Handyboard Sonar Driver
   Originally By Acroname
   Modified by Rand Voorhies - 10/07 - to use timer/counter 2 on PA1 so that TC3 on PA0 may be reserved for the compass.
   Modified by Phil Kassouf and Rand Voorhies - 3/08 - fixed timer overflow issue temporarily

   For an overview of the SRF04, please visit:
    http://www.robot-electronics.co.uk/htm/srf04tech.htm 
    http://www.acroname.com/examples/10034/10034.html

   Pin Connections: 
     Echo Pulse Output: Digital 8
     Trigger Pulse Input: Digital 9
*/

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

float sonar_sample() { 
    int start_time; 
    int dist;
    float udist =0.0;
    
    
    poke(0x1023, 2); /* clear tic2 flag */ 
    dist = 0;
    
    start_time = peekword(0x100e); /* capture start time */ 
    bit_set(0x1000, 0x80); /* send init pulse */ 
    bit_clear(0x1000, 0x80); /* wait until receive echo */ 
    
    while (peek(0x1000) & 0x02) { 
        if ((peekword(0x100e) - start_time) < 0) { /* if too much time has elapsed, abort */
            return 18.0224; 
        }
    defer(); /* let others run while waiting */ 
    }
    
    msleep(10L); /* give unit time to reset */ 

    msleep(10L); /* give unit time to reset */ 
    dist= peekword(0x1012)-start_time; 
    if(dist & 0x8000) {
        dist  = -dist;
        udist = ((float)dist) *1.1/4000.0;
        udist = 18.0224 - udist  ;
    }else{
        udist = ((float)dist) * 1.1/4000.0;
    }
    return udist; /* tic3 has time of echo */ 

}