Issues with servo-mounted ultrasonic sensor on autonomous car - SOLVED

Projects General Guidance
1

I am working on an autonomous RC car (Autonomous RC car guided by GPS coordinates - project thread - #6 by system - Project Guidance - Arduino Forum). For obstacle avoidance, I am using a PING ultrasonic sensor mounted on a servo that sweeps back and forth. While I can get the servo and the sensor to work fine independently, I can't get them to work together. I think it is due to a problem with my wiring. Any ideas on how to solve it?

Here is the wiring used in all cases.

Alone, the servo works.

#include <Servo.h> 
Servo sensingServo;
int pos = 1300;

void setup() 
{
  Serial.begin(9600);
  sensingServo.attach(5);
}

void loop() {
//SWEEP
// results in roughly one over and back pass each second 
// at a resolution of 36 measurements for each pass
 
  for(pos = 800; pos < 1700; pos += 25)
  {
    sensingServo.writeMicroseconds(pos);
    Serial.print("Position:  ");
    Serial.print(pos);    
    Serial.print("\n");
  } 
  for(pos = 1700; pos > 800; pos -= 25)
  {                                
    sensingServo.writeMicroseconds(pos);
    Serial.print("Position:  ");
    Serial.print(pos);    
    Serial.print("\n");
  }   
}

Alone, the sensor also works.

#include <Servo.h> 
// setup ultrasonic distance sensor
const int pingPin = 2;
unsigned int duration, distance;

void setup() {
  Serial.begin(9600);
}

void loop() {
    // read and print ultrasonic sensor distance
    pinMode(pingPin, OUTPUT);
    digitalWrite(pingPin, LOW);
    delayMicroseconds(2);
    digitalWrite(pingPin, HIGH);
    delayMicroseconds(5);
    digitalWrite(pingPin, LOW);
    pinMode(pingPin, INPUT);
    duration = pulseIn(pingPin, HIGH);
    distance = duration / 74 / 2;    
    
    Serial.print("  Distance:  ");
    Serial.print(distance);
    Serial.print("\n");
}

But, when I try to put them together, the servo sweeps erratically and the sensor outputs bad data. Any ideas about how to fix the issue?

#include <Servo.h> 
Servo sensingServo;
int pos = 1300;

// setup ultrasonic distance sensor
const int pingPin = 2;
unsigned int duration, distance;

void setup() 
{
  Serial.begin(9600);
  sensingServo.attach(5);
}

void loop() {
  for(pos = 800; pos < 1700; pos += 25)
  {
    sensingServo.writeMicroseconds(pos);
    // read and print ultrasonic sensor distance
    pinMode(pingPin, OUTPUT);
    digitalWrite(pingPin, LOW);
    delayMicroseconds(2);
    digitalWrite(pingPin, HIGH);
    delayMicroseconds(5);
    digitalWrite(pingPin, LOW);
    pinMode(pingPin, INPUT);
    duration = pulseIn(pingPin, HIGH);
    distance = duration / 74 / 2;    
    
    Serial.print("Position:  ");
    Serial.print(pos);    
    Serial.print("  Distance:  ");
    Serial.print(distance);
    Serial.print("\n");
  } 
  for(pos = 1700; pos > 800; pos -= 25)
  {                                
    sensingServo.writeMicroseconds(pos);
    // read and print ultrasonic sensor distance
    pinMode(pingPin, OUTPUT);
    digitalWrite(pingPin, LOW);
    delayMicroseconds(2);
    digitalWrite(pingPin, HIGH);
    delayMicroseconds(5);
    digitalWrite(pingPin, LOW);
    pinMode(pingPin, INPUT);
    duration = pulseIn(pingPin, HIGH);
    distance = duration / 74 / 2;    
    
    Serial.print("Position:  ");
    Serial.print(pos);    
    Serial.print("  Distance:  ");
    Serial.print(distance);
    Serial.print("\n");
  }   
}

Thanks for your help.

2

Hi,
Read the two links in my signature, it's quite likely a power issue, the second link has a video showing the problem, the first link presents some solutions.

Duane B

rcarduino.blogspot.com

3

You move the servo a bit, then you take a ping, which takes some time, then you Serial.print() about 30 characters at 9600 baud. Then you move the servo again. Notice how the stutters coincide with the printing on screen. Try increasing your baud rate.

4

dxw00d:
You move the servo a bit, then you take a ping, which takes some time, then you Serial.print() about 30 characters at 9600 baud. Then you move the servo again. Notice how the stutters coincide with the printing on screen. Try increasing your baud rate.

Thanks. I tried all of the baud rates listed (14400, 19200, 28800, 38400, 57600, or 115200 from Serial.begin() - Arduino Reference), but the when I tried to print anything, I got garbled text in serial monitor:

While the choppy servo motion is a little annoying, my top priority is getting accurate data from the sensor.

5

That's because you didn't change the speed in the serial console to match. See the 9600 in the drop down, bottom-right.

6

DuaneB:
Hi,
Read the two links in my signature, it's quite likely a power issue, the second link has a video showing the problem, the first link presents some solutions.

Duane B

rcarduino.blogspot.com

Right you were. I powered the servo with three AA batteries and it worked like a charm. Thanks for the help.

7

dxw00d:
That's because you didn't change the speed in the serial console to match. See the 9600 in the drop down, bottom-right.

Thanks. I didn't realize I had to do that.

8

Another issue I'm running into is being able to read multi-dimensional arrays quickly. To avoid obstacles, I use the servo position and the distance measured by the sensor to populate an array of (angle, distance) ordered pairs. I plan to read from this array when choosing the best path for the vehicle. The contents of the array should also be able to be read quickly so that the car can rapidly respond to its surroundings. The problem I'm having is with getting the array to be read quickly. It takes about a second to read the contents of a 2 column, 37 row array. How should I revise my code such that the array can be read faster?

Notice the pause that corresponds to reading the array when the servo is about to change direction:

The last ditch solution is to take less measurements, which would hopefully speed up the rate of array reading. But, the cost of this is a lower resolution map of the vehicle's surroundings. Alternatively, I could have the sensor read distances over a narrow range of angles. But I would also like to avoid this. What other solutions can you think of?

// setup ultrasonic distance sensor
const int pingPin = 2;
unsigned int duration, distance;

// setup servo under sensor
#include <Servo.h> 
Servo sensingServo;
int pos = 1300;
int sensingServoMin = 800;
int sensingServoMax = 1700;
int sensingServoResolution = 25;
boolean sweepLeft = true;

// setup array for storing directions and distances
// array length = ((sensingServoMax - sensingServoMin) / sensingServoResolution) + 1;
int directionsAndDistances[37][2];
int i;

void writeArray()
{
  // move servo into position
  sensingServo.writeMicroseconds(pos);
  
  // read ultrasonic sensor
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);
  distance = duration / 74 / 2; 
    
  // write position and distance to array
  directionsAndDistances[i][0] = pos;
  directionsAndDistances[i][1] = distance;
  
  i++;
  delay(10);
}

