Enable 1D gyro heading measurement and drivebase control

[laurensvalk]

This implements a first version of the heading() and reset_heading() methods in the IMU class.

Limitation: This is only 1D rotation, like driving around on a flat table. 3D attitude estimation is in progress, but we wanted to make sure we have a useful starting point for FLL in the mean time.

Automatic calibration

The sensor incrementally updates its internal calibration whenever it is not moving. No user action is necessary. The hub does not need to be stationary on boot. A method hub.imu.stationary() is available so users can verify that it is stationary.

Accurate heading reading with drift correction

VID_20230321_214216.mp4

The heading value does not drift while stationary because of the constant re-calibration.

While heading estimation is limited 1D, you can still mount the hub at an arbitrary rotation in your robot, as shown in the video above.

The imu.angular_velocity() value is now also automatically accounted for drift, in case you want to build your own balancers, etc.

from pybricks.hubs import PrimeHub
from pybricks.parameters import Color
from pybricks.tools import wait

# Initialize hub as usual. 
# You can optionally specify an alternate hub base orientation.
hub = PrimeHub()

while True:
    # Test stationary.
    hub.light.on(Color.GREEN if hub.imu.stationary() else Color.RED)

    # Get heading
    heading = hub.imu.heading()

    # Display heading
    hub.display.number(round(heading))
    print("{:.1f}".format(heading))
    wait(10)

Builtin drivebase motion adjustment

The gyro can be used in the DriveBase class for automatic corrections while driving.

Example of just driving straight():

VID_20230316_141516.mp4

Example of making fast, accurate turns. Code below:

VID_20230316_205400.mp4

from pybricks.pupdevices import Motor
from pybricks.parameters import Port, Direction
from pybricks.robotics import DriveBase

# Initialize both motors. In this example, the motor on the
# left must turn counterclockwise to make the robot go forward.
left_motor = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.B)

# Initialize the drive base. In this example, the wheel diameter is 56 mm.
# The distance between the two wheel-ground contact points is 112 mm.
drive_base = DriveBase(
    left_motor,
    right_motor,
    wheel_diameter=56,
    axle_track=112,
    positive_direction=Direction.COUNTERCLOCKWISE, 
    use_gyro=True)

# You can set very high speeds and it will still be accurate.
drive_base.settings(
    straight_speed=500,
    straight_acceleration=1000,
    turn_rate=500,
    turn_acceleration=2000
)

# Drive in a straight line and back again a few times.
for i in range(4):

    # Drive forward for 500 mm.
    drive_base.straight(500)

    # Turn around counterclockwise.
    drive_base.turn(180)

    # Drive forward for 500 mm.
    drive_base.straight(500)

    # Turn around clockwise.
    drive_base.turn(-180)

[laurensvalk]

A couple of things to review:

---

pybricks-micropython/lib/pbio/drv/imu/imu_lsm6ds3tr_c_stm32.c

Line 203 in 0a0332f

imu_dev->config.sample_time = (1 / 839.067f); // REVISIT: This implicitly scales gyro angle. Should adjust gyro scale instead.

We probably need to verify what the actual sampling rate is, or measure it as we go. Similarly, we'll want to check if we are occasionally missing a few samples or not.

We'll also need to check this on Technic Hub, where the I2C setup is slightly different, in case there are any difference.

Once that is settled, we may need to experimentally verify lsm6ds3tr_c_from_fs2000dps_to_mdps and adjust as needed. Possibly per axis, see also below.

---

There is currently no adjustment for misalignment of the chip within the hub. This is possible using a calibration routine (see pybricks/support#943), but the benefit is fairly small for everyday users.

However, it may be useful to do this locally when we test for sample rates as above, to ensure we register the full Z measurement when tweaking lsm6ds3tr_c_from_fs2000dps_to_mdps.

---

In pbio, there is currently not a global pbio_imu instance that gets passed around, since there is only one imu instance. It may still be useful to make the code easier to follow.

---

[dlech]

Really nice work on this. It is well documented and easy to follow for such a big changeset.

Mostly just comments on organization (separation of concerns) and a few nitpicks.

[laurensvalk]

Really nice work on this. It is well documented and easy to follow for such a big changeset.

Mostly just comments on organization (separation of concerns) and a few nitpicks.

Thanks for the review! I'll update this and get it merged.

[laurensvalk]

I deferred some pending issues to pybricks/support#1022. Everything else is addressed, so I'll go ahead and merge this.