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(); }