[Bug] Slow gyro turn triggers imu drifting

[scatwang]

Describe the bug
What is the problem?

After a drive base makes a slow left turn with gyro, the imu heading starts to drift.

To reproduce
Steps to reproduce the behavior:

1. setup a drive base with gyro
2. set turn speed to 50
3. makes a left gyro turn by -100 degrees
4. wait (don't stop program) and check if drive base starts to turn slowly
5. print hub.imu.angular_velocity(Axis.Z), a non-zero value should be observed.

    await base.straight(130)
    base.settings(turn_rate=50)

    # -80 won't cause drifting
    # -90 sometimes cause driting
    # -100 is very likely to cause drifting
    await base.turn(-100)

    # base start to turn left slowly
    await wait(15000)

Expected behavior
What did you expect to happen instead?
The drive base should stop still without turnning.

Video

IMG_6094_720p.mov

Full code and logs

#⭐the program sets up the hub⭐#

from pybricks.hubs import PrimeHub
from pybricks.parameters import Direction, Port, Axis
from pybricks.pupdevices import Motor
from pybricks.robotics import DriveBase
from pybricks.tools import run_task, wait, multitask

#⭐the program sets up the devices⭐#

hub = PrimeHub()
drive_left = Motor(Port.C, Direction.COUNTERCLOCKWISE)
drive_right = Motor(Port.D, Direction.CLOCKWISE)
control_left = Motor(Port.B, Direction.CLOCKWISE)
control_right = Motor(Port.A, Direction.CLOCKWISE)
base = DriveBase(drive_left, drive_right, 56, 80)
base.use_gyro(True)

#⭐🌟⭐the program officially begins⭐🌟⭐#

async def debug():
    while True:
        print(hub.imu.heading(), hub.imu.angular_velocity(Axis.Z), sep=",")
        await wait(200)

async def movement():
    base.settings(straight_speed=300)

    await wait(2000)

    await base.straight(130)
    await wait(2000)

    base.settings(turn_rate=50)

    # -80 won't cause drifting
    # -90 sometimes cause driting
    # -100 is very likely to cause drifting
    await base.turn(-100)

    await wait(15000)
 
    raise SystemExit

async def main():
    await multitask(movement(), debug())

run_task(main())

'''
# note: static, wait(2000)
0.001154661,-0.6959249
0.009279251,-0.2759249
0.009129524,0.07407507
0.01072621,0.07407507
0.008993864,0.07407507
0.008115292,-0.06647742
0.009573698,0.003522575
0.009778023,0.07352258
0.006901026,0.003522575
0.008940935,-0.06647742

# note: straight(130)
0.01114392,0.003539726
0.1543877,0.1435397
0.2806633,-2.79646
1.007356,-1.39646
1.105444,0.3535397

# note: static, wait(2000)
1.029559,-0.1364603
1.030267,-0.06646028
1.031807,-0.1364603
1.03364,0.006022408
1.034851,-0.06397759
1.035561,0.006022408
1.037513,0.006022408
1.036562,0.07602242
1.03784,0.1462843
1.037574,0.006284311

# note: turn(-100)
-1.416137,22.47628
-10.17017,63.21628
-22.50772,57.05628
-33.30477,51.94629
-43.26921,49.56628
-53.73329,53.20628
-63.96997,53.41628
-74.05942,49.42628
-83.96741,47.24311
-93.55325,50.74311
-98.68993,-1.686891

# note: await(15000), drifting
-98.1879,-2.386891
-97.68018,-2.596891
-97.16772,-2.526891
-96.66068,-2.596891
-96.16261,-2.538463
-95.67845,-2.468463
-95.20466,-1.908463
-97.46153,13.35154
-97.28672,-1.908463
-96.80643,-2.538463
-96.3241,-2.468463
-95.84118,-2.258463
-96.39831,-2.188463
-95.92083,-1.978463

[laurensvalk]

Thanks for reporting! This was very useful. I was able to reproduce this on the standard large drivebase from the expansion set.

I think what is happening is that during slow turns it is considering itself stationary enough to recalibrate. And because it is moving, that throws off the calibration. To confirm, I've changed your debug function as follows (full code below) to print the imu.stationary() status:

async def debug():
    while True:
        print(hub.imu.ready(), hub.imu.stationary(), hub.imu.heading(), hub.imu.angular_velocity(Axis.Z), sep=",")
        await wait(200)

You can change the calibration thresholds using this method.

I think the defaults are currently a bit too high, causing the issue shown here. The reason that the defaults are high is to ensure it can calibrate even in a noisy FLL environment after feedback we got in #989

To better address this, the upcoming firmware will save your configured settings so they persist when you reboot. This way, we can choose lower defaults, and people in noisy environments can increase them if they have to.

Additional notes:

• It does not matter if it is positive or negative. Turning +180 reproduces this also
• The reason that this happens only for slow, long movements is because those reach the steady state speed. With short fast movements it accelerates to the middle and then decelerates to the end, so it never gets to steady state.

---

start turn
True,True,0.1254532,2.27196
True,False,-6.384631,65.41196
True,False,-20.13102,62.12196
True,False,-30.81437,49.66196
True,False,-41.01778,51.13196
True,False,-51.43689,51.90196
True,False,-61.41873,50.50196
True,False,-71.74469,52.11196
True,True,-81.89523,48.36148  # < Thinks it is stationary when it is not.
True,True,-91.81432,51.30148
True,True,-101.6332,46.82148
True,False,-111.2123,49.90147
True,False,-120.9828,48.43148
True,False,-130.8775,49.20148
True,False,-140.4199,46.96148
True,False,-150.3218,49.69147
True,False,-160.4207,52.28148
True,False,-170.5849,52.42147
done turn

# This is mostly OP's program, modified for the standard Advanced Driving Base

#⭐the program sets up the hub⭐#

from pybricks.hubs import PrimeHub
from pybricks.parameters import Direction, Port, Axis
from pybricks.pupdevices import Motor
from pybricks.robotics import DriveBase
from pybricks.tools import run_task, wait, multitask

#⭐the program sets up the devices⭐#

hub = PrimeHub()
drive_left = Motor(Port.A, Direction.COUNTERCLOCKWISE)
drive_right = Motor(Port.B, Direction.CLOCKWISE)
base = DriveBase(drive_left, drive_right, 88, 18*8)
base.use_gyro(True)

#⭐🌟⭐the program officially begins⭐🌟⭐#

async def debug():
    while True:
        print(hub.imu.ready(), hub.imu.stationary(), hub.imu.heading(), hub.imu.angular_velocity(Axis.Z), sep=",")
        await wait(200)

async def movement():
    base.settings(straight_speed=300)

    await wait(2000)

    print("start drive")
    await base.straight(130)
    print("done drive")
    await wait(2000)

    base.settings(turn_rate=50)

    print("start turn")
    await base.turn(-180)
    print("done turn")

    await wait(15000)
 
    raise SystemExit

async def main():
    await multitask(movement(), debug())

run_task(main())

[scatwang]

Thanks for addressing the problem in just a few hours. I will try to tune the turnning rate and imu settings.

Also, on the firmware side, is it possible to block imu from calibration when the drive base is executing a moving related function?

[laurensvalk]

Right. Maybe it needs to disable calibration during every motor move, not just drive bases. EDIT: Disabling calibration during any controlled movements is quite easy to implement.

The even simpler option would be to disable calibration while any code is running, but we would still want the user to be able to wait for calibration within their program. For example:

# device setup here

# Wait for calibration to finish in case it hasn't already.
while not hub.imu.ready():
    print("wait for calibration to complete!")
    wait(100)
    # and possibly after some time relax the settings ...

[laurensvalk]

The fix is available for testing using these instructions to try the latest version

Please re-open if you experience any further issues or inconsistencies. Thank you!

[scatwang]

Confirm this bug has been fixed with primehub-firmware-build-3565-gita137834e

[laurensvalk]

Thank you!