I made an autonomous robot with 3 ultrasonic sensors. I want to reduce the noise between the 3 sensors and make it gradually slow when it approaches an obstacle. My code is mentioned below. Please help me in this.
- Distance 1 is front sensor
- Distance 2 is left sensor
- Distance 3 is right sensor.
If you can suggest any improvements to this code, I would be grateful.
//CODE FOR AUTONOMOUS WITH 3 ULTRASONIC SENSORS
/*code written By
Faraz Hassan
For Autonomous Rover Obstacle Avoidance */
//include newping and adafruit libraries to the code
#include <NewPing.h>
#include <AFMotor.h>
AF_DCMotor motor1(1);
AF_DCMotor motor2(2);
AF_DCMotor motor3(3);
AF_DCMotor motor4(4);
int trigger_c = A0; // Controls the pulse sent from the right sensor
int echo_c = A1; // Controls the pulse received from the right sensor
int trigger_a = A2; // Controls the pulse sent from the left sensor
int echo_a = A3; // Controls the pulse received from the left sensor
int trigger_b = A4; // Controls the pulse sent from the front sensor
int echo_b = A5; // Controls the pulse received from the front sensor
int tp = 250;//delay
long duration_b,duration_a, duration_c, distance1, distance2, distance3;
void setup()
{
// set all the motor control pins to outputs
Serial.begin(9600);
pinMode(trigger_b, OUTPUT); // Arduino signal output from trigger_front
pinMode(echo_b, INPUT); // Arduino signal input from echo_front
pinMode(trigger_a, OUTPUT); //Arduino signal output from trigger_left
pinMode(echo_a, INPUT); // Arduino signal input from echo_left
pinMode(trigger_c, OUTPUT); // Arduino signal output from trigger_right
pinMode(echo_c, INPUT); //Arduino signal input from echo_right
}
void loop()
{
// Find distance Sonar….
// Clears the trigPin
digitalWrite(trigger_b, LOW); //Sends a 2 µs LOW signal to the trigPin to make sure it’s turned off at the beginning of the program loop.
delayMicroseconds(2);
// Sets the trigPin on HIGH state for 10 micro seconds
digitalWrite(trigger_b, HIGH); // Sends a 10 µs HIGH signal to the trigPin to initiate the sequence of eight 40 KHz ultrasonic pulses sent from the transmitting transducer.
delayMicroseconds(10);
digitalWrite(trigger_b, LOW);
// Reads the echoPin, returns the sound wave travel time in microseconds
duration_b= pulseIn(echo_b, HIGH); //Defines the duration variable as the length (in µs) of any HIGH input
//signal detected at the echoPin.
//The Echo pin output = time it takes the trigger pin emitted ultrasonic pulse to travel to the object and back to the sensor.
// Calculating the distance
distance1= duration_b*0.034/2; // Defines the distance variable as the duration (time in d = s x t) multiplied by the speed of sound converted from meters per second to centimeters per µs (0.0344 cm/µs).
//(d=s*t) * speed of sound converted from meters per second to centimeters per µs (0.0344 cm/µs).
// Prints the distance on the Serial Monitor
Serial.print("front: ");
Serial.print(distance1);
Serial.print(" ");
Serial.print("left: ");
Serial.print(distance2);
Serial.print(" ");
Serial.print("right: ");
Serial.print(distance3);
Serial.println(" ");
// Find distance Sonar2….
// Clears the trigPin
digitalWrite(trigger_a, LOW);
delayMicroseconds(2);
// Sets the trigPin on HIGH state for 10 micro seconds
digitalWrite(trigger_a, HIGH);
delayMicroseconds(10);
digitalWrite(trigger_a, LOW);
// Reads the echoPin, returns the sound wave travel time in microseconds
duration_a = pulseIn(echo_a, HIGH);
// Calculating the distance
distance2= duration_a*0.034/2;
// Prints the distance on the Serial Monitor
digitalWrite(trigger_c, LOW);
delayMicroseconds(2);
// Sets the trigPin on HIGH state for 10 micro seconds
digitalWrite(trigger_c, HIGH);
delayMicroseconds(10);
digitalWrite(trigger_c, LOW);
// Reads the echoPin, returns the sound wave travel time in microseconds
duration_c = pulseIn(echo_c, HIGH);
// Calculating the distance
distance3= duration_c*0.034/2;
motor1.setSpeed(255);
motor2.setSpeed(255);
motor3.setSpeed(255);
motor4.setSpeed(255);
motor1.run(FORWARD);
motor2.run(FORWARD);
motor3.run(FORWARD);
motor4.run(FORWARD);
if(Serial.available()>0)//Checking is Serial data available
{
int input = Serial.read(); //Storing value of read data into variable assigned
// switch(input)
// {
// case 'o' : forward(); //Calling respective functions if mathced with case label
// break;
//
// case 'x' : stopMotors();
// break;
// default : break;
// }
}
if (distance1 < 28|| distance2 < 28|| distance3 < 28) //DISTANCE1 FRONT SENSOR DISTANCE2 LEFT SENSOR DISTANCE 3 RIGHT SENSOR
{ stopMotors();
delay (1000);
if (distance2 < distance3) // LEFT SENSOR DISTANCE < RIGHT SENSOR DISTANCE
{ stopMotors();
delay (1000);
turnRight();
delay (700);
revers();
delay(700);
}
if (distance2 > distance3) // LEFT SENSOR DISTANCE > RIGHT SENSOR DISTANCE
{ stopMotors();
delay (1000);
turnLeft();
delay (700);
revers();
delay(700);
}
if (distance2 == distance3) // LEFT SENSOR DISTANCE == RIGHT SENSOR DISTANCE
{ revers();
delay (1000);
}
}
if (distance1 > 28)
forward(); // FRONT SENSOR DISTANCE > 25
}
void revers()
{
// turn on motor A
Serial.println("<Mars Rover> Backward");
motor1.run(BACKWARD);
motor2.run(BACKWARD);
motor3.run(BACKWARD);
motor4.run(BACKWARD);
motor1.setSpeed(190);
motor2.setSpeed(190);
motor3.setSpeed(190);
motor4.setSpeed(190);
delay(tp);
}
void forward()
{
Serial.println("<Mars Rover> Forward");
motor1.setSpeed(255);
motor2.setSpeed(255);
motor3.setSpeed(255);
motor4.setSpeed(255);
motor1.run(FORWARD);
motor2.run(FORWARD);
motor3.run(FORWARD);
motor4.run(FORWARD);
delay(tp);
}
void turnRight()
{
Serial.println("<Mars Rover> Right");
motor1.setSpeed(255);
motor2.setSpeed(255);
motor3.setSpeed(255);
motor4.setSpeed(255);
motor1.run(FORWARD);
motor2.run(FORWARD);
motor3.run(BACKWARD);
motor4.run(BACKWARD);
// Turn period
delay(tp);
}
void turnLeft()
{ Serial.println("<Mars Rover> Left");
motor1.setSpeed(255);
motor2.setSpeed(255);
motor3.setSpeed(255);
motor4.setSpeed(255);
motor1.run(BACKWARD);
motor2.run(BACKWARD);
motor3.run(FORWARD);
motor4.run(FORWARD);
//Turn period
delay(tp);
}
void stopMotors()
{
// Stop motors
Serial.println("<Mars Rover> Stop");
motor1.setSpeed(0);
motor2.setSpeed(0);
motor3.setSpeed(0);
motor4.setSpeed(0);
delay(tp);
}
