Servos and ultrasonic at the same time

Question

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 ?

CODE :

#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() 
{ 
  servo.attach(12);
  Serial.begin(115200);
  pinMode(ECHO,INPUT);
  pinMode(TRIG,OUTPUT);
  attachInterrupt(digitalPinToInterrupt(ECHO), echo_interupt, FALLING);

} 
void loop() 
{
  // SERVO

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


  // GETTING RESULTS
  if(echoTime > 0){
    distance = echoTime - trigTime;
    distance /= 58;
    echoTime = 0;
    trigTime = 0;
    Serial.println(distance - 8);
  }
  // SENDING PULSE
  if(millis() - sendTime > 20){
  digitalWrite(TRIG,LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG,HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG,LOW);
  trigTime = micros();
  sendTime = millis();
  }
}

 // INTERUPT
void echo_interupt(){
  echoTime = micros();
}

Answer 1

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.

Answer 2

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.