I have made a project for a car that will drive through the hallways of my school. I have set up the code to use three sensors located at the front, left, and right of the car to monitor how close the car is to the walls. I an controlling the steering part of the car using a servo motor that is set correctly to turn in the right direction for adjusting the car into a better location with all the turn methods. When the front sensor receives a distance of less than 6 feet the car should turn completely. If the side sensors receive a distance less than 1 1/2 feet then the car should adjust back to the middle. The problem that I am currently facing is the sensors are turning when that are no objects within the threshold of the set dimensions. I followed the code similar to the example sketch on the New Ping library page. On the page the code handles out of bounds exceptions but for some reason the sensors I have are not handling that. Help would be immensely appreciated. Here is the code:
#include <NewPing.h>
#include <Servo.h>
#define SONAR_NUM 3 // Number or sensors
#define PING_INTERVAL 50 // Milliseconds between pings.
unsigned long pingTimer[SONAR_NUM]; // When each pings.
uint8_t currentSensor = 0; // Which sensor is active.
// Front sensor information
#define sensorPinFront 11
#define ledForFront 5
long durationFront;
long distanceFront;
// Left sensor information
#define sensorPinLeft 12
#define ledForLeft 6
long durationLeft;
long distanceLeft;
// Right Sensor Information
#define sensorPinRight 13
#define ledForRight 7
long durationRight;
long distanceRight;
// Boolean values for turning
bool sensorActivated;
//Servo controlling front turns with servo signal pin
Servo servo;
#define servoPin 2
//Sensor distance value array
int sonarDistances[SONAR_NUM];
int sonarTime[SONAR_NUM];
// Sensor object array.
NewPing sonar[SONAR_NUM] =
{
NewPing(sensorPinFront, sensorPinFront, 350),
NewPing(sensorPinLeft, sensorPinLeft, 350),
NewPing(sensorPinRight, sensorPinRight, 350)
};
void setup() {
Serial.begin(9600);
servo.attach(servoPin);
servo.write(90);
pingTimer[0] = millis() + 75; // First ping start in ms.
for (uint8_t i = 1; i < SONAR_NUM; i++)
{
pingTimer[i] = pingTimer[i - 1] + PING_INTERVAL;
}
}
void echoCheck() { // If ping echo, set distance to array.
if (sonar[currentSensor].check_timer()){
sonarTime[currentSensor] = sonar[currentSensor].ping_median(5, 350);
sonarDistances[currentSensor] = sonar[currentSensor].convert_in(sonarTime[currentSensor]);
}
}
void distanceChecks(){
distanceFront = sonarDistances[0];
distanceLeft = sonarDistances[1];
distanceRight = sonarDistances[2];
sensorActivated = false;
if (distanceFront < 72 && !sensorActivated) //84
{
sensorActivated = true;
digitalWrite(ledForFront, HIGH);
longRightTurn();
}
else
{
sensorActivated = false;
digitalWrite(ledForFront, LOW);
}
if (distanceFront >= 72 || distanceFront == 0 || distanceFront < 0)
{
// Out of range setting
sensorActivated = false;
digitalWrite(ledForFront, LOW);
}
if (distanceLeft < 18 && !sensorActivated) //18
{
sensorActivated = true;
digitalWrite(ledForLeft, HIGH);
shortRightTurn();
}
else
{
sensorActivated = false;
digitalWrite(ledForLeft, LOW);
}
if (distanceLeft >= 18 || distanceLeft == 0 || distanceLeft < 0)
{
// Out of range setting
sensorActivated = false;
digitalWrite(ledForLeft, LOW);
}
if (sonarDistances[2] < 18 && !sensorActivated) //18
{
sensorActivated = true;
digitalWrite(ledForRight, HIGH);
shortLeftTurn();
}
else
{
sensorActivated = false;
digitalWrite(ledForRight, LOW);
}
if (distanceRight >= 18 || distanceRight == 0 || distanceRight < 0)
{
// Out of range setting
sensorActivated = false;
digitalWrite(ledForRight, LOW);
}
}
void stayStraight(){
servo.write(90);
delay(500);
servo.write(66.25);
delay(500);
}
void shortLeftTurn(){
servo.write(180);
delay(2500);
servo.write(0);
delay(2500);
sensorActivated = false;
}
void shortRightTurn(){
servo.write(0);
delay(2500);
servo.write(180);
delay(2500);
sensorActivated = false;
}
void longRightTurn(){
servo.write(0);
delay(3000);
sensorActivated = false;
}
void loop() {
for (uint8_t i = 0; i < SONAR_NUM; i++) { // Loop through all the sensors
if (millis() >= pingTimer[i]) { // Is it this sensor's time to ping?
pingTimer[i] += PING_INTERVAL * SONAR_NUM; // Set next time this sensor will be pinged.
if (i == 0 && currentSensor == SONAR_NUM - 1) // Sensor ping cycle complete, do something with the results.
{
// Perform distance checks on the specified sensors
distanceChecks();
}
sonar[currentSensor].timer_stop(); // Make sure previous timer is canceled before starting a new ping (insurance).
currentSensor = i; // Sensor being accessed.
sonarDistances[currentSensor] = 0; // Make distance 0 in case there is no ping echo for the sensor
sonar[currentSensor].ping_timer(echoCheck); // Do the ping (processing continues, interrupt will call echoCheck to look for echo).
}
}
stayStraight();
}