// // Rover // // Two gear-motors each with a wheel are controlled by two // pins each, so they can run forwards or backwards. // The rover is front-wheel drive, and drags a loop of // cable-tie as the rear support. // The computer and breadboard hide in a small cardboard box // to which the wheels are attached by a couple more // cable-ties. // #include Servo head; enum { TRIGGERPIN = P2_2, ECHOPIN = P2_1 }; // // The right motor and wheel are connected to P1.3 and P1.4. // If P1.3 is high and P1.4 is low, it moves forward. // The left motor and wheel are connected to P1.6 and P1.7. // If P1.6 is high and P1.7 is low, it moves forward. // By reversing which pin is high and which is low, a motor // runs backwards. // Motors stop if both their pins are in the same state. // We can turn by keeping one motor stopped while moving // the other, or we can spin by moving one forward and the // other backwards. // enum { RIGHT_FORWARD = 0b00001000, LEFT_FORWARD = 0b01000000, RIGHT_BACKWARD = 0b00010000, LEFT_BACKWARD = 0b10000000 }; void forward( void ) { P1OUT = RIGHT_FORWARD | LEFT_FORWARD; } void reverse( void ) { P1OUT = RIGHT_BACKWARD | LEFT_BACKWARD; } void stop( void ) { P1OUT = 0; } void forward_right( void ) { P1OUT = LEFT_FORWARD; // To go forward and right, move only the left wheel forward } void forward_left( void ) { P1OUT = RIGHT_FORWARD; // To go forward and left, move only the right wheel forward } void spin_right( void ) { P1OUT = RIGHT_BACKWARD | LEFT_FORWARD; // left forward, right back } void spin_left( void ) { P1OUT = RIGHT_FORWARD | LEFT_BACKWARD; // right forward, left back } void reverse_right( void ) { P1OUT = LEFT_BACKWARD; // stop right wheel, left goes backwards } void reverse_left( void ) { P1OUT = RIGHT_BACKWARD; // stop left wheel, right goes backwards } typedef void (*function)( void ); function list[] = { forward, reverse, stop, forward_right, forward_left, spin_right, spin_left, reverse_right, reverse_left }; 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; } enum { NUM_VALUES=8, NOISE=2 }; int values[ 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++ ) values[ x ] = get_distance(); sort( values, 0, NUM_VALUES-1 ); unsigned int dist = 0; for( unsigned int x = NOISE; x < NUM_VALUES-NOISE; x++ ) dist += values[ x ]; dist /= NUM_VALUES - (2*NOISE); return dist; } unsigned int max_distance = 0; unsigned int min_distance = 0; unsigned int max_distance_angle = 90; unsigned int min_distance_angle = 90; // // Arrange start and step so that 90 is the value in the middle // enum { START_ANGLE = 42, END_ANGLE = 138, STEP = 4 }; unsigned distances[ (END_ANGLE - START_ANGLE) / STEP ]; unsigned int find_open_space( void ) { max_distance = 0; max_distance_angle = 90; min_distance = 9999; min_distance_angle = 90; unsigned int angle = 0; angle = START_ANGLE; head.write( angle ); delay( 300 ); // allow servo time to move for( unsigned int a = 0; a < (END_ANGLE-START_ANGLE) / STEP; a++ ) { head.write( angle ); angle += STEP; delay( STEP ); // allow servo time to move distances[a] = avg_distance(); } for( unsigned int a = 0; a < (END_ANGLE-START_ANGLE) / STEP; a++ ) { if( distances[a] < min_distance ) { min_distance = distances[a]; min_distance_angle = START_ANGLE + a * STEP; } if( distances[a] > max_distance ) { max_distance = distances[a]; max_distance_angle = START_ANGLE + a * STEP; } } // // If there is a lot of clear space in front, just go there. // if( distances[ 90 / STEP ] > 150 ) { max_distance = distances[ 90 / STEP ]; Serial.print( "90: distance is " ); Serial.println( distances[ 90 / STEP ] ); return max_distance_angle; } for( unsigned int a = 0; a < (END_ANGLE-START_ANGLE) / STEP; a++ ) { Serial.print( START_ANGLE + a * STEP ); Serial.print( ": distance is " ); Serial.println( distances[a] ); } return max_distance_angle; } enum { BEEP_LENGTH = 300 }; // // Frequency in Hz, duration in microseconds // void beep( unsigned int frequency, unsigned long duration ) { // // We can't use the timer because the Servo class has the interrupt handler for it. // unsigned int period = 1000000UL / frequency; unsigned int count = duration / period; for( unsigned int i = 0; i < count; i++ ) { P2OUT ^= 0b1000; delayMicroseconds( period ); } } // // 9500 is nice and loud on the tiny piezoelectric PC speaker // So are 8800 and 4500. // void uh_oh( void ) { Serial.println( "Uh oh!" ); beep( 8800, 100000L ); beep( 8850, 50000L ); beep( 8700, 300000L ); } void tee_hee_hee( void ) { Serial.println( "Tee hee hee!" ); beep( 4500, 100000L ); delay( 10 ); beep( 4480, 180000L ); delay( 30 ); beep( 4470, 300000L ); } void setup( void ) { Serial.begin( 9600 ); delay( 8000 ); Serial.println( "" ); Serial.println( "Rover 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 ); } enum { MARGIN = 10 }; void loop( void ) { unsigned int angle = find_open_space(); Serial.print( "Angle is " ); Serial.print( angle ); Serial.print( " Distance is " ); Serial.print( max_distance ); head.write( angle ); beep( 1500, 300000L ); head.write( 90 ); unsigned int dist = avg_distance(); if( dist < 30 ) { stop(); uh_oh(); Serial.println( " Reverse" ); reverse(); delay( 500 ); stop(); } else if( min_distance < 30 ) { if( min_distance_angle > 90 ) { Serial.println( " Spin left (min)" ); spin_left(); delay( 200 ); stop(); } else { Serial.println( " Spin right (min)" ); spin_right(); delay( 200 ); stop(); } } else if( angle >= 90 - MARGIN && angle <= 90 + MARGIN ) { Serial.println( " Forward" ); head.write( 90 ); for( int count = 0; count < 40; count++ ) { if( avg_distance() < 30 ) { stop(); tee_hee_hee(); break; } forward(); delay( 100 ); } stop(); } else if( angle > 90 + MARGIN ) { Serial.println( " Spin left" ); spin_left(); delay( 200 ); stop(); } else if( angle < 90 - MARGIN ) { Serial.println( " Spin right" ); spin_right(); delay( 200 ); stop(); } }