int PING()
{
// Use this FORMULA for TOP:
/* TOP/ICR1 = Clock_Speed / (2 * Prescaler *
Output_PWM_Frequency) */
// Thus ICR1 = 8000000 / (2 * 1 * 40000) = 100
// Make PD5 as output pin
DDRD = 0b00100000;
PORTD = 0b00100000;
// Turn off PWM while we set it up
TCCR1B |= 0;
TCCR1A |= 0;
// Setup the timer --> 16-bit Phase/Frequency Correct PWM mode with Pre-scaler = 1
TCCR1B |= (1 << WGM13);
TCCR1B |= (1 << CS10);
// Setup the Compare Output to be set to toggle mode. See pg.108 in datasheet
TCCR1A |= (1 << COM1A0);
// Set ICR1 so the output toggles at 40KHz
ICR1 = 100;
// Set the Comparator so we're at 50% duty cycle
OCR1A = ICR1 / 2;
}