Servo+Ultrasonic sensor wrong measures at some angles

Testing a pseudo-robot platform I encontered this problem; I reduced everything to a minimum HW and SW for this testing. The HC-SR04 is mounted over a SG90 servo to scan around for 180°.
When the HC-SR04 is turned by the servo, from 0° to 180°, it returns valid measures in the approximative range 0..85° and 105..180°. The front range from around 85 to 105 degrees returns a quasi constant wrong value of 791, regardless of any nearer obstacle.
This is a monitor print-out of the wrong measures:

-----------------------
88: 791
90: 791
92: 791
94: 792
96: 791
98: 791
100: 791
102: 791
104: 791
106: 792
108: 792
110: 792
112: 791
126: 791
124: 791
122: 791
118: 792
116: 791
114: 792
112: 791
110: 792
108: 792
106: 792
104: 792
102: 791
-----------------------
Errors: 25

Please note the if a take a 'manual' (without servo) reading at 90° (center) the results are perfect for any real distance. The problem manifests only when the HC is moved by the servo. I even tried with different (higher) delays after a rotation.
This is my simple testing code:

#include <Servo.h>
#define trigPin 12
#define echoPin 13
long duration;
int distance;
int rotDelay = 15;
int cntErrors;
Servo myservo;

void calculateDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.017;  //340mS / 1000 / 2
}

void setup() {
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  myservo.attach(10);
  Serial.begin(115200);
  myservo.write(0);
  delay(1000);
}

void loop() {
  int i;

  Serial.println("-----------------------");
  for (i =0; i <= 180; i += 2) {
    myservo.write(i);
    delay(rotDelay);
    calculateDistance();
    if (distance > 200) {  //should never happen
      cntErrors++;
      Serial.print(i);
      Serial.print(": ");
      Serial.println(distance);
    }
  }

  for (i = 180; i >= 0; i -= 2) {
    myservo.write(i);
    delay(rotDelay);
    calculateDistance();
    if (distance > 200) {
      cntErrors++;      
      Serial.print(i);
      Serial.print(": ");
      Serial.println(distance);
    }
  }
  Serial.println("-----------------------");
  Serial.print("Errors: ");
  Serial.println(cntErrors);
  while (1) { delay(1); };
}

Thanks

Please tell us your are NOT moving the sensor between generating the pulse and getting an echo back.

you found the noise making part, what do you want to do with it?

I always allow about 50ms between range finder triggers to allow the echo from previous pings to subside.

The sensing area of the HCSR04 is about 30 degrees wide. Pinging every 2 degrees seems excessive.

You are right, but the 2 degrees was set only to determine the valid/invalid range.

I expanded my test sketch to accept commands and values from the serial and try different parameters.
At the end, my best and positive results, tested with stepping of 15 and 30 degrees, arrived setting a delay of 600mS after each rotation step. This value is critical since just at 550mS I get some spurios readings.
This is an empirical solution and the scanning time is quite slow due to this delay, but it is the best I found.
What surprise me is that from the specs I found this servo should have a rotation speed of 0.120 sec for 60°, which gives a theoretic delay of 60mS for 30°; even accounting for some deviation I can not believe to see a 10x value.

I would like to try a different Sg90 servo but unfortunately I have only this one at the moment, awaiting for new samples in the next days.

Anyway, this is my last test code should it be of interest:

#include <Servo.h>
#define SERVOPIN 10
#define trigPin 12
#define echoPin 13

#define SERVO_CW 0
#define SERVO_CENTER 90
#define SERVO_AC 180
#define MAX_CMD_LEN 7  //max incoming text lenght: Cmd (1 char) + 5 digits + /0

long duration;
int distance;  //ultrasoni distance measured
int rotDelay = 600;
uint8_t steps = 30;

// Serial handling
char msgIn[MAX_CMD_LEN];
char buf[60];
char cValue[6];  //max 65535 (5 char) + /0
char cmd;
uint16_t nValue;

Servo myservo;

void Range() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.017;  //340mS / 1000 / 2
}

void MoveSonar(uint8_t a) {
  myservo.write(a);
  delay(rotDelay);
}

void Scan() {
  uint8_t i;
  MoveSonar(0);
  delay(200);

  for (i = 0; i <= 180; i += steps) {
    MoveSonar(i);
    Range();
    Serial.print(i);
    Serial.print(": ");
    Serial.println(distance);
  }
}

void setup() {
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  myservo.attach(SERVOPIN);
  Serial.begin(115200);
  Serial.println("Commands:  Scan, Move, Range, Delay, stePs ");
  Serial.print("(D) Delay: ");
  Serial.println(rotDelay);
  Serial.print("(p) Steps: ");
  Serial.println(steps);
  MoveSonar(SERVO_CENTER);
}

