1
$\begingroup$

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);
}
$\endgroup$
2
$\begingroup$

There are two issues in the question:

  1. noise (interference) among the three sensors
  2. how to handle the motion with relation to the sensor readings

The noise between sensors is most probably a hardware issue (cabling or EMI) or it might be related to the sensors being close to each other. Without more info about the current implementation, this is all I can say at first thought. 

Coming to the motion algorithms:

The current distance to action decisions are based on distinct cases (for example the distances being smaller than a treshold) however gradual motion requires a proportional control mechanism. 

Proportional control can be applied to any sensed variable that you want to keep in control. Applying it to the distance variable, you could set up a speed - distance relationship, as follows:

d_min_allowed = 10 //the distance that you want the robot to completely stop
d_slowdown_start = 28 //the distance that you want the robot to start slowing down)
distance = (distance1+distance2+distance3)/3   // updated sensor readings

if (distance < d_slowdown_start) {
// start slowdown
error = (distance - d_min_allowed);
motorspeed = Kp*(error);
(Kp=255/ (d_slowdown_start-d_min_allowed) = 255 / 18)
}

Once you get this one running, setting up similar proportional control mechanisms for steering will be a joyful challenge.

For more information and background, you can check the following topics online: system control, proportional control, PID control.

$\endgroup$
  • $\begingroup$ @FarazHassan can you validate it as the answer, so that it will help others find it easily as well. $\endgroup$ – Gürkan Çetin Sep 25 '17 at 15:58

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.