Adding a Ping Sensor Servo

Projects General Guidance
1

I'm trying to add a servo to an obstacle avoiding robot's ping sensor. The robot's ping sensor is attached to a servo which I want to rotate 180 degrees back and forth, all the while avoiding obstacles with pings. I pieced together a code to avoid obstacles (which works) but when I add in a code for the servo the robot does not function properly. I am relatively new to programming so any help for this beginner would be great!

Here is the code without my ping sensor servo attempt:

#include <Servo.h>
Servo myservo;

int pwm_speed = 255;

int trig = 12;

int echo = 13;

int pos= 0;
void setup() {

myservo.attach(2);

pinMode(11, OUTPUT);
pinMode(10, OUTPUT);
pinMode(9, OUTPUT);
pinMode(6, OUTPUT);
pinMode(5, OUTPUT);
pinMode(3, OUTPUT);

pinMode(trig, OUTPUT);
pinMode(echo,INPUT);
}
void loop(){
digitalWrite(trig, LOW);
delayMicroseconds(2);
digitalWrite(trig, HIGH);
delayMicroseconds(5);
digitalWrite(trig, LOW);
int duration = pulseIn(echo, HIGH);
int distance = duration / 29 / 2;
if(distance > 10){
  
   forward(100);
}else if(distance < 10){
 
  backward(1000);
  
   left(1000);
   
}
}
void forward(int delay_time){
  digitalWrite(11, HIGH);
  digitalWrite(10, LOW);
  digitalWrite(9, HIGH);
  digitalWrite(6, LOW);
  analogWrite(5, pwm_speed);
  analogWrite(3, pwm_speed);
  delay(delay_time);
}

void backward(int delay_time){
  digitalWrite(11, LOW);
  digitalWrite(10, HIGH);
  digitalWrite(9, LOW);
  digitalWrite(6, HIGH);
  analogWrite(5, pwm_speed);
  analogWrite(3, pwm_speed);
  delay(delay_time);
}

void left(int delay_time){
  digitalWrite(11, HIGH);
  digitalWrite(10, LOW);
  digitalWrite(9, HIGH);
  digitalWrite(6, LOW);
  analogWrite(5, 0);
  analogWrite(3, pwm_speed);
  delay(delay_time);
}

void motors_stop(int delay_time){
  
  digitalWrite(11, LOW);
  digitalWrite(10, LOW);
  digitalWrite(9,LOW);
  digitalWrite(6, LOW);
  analogWrite(5, 0);
  analogWrite(3, 0);
  delay(delay_time);
}

And here is my unsuccessful attempt at adding the ping sensor servo:

#include <Servo.h>
Servo myservo;

int pwm_speed = 255;

int trig = 12;

int echo = 13;

int pos= 0;
void setup() {

myservo.attach(2);

pinMode(11, OUTPUT);
pinMode(10, OUTPUT);
pinMode(9, OUTPUT);
pinMode(6, OUTPUT);
pinMode(5, OUTPUT);
pinMode(3, OUTPUT);

pinMode(trig, OUTPUT);
pinMode(echo,INPUT);
}

void loop(){
digitalWrite(trig, LOW);
delayMicroseconds(2);
digitalWrite(trig, HIGH);
delayMicroseconds(5);
digitalWrite(trig, LOW);
int duration = pulseIn(echo, HIGH);
int distance = duration / 29 / 2;
if(distance > 10){
   
   forward(100);
}else if(distance < 10){
  
  backward(1000);
  
   left(1000);
   
}
for (pos = 0; pos <= 180; pos += 1) { 
                                      
    myservo.write(pos);              
    delay(15);                       
  }
  for (pos = 180; pos >= 0; pos -= 1) { 
    myservo.write(pos);              
    delay(15);                       
  }
}

void forward(int delay_time){
  digitalWrite(11, HIGH);
  digitalWrite(10, LOW);
  digitalWrite(9, HIGH);
  digitalWrite(6, LOW);
  analogWrite(5, pwm_speed);
  analogWrite(3, pwm_speed);
  delay(delay_time);
}

