Final version of the code:
// —————————————————————————
// Example NewPing library sketch that does a ping about 20 times per second.
// —————————————————————————
#include <Servo.h>
#include <NewPing.h>
NewPing sonar1(12, 10, 200); // Sensor 1: trigger pin, echo pin, maximum distance in cm
//NewPing sonar2(7, 6, 200); // Sensor 2: same stuff
const int numReadings = 18;
int fr_readings[numReadings]; // the readings from the analog input
int fr_index = 0; // the index of the current reading
int fr_total = 0; // the running total
int fr_average = 0; // the average
// initialize buffer
int bk_readings[numReadings]; // the readings from the analog input
int bk_index = 0; // the index of the current reading
int bk_total = 0; // the running total
int bk_average = 0; // the average
int sm_average = 0;
#define pingSpeed 100 // Ping frequency (in milliseconds), fastest we should ping is about 35ms per sensor
Servo LeftServo;
Servo RightServo;
int val_x; //front proximity sensor
int val_y; //rear proximity sensor
//NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
void setup() {
LeftServo.attach(13);
RightServo.attach(4);
//Serial.begin(9600); // Open serial monitor at 115200 baud to see ping results.
// initialize all the readings to 0:
for (int thisReading = 0; thisReading < numReadings; thisReading++)
fr_readings[thisReading] = 0;
//for (int thisReading = 0; thisReading < numReadings; thisReading++)
// bk_readings[thisReading] = 0;
RightServo.write(0);
LeftServo.write(0);
//delay(2000);
}
//void compareDistance() {
// Serial.println(fr_average);
// Serial.println(bk_average);
// Serial.print(“sm_average: “);
// Serial.println(sm_average);
// if (fr_average < bk_average) { //if someone is closer in the front
// sm_average = fr_average;
// }
// else {
// sm_average = bk_average;
//
// }
// }
void loop() {
delay(50); // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
unsigned int uS_front = sonar1.ping(); // Send ping, get ping time in microseconds (uS).
// unsigned int uS_back = sonar2.ping(); // Send ping, get ping time in microseconds (uS).
//Serial.print(“Ping: “);
// Serial.print(uS / US_ROUNDTRIP_CM); // Convert ping time to distance in cm and print result (0 = outside set distance range)
//Serial.println(“cm”);
val_x = (uS_front / US_ROUNDTRIP_CM) ;
Serial.print(“val_x2: “);
Serial.println(val_x);
val_x = map(val_x, 8 , 130, 0, 90);
val_x = abs(val_x);
// val_y = (uS_back / US_ROUNDTRIP_CM) ;
// val_y = map(val_y, 0, 200, 0, 90);
//Serial.print(“val_y: “);
//Serial.println(val_y);
// subtract the last reading:
//if (val_x
fr_total= fr_total – fr_readings[fr_index];
//bk_total= bk_total – bk_readings[bk_index];
// read from the sensor:
fr_readings[fr_index] = val_x;
//bk_readings[bk_index] = val_y;
// add the reading to the total:
fr_total= fr_total + fr_readings[fr_index];
// bk_total= bk_total + bk_readings[bk_index];
// advance to the next position in the array:
fr_index = fr_index + 1;
// bk_index = bk_index + 1;
// if we’re at the end of the array…
if (fr_index >= numReadings)
// …wrap around to the beginning:
fr_index = 0;
// if (bk_index >= numReadings)
// …wrap around to the beginning:
// bk_index = 0;
// calculate the average:
fr_average = fr_total / numReadings;
fr_average = abs(90-fr_average);
//bk_average = bk_total / numReadings;
// send it to the computer as ASCII digits
// Serial.print(“average: “);
//Serial.println(fr_average);
// compareDistance();
// sm_average = fr_average;
RightServo.write(fr_average);
LeftServo.write(fr_average);
// Serial.print(“Front average: “);
// Serial.println(fr_average); // Convert ping time to distance in cm and print result (0 = outside set distance range)
//Serial.print(“Back average: “);
//Serial.println(bk_average);
delay(50);
}