void setup() 
{
  Serial.begin(9600);
  sensingServo.attach(5);  
}

void loop() { 
  i=0;  
  
  if (sweepLeft)
  {
    // sweep left
    for(pos = sensingServoMin; pos <= sensingServoMax; pos += sensingServoResolution)
    {
      writeArray();
    }
    sweepLeft = false;
  }
  
  else
  {
    // sweep right
    for(pos = sensingServoMax; pos >= sensingServoMin; pos -= sensingServoResolution)
    {                                
      writeArray();
    }
    sweepLeft = true;
  }
  
  // print contents of position and distance array
  for(int j = 0; j < 37; j += 1)
  {
    Serial.print("Position:  ");
    Serial.print(directionsAndDistances[j][0]);    
    Serial.print("  Distance:  ");
    Serial.print(directionsAndDistances[j][1]);
    Serial.print("\n");   
  }
}
9

JeffsInventions:
It takes about a second to read the contents of a 2 column, 37 row array. How should I revise my code such that the array can be read faster?

You could increase the baud rate.

You are transmitting:
"Position: 0 Distance: 0" which is 28 characters (new line), 37 times. Which is a total of 1036 characters. Each character is 8 bits long. Your speed is 9600 bits per second. So (1036*8)/9600 = 0.863 seconds... Plus overheads.

10

I was about to say that. Each line is around 35 characters, times 37 lines, times 1/960 for each byte, which is:

 35 * 37 * 1/960 = 1.348 seconds

If you increase the baud rate to 115200 you get:

35 * 37 * 1/11520 = 0.1124 seconds

Do you need to display the information at all? If so, maybe be a little less wordy about it.

11

Separate the reading of the sensor from the reading of the array.
The sensor has inherent delay; you even add 10 ms for each sample.

Have one loop continuously read and refresh your array. Making it [2][37] instead of [37][2] might increase speed (compiler can use smaller increments).
Also don't include a subroutine call in the loop.

Then, when and if you need to get the current contents of the array, send a message with no words, just a series of range numbers.
No word "Distance", Just 37 numbers ( 16 18 20 20 24 60 60 ........ )

Most important don't transmit the position info. No word position and position numbers. It's fixed and precalculated anyway. It doesn't change.