void backward(int delay_time){
  digitalWrite(11, LOW);
  digitalWrite(10, HIGH);
  digitalWrite(9, LOW);
  digitalWrite(6, HIGH);
  analogWrite(5, pwm_speed);
  analogWrite(3, pwm_speed);
  delay(delay_time);
}

void left(int delay_time){
  digitalWrite(11, HIGH);
  digitalWrite(10, LOW);
  digitalWrite(9, HIGH);
  digitalWrite(6, LOW);
  analogWrite(5, 0);
  analogWrite(3, pwm_speed);
  delay(delay_time);
}

void motors_stop(int delay_time){
  
  digitalWrite(11, LOW);
  digitalWrite(10, LOW);
  digitalWrite(9,LOW);
  digitalWrite(6, LOW);
  analogWrite(5, 0);
  analogWrite(3, 0);
  delay(delay_time);
}
2

That is because you are basically saying in your code:

1. Ping
2. Get distance
3. Based on distance do something
4. Sweep the ping sensor 180 degrees one way
5. Sweep the ping sensor 180 degrees the other way
6. Go to 1

When what you really want to do is:

1. Sweep sensor...for each position in the sweep:

    a. Ping
    b. Get distance for that position
    c. Store the distance (in an array)

2. When done with the sweep:

    a. Read thru the array
    b. Based on the readings you have, do something

3. Go to 1

In essence "do something" just means "drive forward" or "turn to the direction with less obstacles" (or whatever). Also - you'll probably want to set a variable or something to make the sensor sweep in one direction, then after it has made the decision to move (in whatever way it needs to), the variable is set in such a manner so that on the next sweep, it moves in the opposite direction.

Quick pseudo-code (hopefully this is clear enough):

// Initialize array of distance readings to zero (0)
Array(0...180) = 0

Angle = 0 // Initialize starting angle of servo
SweepDir = 1 // Direction to sweep in (positive or negative)
HitsOnLeft = 0 // Number of blockages on the "left side" of center
HitsOnRight = 0 // Number of blockages on the "right side" of center

Loop:

    SweepAndRead() // Rotate and read the sensor, and store the distance
    
    EvaluateArray() // Check the array to figure out relative blockage
    
    DecideMove() // Move based on left or right blockage

---

SweepAndRead():

    ServoAngle(Angle) // Set servo angle
    
    Distance = Ping() // Get the distance reading from the Ping sensor

    Array(Angle) = Distance // Store the angle in the array for later evaluation
        
    Angle += SweepDir // Change the angle of the servo
    
    // When at the angle limit, reverse sweep direction
    If Angle == 180 or Angle == 0:
      
        SweepDir = -SweepDir

---

EvaluateArray():

    HitsOnLeft = 0 // Initialize our hit counters
    HitsOnRight = 0
  
    // Check each angle reading distance
    ForEach CheckPos (0...180):

        // Is there an obstacle close by?
        If Array(CheckPos) < 50:
        
            // Is it to the left or right of center?
            If CheckPos < 90:
            
                HitsOnLeft++
               
            Else:
            
                HitsOnRight++   
                   
---

DecideMove():

    // If there are more hits on the left than the right, turn to the right
    If HitsOnLeft > HitsOnRight:
    
        TurnRight()
        
    // If there are more hits on the right than the left, turn to the left
    If HitsOnRight > HitsOnLeft:
    
        TurnLeft()
    
    // Otherwise, if no hits were found, keep moving forward!
    If HitsOnRight == 0 And HitsOnLeft == 0:
    
        MoveForward()
    
---

// Fule steam ahead!
MoveForward():

    MotorLeft(100)
    MotorRight(100)

---

TurnLeft:

    MotorLeft(-100)
    MotorRight(100)

---

TurnRight:

    MotorLeft(100)
    MotorRight(-100)

I can't guarantee that this "code" will work or is free of errors - but it should be good enough to show you where your mistake was, and how to correct it. Good luck!

:smiley: