Raspberry pi servo and ultrasonic sensors jitter

I would like to know if someone else experienced the same problem with their servo motor combined with ultrasonic sensors on a Raspberry Pi (I own a 3B).

Essentially, I have mounted two ultrasonic sensors (HC-SR04) with 180° apart from each other on top of a servo motor (HS-422) to act as a radar.

Here's a sample code. The Servo and Sonic classes were made by me. They are fully functional and they use Joan's pigpio.

import pigpio, time
from Files.servo import Servo
from Files.sonic import Sonic

pi = pigpio.pi()

servoMotor = Servo(name = 'Test', gpio_list = [14], pi)
sensor = Sonic(name = 'Test2', trig = 2, echo = 3, pi)

servoMotor.rotate(0)
time.sleep(1)
try:
    while True:
        for i in range(181):
            servoMotor.rotate(angleR = i)
            time.sleep(0.01)
            print(i, sensor.distance())
        for i in range(181):
            servoMtor.rotate(angleR = 180 - i)
            time.sleep(0.01)
            print((180 - i), sensor.distance())

except KeyboardInterrupt:
    pi.stop()

The problem: When I make the servo rotate angle by angle (180 times), everything works just fine. When I instance my ultrasonic sensors along with the servo and return their distance(), everything starts to lag, and the servo moves really slowly.

I tried to print the distances to see if it was a servo jitter problem, but my suspicion was confirmed: it's all lagging.

As if the RPi was too slow.

Sometimes, I get a moment of "normal" speed for like 20° and then it slows down again. This is what confirmed to me it isn't a gpio library problem nor a sensor problem.

Is there a way out of this? Is the RPi really lagging?

Solution

Thank you for your reply.

I believe the sole fact that the sensors take about 10 ms to return a distance means for a whole 180°, it would take about:

180 * (0.010 + 0.010) = +/- 3.60 sec

I added another 0.01 because of my loop. The thing is, it takes more than 15 sec to do a 180° which doesn't make any sense. I also tried to put my hands around the sensors to make the duration to gather data lower but it didn't work.

With a lot of googlin', I found out that python isn't ideal for this type of usage (real time ?) and that although pigpio is based on hardware, it seems if there's some computing in parallel, things don't go your way. For reference, here's the distance() code:

 def distance(self):
    self.pi.write(self.trig, 1)
    time.sleep(0.00001)
    self.pi.write(self.trig, 0)
    while self.pi.read(self.echo) == 0:
        pass
    start = time.time()
    while self.pi.read(self.echo) == 1:
        pass
    stop = time.time()
    return ((stop - start) * 17000)

Would multithreading help?

When I first tried pigpio, calculating the duty cycle myself for the Servo would definitely make it jitter. When I opted for pigpio's servo functions, it went all smooth !

Would controlling the ultrasonic sensors with pigpio's set_servo_pulsewidth() make any difference?

I will be trying both of these solutions and report them back here.