// // Cecil // // Cecil has a sonar unit that can look ahead and determine the distance to an object. // If an object appears in front of him, he leans towards it, as if curious. // If the object gets closer than about a foot, it moves backwards, as if to keep some personal space. // #include Servo head; enum { TRIGGERPIN = P2_2, ECHOPIN = P2_1 }; enum { START_ANGLE = 90, MAX_ANGLE=150, MIN_ANGLE=20, NEAR=30, FAR=80 }; enum { FORWARD_STEP=2, BACK_STEP=2 }; enum { NUM_VALUES=8, NOISE=2 }; unsigned long last_time = 0; enum { WAIT_TIME=8 }; unsigned int get_distance( void ) { 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; } unsigned int angle = START_ANGLE; void setup( void ) { delay( 5000 ); Serial.begin( 9600 ); delay( 1000 ); Serial.println( "Cecil starting up" ); P1DIR = 0b11111110; P1OUT = 0; P2DIR = 0b1101; P2OUT = 0b1101; // P2_0 is servo output, P2_1 is echo input, P2_2 is ping trigger, P2_3 is the speaker head.attach( P2_0 ); head.write( angle ); } void back_up( void ) { if( angle < MAX_ANGLE ) angle += BACK_STEP; else return; Serial.print( "Back up" ); head.write( angle ); } void straighten_up( void ) { if( angle > START_ANGLE ) { angle--; Serial.print( "Straighten up forward (angle = " ); Serial.print( angle ); Serial.print( " )" ); head.write( angle ); } else if( angle < START_ANGLE ) { angle++; Serial.print( "Straighten up backward (angle = " ); Serial.print( angle ); Serial.print( " )" ); head.write( angle ); } } void lean_in( void ) { if( angle > MIN_ANGLE ) angle -= FORWARD_STEP; else return; Serial.print( "Lean in" ); head.write( angle ); } void loop( void ) { unsigned int dist = avg_distance(); Serial.print( "Distance is " ); Serial.print( dist ); Serial.print( " " ); if( dist > NEAR && dist < FAR ) { lean_in(); } else if( dist > FAR ) { straighten_up(); } else if( dist > 0 && dist < NEAR ) { back_up(); } Serial.println( "" ); }