For debug mode only you can have a verbose setting that adds the position info.

12

You could increase the baud rate.

I told him to do that in his last thread: http://arduino.cc/forum/index.php/topic,102369.0.html

13

Oh I see:

I am working on an autonomous RC car (Autonomous RC car guided by GPS coordinates - project thread - #6 by system - Project Guidance - Arduino Forum). For obstacle avoidance, I am using a PING ultrasonic sensor mounted on a servo that sweeps back and forth.

That looks eerily familiar to this thread.

@JeffsInventions - please don't start new threads with exactly the same words in the hope of getting extra advice. It is just confusing to people, when some read one thread and some the other. At the very least, post a link to the earlier thread.

I feel a strong urge to merge here ...

... done! Threads merged.

14

Nick Gammon, dxw00d, and James C4S:
Thanks. Increasing the baud rate made it go much faster.

Nick Gammon:
You're right to say that in the long run, I won't need to display the information.

Sorry about the multiple threads--I made two because while both of my questions (1-how to avoid bad sensor data from the ping while using a servo and 2-how to speed up reading an array of sensor data) were about the same project, I thought they different enough issues to warrant separate threads. I'll keep similar questions in a single thread in the future.

Techylah: thanks for the optimization ideas

Techylah:
Then, when and if you need to get the current contents of the array, send a message with no words, just a series of range numbers.
No word "Distance", Just 37 numbers ( 16 18 20 20 24 60 60 ........ )

Most important don't transmit the position info. No word position and position numbers. It's fixed and precalculated anyway. It doesn't change.

For debug mode only you can have a verbose setting that adds the position info.

Makes sense.

Techylah:
The sensor has inherent delay; you even add 10 ms for each sample.

I experimented with reducing the 10ms delay, but found that it resulted in bad data.

Techylah:
Separate the reading of the sensor from the reading of the array.
Have one loop continuously read and refresh your array.

I didn't understand these suggestions. Could you elaborate?

Techylah:
Making it [2][37] instead of [37][2] might increase speed (compiler can use smaller increments).
Also don't include a subroutine call in the loop.

Could you describe in more detail why these slow it down?

15

"Making it [2][37] instead of [37][2] might increase speed (compiler can use smaller increments).
Also don't include a subroutine call in the loop."

Could you describe in more detail why these slow it down?

If it's set up the first way, the compiler can keep the address in a CPU register. After writing the data to that address it then increment the register by 1 or 4 (for byte or int arrays). These increment register instructions are typically built in to the instruction set. Doing it the second way, the compiler must insert either an "add 37" instruction or an "Add 148" instruction to get to the next address.
Also the overhead for calling a subroutine, especially if it has arguments, is more than not having a subroutine call at all. Many compilers have a "inline function" system to let you appear to write a function, but have the code for it inserted in your flow as if it weren't a function or subroutine. That takes up more space but is faster.

These are really for squeezing the most speed out of a tight routine. In this case it is the printing of needless words and numbers that is taking the vast majority of the time.

16

Techylah:
"Making it [2][37] instead of [37][2] might increase speed (compiler can use smaller increments).
Also don't include a subroutine call in the loop."

Could you describe in more detail why these slow it down?

If it's set up the first way, the compiler can keep the address in a CPU register. After writing the data to that address it then increment the register by 1 or 4 (for byte or int arrays). These increment register instructions are typically built in to the instruction set. Doing it the second way, the compiler must insert either an "add 37" instruction or an "Add 148" instruction to get to the next address.
Also the overhead for calling a subroutine, especially if it has arguments, is more than not having a subroutine call at all. Many compilers have a "inline function" system to let you appear to write a function, but have the code for it inserted in your flow as if it weren't a function or subroutine. That takes up more space but is faster.

These are really for squeezing the most speed out of a tight routine. In this case it is the printing of needless words and numbers that is taking the vast majority of the time.

Thanks for the explanation. You were right about the extra text bogging it down. Now it only prints out the direction the car would go (represented as a pulse width from 800 us to 1700 us, where 800 us is hard right and 1700 us is hard left). Even this will be unnecessary in the final version of the vehicle. I found the other measures you suggested interesting and I will keep them in mind when looking for ways to optimize code in future projects (maybe this one if I find the sensing is too sluggish when I take the car out for a drive).

Now, the car can choose the shortest of 36 obstacle free paths towards a waypoint in under a second. In this example, the way point is in the direction that corresponds to a pulse width of 1400 us.

boolean debug = false;

// setup ultrasonic distance sensor
const int pingPin = 2;
unsigned int duration, distance;
int sensorWait = 10;

// setup servo under sensor
#include <Servo.h> 
Servo sensingServo;
int pos = 1300;
int sensingServoMin = 800;
int sensingServoMax = 1700;
int sensingServoResolution = 25;
boolean sweepLeft = true;

// setup array for storing directions and distances
// array length = ((sensingServoMax - sensingServoMin) / sensingServoResolution) + 1;
int directionsAndDistances[37][2];
int directionsAndDistancesLength = 37;
int i;

// setup arrray for choosing direction
int obstacleFreeDirections[37][3];
int minimumDistance = 48;
int waypointDirection = 1400;

void readSensor()
{
  // move servo into position
  sensingServo.writeMicroseconds(pos);
  
  // read ultrasonic sensor
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);
  distance = duration / 74 / 2; 

  // write position and distance to array
  directionsAndDistances[i][0] = pos;
  directionsAndDistances[i][1] = distance;  
}

