So recently I started making a sonar out of ultrasonic HC-SR04 sensor and servo motor. The idea was to slowly rotate servo 180 degrees while measuring distance.

Soon after I noticed that regular pulseIn() command could wait 20ms while doing nothing. So I started learning interrupts. I came up with a code which sends TRIG signal and records sending time with micros() function. When an interrupt occurs on ECHO pin, we subtract current time with TRIG time and get time of round trip of the sound.

The code works as intended without servos connected, but it behaves interestingly ! I always get distance which is 8 cm bigger than actual distance. So I had to subtract 8 cm every time ! Also I noticed that max reliable range decreased , using pulseIn() I could measure distance of 400cm, and with mine code it starts to show weird numbers after 150cm...

So i thought that 150cm is enough for me, and tried to run the code with servos attached. everything ran as intended, until the servos started to move. Now I was getting distance along with pretty much random numbers, periodically positive and negative.

I could just use one the fancy libraries out there, but I am curious what am I doing wrong, why i need to subtract 8cm each time , why my reliable range decreased so much, and most importantly why am I getting weird values while servos are on ?


#include <Servo.h>

#define ECHO 2
#define TRIG 7

 Servo servo;
 int distance;
 volatile long echoTime = 0;
 long trigTime = 0,sendTime = 0,timeServo = 0;
 bool scanning = false;
 byte servoDir = -1,servoAngle = 0;

void setup() 
  attachInterrupt(digitalPinToInterrupt(ECHO), echo_interupt, FALLING);

void loop() 
  // SERVO

   if((millis() - timeServo > 10)){
     if(servoAngle <= 0 || servoAngle >= 180)servoDir = -servoDir;
     servoAngle += servoDir;
     timeServo = millis();

  if(echoTime > 0){
    distance = echoTime - trigTime;
    distance /= 58;
    echoTime = 0;
    trigTime = 0;
    Serial.println(distance - 8);
  if(millis() - sendTime > 20){
  trigTime = micros();
  sendTime = millis();

void echo_interupt(){
  echoTime = micros();

2 Answers 2


8cm is 233µs of sound travel time. There and back again would be 466µs. 466µs, at 16MHz, is around 7400 instructions.

When an interrupt is triggered you don't get an instant response. First the built in interrupt function is called. That then looks to see if you have registered an interrupt function. It then calls that interrupt function.

All that takes time, and when you're working with microseconds it makes a big difference.

And then you have to go and get the current micros() value, which again takes time to do.

So it all adds up to a delay. Not ideal.

For greater accuracy you should investigate using input capture which will start a timer running when you send the trigger and the timer will automatically stop the nanosecond the echo is received. From that you can calculate a much more precise distance.

As for why you are getting strange results with the servos on? Most likely because you are moving the receiving head while the pulse is being received. I made a similar system while playing once and found that the best results were achieved by moving the head to the desired position, waiting a moment for any vibration to settle, do the ping and get the result, then move the head to the next position.

Oh, and by the way, the HC-SR04 is (from memory) only really rated for up to 2m - I have never achieved anything more than around 1.8m with any accuracy.

  • Great, now I can work on time calculation. Regarding strange results, ultrasonic head wasn't mounted on servo at all, it was in different place and standing still, only connected to the same Arduino board. I'll work on time calculation , perhaps it could solve the problem.
    – boofati
    Oct 12, 2015 at 18:02
  • @boofati: You could bypass the arduino library and declare your real isr. Here's a short tutorial: avr-tutorials.com/interrupts/… Oct 12, 2015 at 18:39
  • @Majenko: the downside of input capture is that it uses an IP block, so it doesn't scale easily - unless you consider adding more AVRs as scaling :-) Oct 12, 2015 at 18:40
  • @IgorStoppa Or switching to a real microcontroller instead ;)
    – Majenko
    Oct 12, 2015 at 18:41
  • @Majenko: then where is the fun? Yes, any Cortex M-something could do it much better, but really, then there is no more challenge :-) Oct 12, 2015 at 18:43

I had problems with Servo library and VirtualWire. They both use the same internal timer. The solution was to change Servo.h to ServoTimer2.h lib.

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

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