int ping_ultrasonic_median(int echo, int trig, int max_distance, int iterations)
{
int flight_time[iterations];
int i;
int start_time;
for(i = 0; i < iterations; i++){
flight_time[i] = ping_ultrasonic(echo, trig, max_distance);
start_time = ReadCoreTimer();
while(ReadCoreTimer() - start_time < 4000000){//i think this is 50 ms
}
}
return find_median(flight_time, iterations);
}
/**
* Top-level function to initialize the ultrasonic sensors
*/
void init_ultrasonic()
{
init_ultrasonic_struct(&usensors[ULTRASONIC_LEFT],
&ULTRASONIC_LEFT_PORT,
ULTRASONIC_LEFT_TRIG,
ULTRASONIC_LEFT_ECHO,
ULTRASONIC_LEFT_CHMUX);
init_ultrasonic_struct(&usensors[ULTRASONIC_FRONT],
&ULTRASONIC_FRONT_PORT,
ULTRASONIC_FRONT_TRIG,
ULTRASONIC_FRONT_ECHO,
ULTRASONIC_FRONT_CHMUX);
init_ultrasonic_struct(&usensors[ULTRASONIC_BOTTOM],
&ULTRASONIC_BOTTOM_PORT,
ULTRASONIC_BOTTOM_TRIG,
ULTRASONIC_BOTTOM_ECHO,
ULTRASONIC_BOTTOM_CHMUX);
init_ultrasonic_struct(&usensors[ULTRASONIC_RIGHT],
&ULTRASONIC_RIGHT_PORT,
ULTRASONIC_RIGHT_TRIG,
ULTRASONIC_RIGHT_ECHO,
ULTRASONIC_RIGHT_CHMUX);
init_ultrasonic_struct(&usensors[ULTRASONIC_BACK],
&ULTRASONIC_BACK_PORT,
ULTRASONIC_BACK_TRIG,
ULTRASONIC_BACK_ECHO,
ULTRASONIC_BACK_CHMUX);
/* Initialize ultrasonic timer */
ULTRASONIC_TIMER.CTRLB = TC0_CCAEN_bm | TC_WGMODE_NORMAL_gc;
ULTRASONIC_TIMER.CTRLD = TC_EVACT_PW_gc | ULTRASONIC_EVSEL; // Connect event channel 4 to timer
ULTRASONIC_TIMER.INTCTRLA = TC_OVFINTLVL_MED_gc;
ULTRASONIC_TIMER.INTCTRLB = TC_CCAINTLVL_MED_gc;
ULTRASONIC_TIMER.PER = ULTRASONIC_TIMEOUT;
ULTRASONIC_TIMER.CTRLA = TC_CLKSEL_DIV64_gc;
/* Enable medium priority interrupts */
PMIC.CTRL |= PMIC_MEDLVLEN_bm;
sei();
/* Trigger first sensor measurement */
ping_ultrasonic();
}
