const int numOfReadings = 10; // number of readings to take/ items in the array
int readings[numOfReadings]; // stores the distance readings in an array
int arrayIndex = 0; // arrayIndex of the current item in the array
int total = 0; // stores the cumlative total
int averageDistance = 0; // stores the average value
// setup pins and variables for SRF05 sonar device
int echoPin = 12; // SRF05 echo pin (digital 2)
int initPin = 13; // SRF05 trigger pin (digital 3)
unsigned long pulseTime = 0; // stores the pulse in Micro Seconds
unsigned long distance = 0; // variable for storing the distance (cm)
int motor1Pin1 = 3; // pin 2 on L293D
int motor1Pin2 = 4; // pin 7 on L293D
int enable1Pin = 9; // pin 1 on L293D
int motor2Pin1 = 5; // pin 10 on L293D
int motor2Pin2 = 6; // pin 15 on L293D
int enable2Pin = 10; // pin 9 on L293D
void setup() {
// set the motor pins as outputs:
pinMode(motor1Pin1, OUTPUT);
pinMode(motor1Pin2, OUTPUT);
pinMode(enable1Pin, OUTPUT);
pinMode(motor2Pin1, OUTPUT);
pinMode(motor2Pin2, OUTPUT);
pinMode(enable2Pin, OUTPUT);
// set enablePins high so that motor can turn on:
digitalWrite(enable1Pin, HIGH);
digitalWrite(enable2Pin, HIGH);
pinMode(initPin, OUTPUT); // set init pin 3 as output
pinMode(echoPin, INPUT); // set echo pin 2 as input
// create array loop to iterate over every item in the array
for (int thisReading = 0; thisReading < numOfReadings; thisReading++) {
readings[thisReading] = 0;
}
}
void loop() {
digitalWrite(initPin, HIGH); // send 10 microsecond pulse
delayMicroseconds(10); // wait 10 microseconds before turning off
digitalWrite(initPin, LOW); // stop sending the pulse
pulseTime = pulseIn(echoPin, HIGH); // Look for a return pulse, it should be high as the pulse goes low-high-low
distance = pulseTime/58; // Distance = pulse time / 58 to convert to cm.
total= total - readings[arrayIndex]; // subtract the last distance
readings[arrayIndex] = distance; // add distance reading to array
total= total + readings[arrayIndex]; // add the reading to the total
arrayIndex = arrayIndex + 1; // go to the next item in the array
// At the end of the array (10 items) then start again
if (arrayIndex >= numOfReadings) {
arrayIndex = 0;
}
averageDistance = total / numOfReadings; // calculate the average distance
delay(10);
// check the average distance and move accordingly
if (averageDistance <= 10){
// go backwards
digitalWrite(motor1Pin1, HIGH);
digitalWrite(motor1Pin2, LOW);
digitalWrite(motor2Pin1, HIGH);
digitalWrite(motor2Pin2, LOW);
}
if (averageDistance <= 25 && averageDistance > 10) {
// turn
digitalWrite(motor1Pin1, HIGH);
digitalWrite(motor1Pin2, LOW);
digitalWrite(motor2Pin1, LOW);
digitalWrite(motor2Pin2, HIGH);
}
if (averageDistance > 25) {
// go forward
digitalWrite(motor1Pin1, LOW);
digitalWrite(motor1Pin2, HIGH);
digitalWrite(motor2Pin1, LOW);
digitalWrite(motor2Pin2, HIGH);
}
}
