Sonar

uint16_t sonar1Read(void)
{
    sonar1_ack = 0;
    trig1_PORT &= ~(1 << trig1_PIN);
    trig1_PORT |= (1 << trig1_PIN);
    _delay_us(10);
    trig1_PORT &= ~(1 << trig1_PIN);

    while (sonar1_ack == 0)
    {
        TCCR1B |= (1 << CS11);
        if ((PIND & (1 << PIND3)))
        {
            TCNT1 = 0;
            TCCR1B |= (1 << CS11);
            while ((PIND & (1 << PIND3)))
            {
                if (TCNT1 > 5000)
                {
                    TCCR1B = 0;
                    TCNT1 = 0;
                    return 111;
                }
            }
            TCCR1B = 0;
            pulse1 = TCNT1 / 2;
            TCNT1 = 0;
            sonar1_ack = 1;
//          Serial_sendInt(pulse1, DEC);
//          Serial_sendString("\t");
        }
        else if (TCNT1 > 10000)
        {
            TCCR1B = 0;
            TCNT1 = 0;
            return 112;
        }
    }
    return (.035 * pulse1 / 2);
}