int chooseDirection()
{
  if(debug)
  {
    Serial.println("Directions and distances");
  
    for (int i = 0; i < directionsAndDistancesLength; i++)
    {
    Serial.print(directionsAndDistances[i][0]);
    Serial.print(",");
    Serial.print(directionsAndDistances[i][1]);
    Serial.print("\n");      
    }
  }  
  
  // filter out directions with obstacles
  for(int i,j = 0; i < directionsAndDistancesLength; i++)
  {
    if (directionsAndDistances[i][1] >= minimumDistance)
    { 
    obstacleFreeDirections[j][0] = directionsAndDistances[i][0];
    obstacleFreeDirections[j][1] = directionsAndDistances[i][1];
    }
    else
    {
      obstacleFreeDirections[j][0] = 0;
      obstacleFreeDirections[j][1] = 0;     
    }
    obstacleFreeDirections[j][2] = 0;    
    j++;
  }
  
  if(debug)
  {
    Serial.println("Obstacle free directions");
    
    for (int i = 0; i < directionsAndDistancesLength; i++)
    {
      Serial.print(obstacleFreeDirections[i][0]);
      Serial.print(",");
      Serial.print(obstacleFreeDirections[i][1]);
      Serial.print(",");
      Serial.print(obstacleFreeDirections[i][2]);    
      Serial.print("\n");               
    }    
  }
 
  // calculate the discrepancy between the waypoint direction and each direction
  for(int i = 0; i < directionsAndDistancesLength; i++)
  {
    if(obstacleFreeDirections[i][0] != 0)
    {
    obstacleFreeDirections[i][2] = abs(waypointDirection - obstacleFreeDirections[i][0]);
    }
  }
  
  if(debug)
  {
    Serial.println("Discrepancies from way point");
  
    for (int i = 0; i < directionsAndDistancesLength; i++)
    {
    Serial.print(obstacleFreeDirections[i][0]);
    Serial.print(",");
    Serial.print(obstacleFreeDirections[i][1]);
    Serial.print(",");    
    Serial.print(obstacleFreeDirections[i][2]);    
    Serial.print("\n");
    }
  }
  
  // choose the direction closest to the waypoint direction with no obstacles

  // find the first obstacle free direction
  int solutionDirectionReference = 0; 
  while(obstacleFreeDirections[solutionDirectionReference][0] == 0)
  {
    solutionDirectionReference++;
  }
  
  // find the obstacle free direction closest to the waypoint direction
  for(int i = 0; i < directionsAndDistancesLength; i++)
  {
    if(obstacleFreeDirections[i][0] != 0 && obstacleFreeDirections[i][2] < obstacleFreeDirections[solutionDirectionReference][2])
    {
      solutionDirectionReference = i;
    }
  }

  if(obstacleFreeDirections[solutionDirectionReference][0] > 15000)
  {
    return 0;
  }
  else
  {
    return obstacleFreeDirections[solutionDirectionReference][0];
  }
}

void setup() 
{
  Serial.begin(115200);
  sensingServo.attach(5);  
}

void loop() { 
  if (sweepLeft)
  {
    // sweep left
    i = 0;
    for(pos = sensingServoMin; pos <= sensingServoMax; pos += sensingServoResolution)
    {
      readSensor();
      i++;
      delay(sensorWait);
    }
    sweepLeft = false;
  }
  
  else
  {
    // sweep right
    i = directionsAndDistancesLength - 1;

    for(pos = sensingServoMax; pos >= sensingServoMin; pos -= sensingServoResolution)
    {
      readSensor();
      i--;
      delay(sensorWait);
    }
    sweepLeft = true;
  }
  
  int chosenDirection = chooseDirection();
//  Serial.print("Direction chosen:  ");
  Serial.print(chosenDirection);
  Serial.print("\n");    
}