// // We use a sonar unit that can look ahead and determine the distance to an object. // enum { TRIGGERPIN = P2_2, ECHOPIN = P2_1 }; enum { NUM_VALUES=32, NOISE=8 }; unsigned long last_time = 0; enum { WAIT_TIME=8 }; unsigned int get_distance( void ) { // // There is a limit to how quickly we can do the next reading. // The spec says 50 milliseconds, but I find I can get by on about 8 before the readings get too variable. // That may depend on manufacturing tolerances on individual units, so you may want to play with WAIT_TIME. // unsigned long int now = millis(); if( now < last_time + WAIT_TIME ) delay( WAIT_TIME - (now - last_time) ); digitalWrite( TRIGGERPIN, LOW ); delayMicroseconds( 2 ); digitalWrite( TRIGGERPIN, HIGH ); delayMicroseconds( 10 ); digitalWrite( TRIGGERPIN, LOW ); unsigned int duration = pulseIn( ECHOPIN, HIGH, 38000 ); // 38 milliseconds if no obstacle unsigned int distance = duration / 58.77; if( distance < 0 ) distance = 0; if( distance > 500 ) // The HC-SR04 has a 5 meter range distance = 500; last_time = millis(); return distance; } int distances[ NUM_VALUES ]; void sort( int *list, int left, int right ) { int i = left; int j = right; int pivot = list[ (left + right) / 2 ]; while( i <= j ) { while( list[ i ] < pivot ) i++; while( list[ j ] > pivot ) j--; if( i <= j ) { unsigned int tmp = list[ i ]; list[ i++ ] = list[ j ]; list[ j-- ] = tmp; } } if( left < j ) sort( list, left, j ); if( i < right ) sort( list, i, right ); } unsigned int avg_distance( void ) { for( unsigned int x = 0; x < NUM_VALUES; x++ ) distances[ x ] = get_distance(); sort( distances, 0, NUM_VALUES-1 ); unsigned int dist = 0; for( unsigned int x = NOISE; x < NUM_VALUES-NOISE; x++ ) dist += distances[ x ]; dist /= NUM_VALUES - (2*NOISE); return dist; } enum { BEEP_LENGTH = 100000 }; // // Frequency in Hz // void beep( unsigned int frequency ) { // // We can't use the timer because the Servo class has the interrupt handler for it. // unsigned int period = 1000000UL / frequency; for( unsigned int i = 0; i < BEEP_LENGTH / period; i++ ) { P2OUT ^= 0b1000; delayMicroseconds( period ); } } void setup( void ) { delay( 5000 ); Serial.begin( 9600 ); delay( 1000 ); Serial.println( "Sonar starting up" ); P2DIR = 0b1100; P2OUT = 0b1100; // P2_1 is echo input, P2_2 is ping trigger, P2_3 is the speaker } void loop( void ) { unsigned int dist = avg_distance(); Serial.print( "Distance is " ); Serial.println( dist ); if( dist > 0 && dist < 100 ) beep( 3000 - dist * 20 ); }