void loop() {
  uint8_t inByte, ln, ch;
  static uint8_t message_pos = 0;
  static char message[MAX_CMD_LEN + 1];


  while (Serial.available() > 0) {
    char inByte = Serial.read();
    if (inByte != '\n' && message_pos < MAX_CMD_LEN) {
      message[message_pos] = inByte;
      message_pos++;
    } else {
      message[message_pos] = '\0';
      nValue = 0;

      for (ln = 0; ln < message_pos; ln++) {
        ch = message[ln];
        if (ln == 0) {
          if (ch >= 'a' && ch <= 'z') ch -= 0x20;  //to Upper
          if (ch < 'A' && ch > 'Z') return;        //invalid letter
          cmd = ch;
        } else {
          nValue = 10 * nValue + (ch - '0');
        }
      }

      message_pos = 0;
      doCommand();
      cmd = "";
      nValue = 0;
    }
  }
}

void doCommand() {
  switch (cmd) {

    case 'S':
      Serial.println("Scanning...");
      Scan();
      break;

    case 'M':
      Serial.print("Move to  ");
      Serial.println(nValue);
      MoveSonar(nValue);
      Range();
      Serial.print("Distance:");
      Serial.print(distance);
      Serial.println(" cm");
      break;

    case 'R':
      Range();
      Serial.print("Range:");
      Serial.print(distance);
      Serial.println(" cm");
      break;

    case 'D':
      rotDelay = nValue;
      Serial.print("Delay: ");
      Serial.println(rotDelay);
      break;

    case 'P':
      steps = nValue;
      Serial.print("Steps: ");
      Serial.println(steps);
      break;
  }
}

What is powering the servo? A schematic would be most welcome.

The movement speed spec for the servo is at some specific supply voltage. That speed will change with different voltage (slower speed with lower voltage, of course). And if the supply is taxed the voltage will be lower yet and may take time to recover.

The HCSR04 sensors that I have used are a bit sensitive to supply voltage. Accuracy suffers if Vcc is less than 5V.

Power is (almost) 5V taken from the USB bus of the Mega2560.
All the sensors are cabled onto an expansion board fitted in the Mega (it is a cheap chinese 2WD robot platform).
But, again, I see a 10x response time; I can't believe a lower supply (i.e. 4.5V) could justify it.
In the meantime I found something more, possibily usefull: with a shorter delay, let's say 300ms, I have a high number of 0 reading and the servo rotation slow down.
It seems to me that the HC timeouts during the reading, even with an obstacle at 20cm.
Should I try to solder a capacitor (10uF ?) directly on the HC small board?
Is it reasonable, what u say?

void calculateDistance() {
  int temp=readMicroseconds();
  myservo.detach();
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.017;  //340mS / 1000 / 2
  myservo.attach(10);
  myservo.writeMicroseconds(temp);
}

Ok, I can understand the detach-attach, but what's the meaning of reading the microseconds to then write it to the servo?
Sorry, I'm ignorant about the internal servo mechanism: does it keep an internal uS counter?

Servo library has a method that called "readMicroseconds()" that gives you the last pulse width written to the servo. and when you reattach the servo you have to give it an angle to keep it from resetting to the middle thus you write the last know position before you deattached the servo. keeping it in the same position

actually we can omit this Read/Write, very soon comes next writing anyway, but after attach() servo goes to 90 grad and i hope with immediately setting a new angle to avoid back and forth moving.

Hi, @orionis

Can I suggest for a start you use the NewPing library, rather than going to the trouble of calculating in your code.

Thanks.. Tom.. :grinning: :+1: :coffee: :australia:

TO has no problem with multiplication.
i prefer a dividing

distance = duration /58;

APOLOGIES to all ! :cry: :flushed:

I found the problem...stupid...stupid...stupid...me
As is wrote, over the Mega sits a sensors board (part of the robot kit) ; here is the picture.


This board is supposed to be powered by a lithium battery pack (I do have them yet) with a cable from the holder to the board (small 2 pin connector in the lower left).

Now, since I'm powering everything from the 5V derived form Mega USB, I ASSUMED the same power was going to the exp board through the dedicated power pins...not true!
Checking with a tester I discovered that the Mega 5V bus and the 5V bus on the exp are not connected (see note later).
I plugged a couple of wires (on the right in the pic) from the Mega strip to a connector on the exp and...magically...everything is now absolutely ok. Possibly the exp was driven only from some HIGH pin.

Now the readings are always correct, even with a short rotation delay; now is operating with 30ms delay and the servo is really spinning.

I wish to thank you all for the support and please excuse me for your time.

NOTE: I wonder how the Chinese manufacturer thought to supply the original UNO board (I replaced it with a Mega) when everything is finally assembled. The only power comes from the battery connector on the expansion leaving the MCU board alone....mah!