[Feature] Builtin 1D heading self-correction routine

[laurensvalk]

Is your feature request related to a problem? Please describe. Gyro calibration for a typical navigation task has (at least) these two aspects:

1. Calibrating a few seconds while stationary to find the gyro offsets.
2. Adjusting for what is actually 360 degrees when you make one full turn. Now a hub might consistently report 357 or 361, which varies by hub. In other words, the gyro is quite precise, but not accurate (off by about +/- 1%)?

Pybricks already implements 1 but not 2. Now that we have enabled persistent storage for settings, it is reasonable to implement 2 also.

Describe the solution you'd like

The goal is to obtain and store a correction factor so that one turn will result in 360 degrees. We could have a routine that interactively guides the user through these steps to find and save it.

This could be in a fancy window with animations and all, but we can start much simpler. For example, we could have the user run a script/block that will produce the following as you follow the instructions in the output window:

> Put the hub on a flat table. Align against a fixed reference like a wall or heavy book. Press hub button when ready.
Processing...

> Keep the hub flat and slide it to make a full turn clockwise. Put it against the same reference. Press hub button when ready.
Processing...

> So far so good! Now make 5 full clockwise turns.
Completed 1 out of 5 turns. Keep going!
Completed 2 out of 5 turns. Keep going!
Completed 3 out of 5 turns. Keep going!
Completed 4 out of 5 turns. Keep going!
Completed 5 out of 5 turns. Put it against the same reference. Press hub button when ready.
Processing...

> For every 360-degree turn, your sensor reported 363.8 degrees.

> The correction factor of `-3.8` degrees will be saved on your hub, in order to automatically improve the values you'll get from `heading()`.

After doing this hub.imu.heading(use_stored_correction=True) will give you the adjusted value. Or maybe it should be the default.

The value would persist until you update to a new firmware.

The value would only be useful for the hub orientation that was used to do the calibration.

[laurensvalk]

Would this help your students, @afarago , @DrTom, @ggramlich, @harikun77, @thehomelessguy, @MonongahelaCryptidCooperative, @TheWendyPower? Or would it be more confusing than helpful, even if it is optional?

How did you previously correct for the gyro being precise, but not accurate (off by about +/- 1%)?

Do your students notice it at all? Have they noticed that this varies from hub to hub?

As always, thank you for any feedback!

[DrTom]

Yes, it has been noticed. It can be a problem but most times it is not an major aspect. At some point during a run some drift accumulates and I believe that causes quickly a larger problem than the systematic error.

Currently one team compensates for this problem; the others I am quite sure didn't even notice.

That said. It would be nice if a correction factor could be supplied when creating a hub along with the parameters for the axis. I do not think a build in, interactive process is necessary. The drivebase has to be manually calibrated with respect to wheel size and track width. It seems to me this is similar to that. Some good documentation would be helpful though.

[afarago]

One of my teams noticed yet due to other uncertainties we agreed to limit the "uncontrolled" turns to max ~360 degrees. This way the offset was eliminated. Afterwards a wall align or clean start is required.

I completely agree with @DrTom any built-in procedure to the firmware seems unnecessary. i think it is good to have a best practice (even a code) to measure though.

Both in EV3 and SPIKE/RI it seemed that CCW and CW gyro shift was not uniform. Never had the time to really get to the bottom.

[laurensvalk]

That said. It would be nice if a correction factor could be supplied when creating a hub along with the parameters for the axis. I do not think a build in, interactive process is necessary.

Thanks for the feedback! The potential issue with putting the correction factor in the hub initialization, is that your code becomes suitable only for that one hub. I was thinking that storing it on the hub might solve this.

One of my teams noticed yet due to other uncertainties we agreed to limit the "uncontrolled" turns to max ~360 degrees. This way the offset was eliminated. Afterwards a wall align or clean start is required.

This is definitely a good approach.

[ggramlich]

Finally I read the full proposal and I am not sure, if I get all the implications. We currently do not explicitly read the heading, but I assume that we implicitely use it with useGyro() on the drivebase.

Our team members all have their own hub and we noticed that fine tuning which was done by one team member did not work well on the other hubs. One cause probably was the inaccuracy of the gyro. Other causes were things like slightly different absolute positions of the motors for the arms. This lead to some frustration first and later lead to a working mode where they only did the fine tuning on the robot used for the competition (which was also frustrating as the kids with the other hubs could barely experiment on their own and only during the meetings as the kids live more than 50km apart).

If it where possible to store a correction factor on the hub (with a reasonable default) that would be used with the drivebase useGyro() that would be awesome. Putting the correction factor as a value in the setup would be quite cumbersome for our use case, as the kids would always need to change it, when trying out the programs of their team mates.

The process you proposed would be nice, but also a simpler way like giving a sample program to measure the deviation and then a function call (python only, no block coding needed for that) to store the correction factor in persistent storage would be fine. It would also be interesting to see a sample program to measure the deviation without having automatic correction, so that we can find out, how much the gyros ond the hubs differ currently.

[laurensvalk]

Finally I read the full proposal and I am not sure, if I get all the implications.

Based on the rest of your reply, I think you understood it exactly right. Thanks for the input!

Our team members all have their own hub and we noticed that fine tuning which was done by one team member did not work well on the other hubs. One cause probably was the inaccuracy of the gyro. (...) They only did the fine tuning on the robot used for the competition. (...) If it where possible to store a correction factor on the hub (with a reasonable default) that would be used with the drivebase useGyro() that would be awesome. Putting the correction factor as a value in the setup would be quite cumbersome for our use case, as the kids would always need to change it, when trying out the programs of their team mates.

This is indeed what I was concerned about, and why it would be nice to have the value stored.

I see @DrTom's case of supplying a value directly also being useful.

---

I think we can essentially have both. Since the "builtin routine" is likely going to be a Python script, that script will have to make a call to eventually save the result. If we expose that call in the API, anyone who wants to store their own value can do so, or even make their own automatic routine.

---

Here is the script that we might include. I'd actually be curious for your findings! I have a few hubs that are reasonably spot on, but another that is quite far off. For that one I get:

Processing...
For every 360-degree turn your gyro reports: 364.3535

It doesn't store the result yet. It just shows it to you.

What do you get? You can run it on any Prime / Technic / Essential / Inventor Hub. Flat on the table is the easiest. But it can be in the robot if you make sure to keep all wheels on the table as you spin it around.

from pybricks.hubs import ThisHub
from pybricks.pupdevices import Motor
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop, Axis
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch

# Number of turns to confirm the result.
NUMBER_CONFIRM_TURNS = 5

# Maximum speed values before we consider the result invalid.
MAX_XY_SPEED = 30
MAX_Z_SPEED = 800

# Routine to wait on a button, with some extra time to avoid vibration directly after.
def wait_for_click():
    while hub.buttons.pressed():
        wait(1)
    while not hub.buttons.pressed():
        wait(1)
    print("Processing...")
    while hub.buttons.pressed():
        wait(1)
    wait(1500)

# REVISIT: Decide how to deal with non-standard orientation in case of the builtin routine.

hub = ThisHub()
hub.system.set_stop_button(None)
print("Put the hub on a flat table. Align against a fixed reference like a wall or heavy book. Press hub button when ready.")

# Wait for fixed reference and store the initial angle value.
wait_for_click()
while not hub.imu.ready() or not hub.imu.stationary():
    wait(1)
start_z = hub.imu.rotation(-Axis.Z)

# Wait for a full rotation and get the result.
print("Keep the hub flat and slide it to make a full turn clockwise. Put it against the same reference. Press hub button when ready.")
wait_for_click()
one_turn = hub.imu.rotation(-Axis.Z) - start_z

# Require clockwise...
if one_turn < 0:
    raise ValueError("You turned it the wrong way. Please try again.")

# Sanity check. Should be close to 360.
if not (350 < one_turn < 370):
    print(one_turn)
    raise ValueError("The error was more than 10 degrees, which is unexpected. Please try again.")

# Instruct to make more turns.
print("So far so good! Now make", NUMBER_CONFIRM_TURNS, "full clockwise turns.")

for i in range(NUMBER_CONFIRM_TURNS):

    # The rotation target is just under a rotation so we can show the next
    # message to keep going in time, avoiding doubts about what to do.
    target = one_turn * (i + 2) - 10

    # Wait until the hub reaches the target.
    while hub.imu.rotation(-Axis.Z) - start_z < target:
        wait(1)

        if hub.buttons.pressed():
            raise RuntimeError("Don't press the button until all turns are complete!")

        if abs(hub.imu.angular_velocity(Axis.Z)) > MAX_Z_SPEED:
            raise RuntimeError("Not so fast! Try again.")

        if abs(hub.imu.angular_velocity(Axis.X)) + abs(hub.imu.angular_velocity(Axis.Y)) > MAX_XY_SPEED:

            raise RuntimeError("Please keep the hub flat! Try again.")

    # Inform user of status.
    print("Completed", i + 1, "out of", NUMBER_CONFIRM_TURNS, "turns. ", end="")
    if i < NUMBER_CONFIRM_TURNS - 1:
        print("Keep going!")
    else:
        print("Put it against the same reference. Press hub button when ready.")

# Wait for final confirmation.
wait_for_click()

# Verify the result.
expected = one_turn * (NUMBER_CONFIRM_TURNS + 1)
result = hub.imu.rotation(-Axis.Z) - start_z

if abs(expected - result) > NUMBER_CONFIRM_TURNS / 2:
    print("The", NUMBER_CONFIRM_TURNS, "extra turns where different from the first turn. Try again.")
    print("Expected", expected, "but got", result)

# Get the final result to save.
average_turn = result / (NUMBER_CONFIRM_TURNS + 1)
multiplier = 360 / average_turn
print("For every 360-degree turn your gyro reports:", average_turn)
print("Correction factor:", multiplier)

# TODO: Save this value on the hub.

[laurensvalk]

The following proposal might have the best of both worlds. Green are new additions.

This is the more technical, to-the-point method where you can set/save and get the value directly. With an extra setting in the existing .settings method.

Additionally, here is the simpler routine that most users can use. Possibly with a dedicated block.

[afarago]

I have tested it an CW and CCW gives consistently different results even after checking with multiple hubs. I compared CW and CCW for a single hub.

Not a huge amount, still something that worth checking here imo.

CW For every 360-degree turn your gyro reports: 363.4356 Correction factor: 0.9905468 For every 360-degree turn your gyro reports: 363.1423 Correction factor: 0.9913468

CCW For every 360-degree turn your gyro reports: -362.7817 Correction factor: -0.9923323 For every 360-degree turn your gyro reports: -362.7711 Correction factor: -0.9923612

[laurensvalk]

Thank you for adding some data to this!

Agreed it should be looked into as well. Still, cutting down an error of 3.6 degrees (1%) to about 0.36 degrees (0.1%) with the proposed method is already a pretty big step. :slightly_smiling_face:

[afarago]

Yes, agreed, even the measurement itself surprised me, I never checked and being 6-7 degrees off was quite surprising!

So this will be an awesome improvement for us, the pybricks users!

[ggramlich]

Here are my values from one Spike Prime and two Robot Inventors (CCW values displayed positive)

Spike

For every 360-degree turn your gyro reports: 365.3367
Correction factor: 0.9853925
For every 360-degree turn your gyro reports: 365.2092
Correction factor: 0.9857364
For every 360-degree turn your gyro reports: 365.1848
Correction factor: 0.9858023

CCW
For every 360-degree turn your gyro reports: 365.5557
Correction factor: 0.9848021
For every 360-degree turn your gyro reports: 365.4167
Correction factor: 0.9851767
For every 360-degree turn your gyro reports: 365.358
Correction factor: 0.9853349

RI1

For every 360-degree turn your gyro reports: 361.9772
Correction factor: 0.9945377
For every 360-degree turn your gyro reports: 362.2524
Correction factor: 0.9937821
For every 360-degree turn your gyro reports: 362.1362
Correction factor: 0.994101

CCW
For every 360-degree turn your gyro reports: 362.221
Correction factor: 0.9938683
For every 360-degree turn your gyro reports: 362.1195
Correction factor: 0.9941469
For every 360-degree turn your gyro reports: 362.2118
Correction factor: 0.9938935

RI2

For every 360-degree turn your gyro reports: 363.6843
Correction factor: 0.9898694
For every 360-degree turn your gyro reports: 363.5801
Correction factor: 0.9901532
For every 360-degree turn your gyro reports: 363.0981
Correction factor: 0.9914677

CCW
For every 360-degree turn your gyro reports: 363.9792
Correction factor: 0.9890674
For every 360-degree turn your gyro reports: 363.8464
Correction factor: 0.9894285
For every 360-degree turn your gyro reports: 363.7965
Correction factor: 0.9895641

[laurensvalk]

Excellent, nice to have some extra data! Because if we're finding a trend here, we could not only add the calibration routine but also guesstimate a baseline to improve the default for everyone.

For bonus points, would anyone want to retry this with their team's ready-built robot? If your hub is flat, the result should be approximately the same. But it is a bit trickier to complete the routine successfully, so I wonder if this is too hard or acceptable.

[afarago]

This is the data for our FLL competition robot this year. Interesting side fact that due to the 62.4mm wheel the robot is not perfectly level (our design flaw), yet the gyro seems to perform very well.

RI

CW For every 360-degree turn your gyro reports: 361.6986 Correction factor: 0.9953037 For every 360-degree turn your gyro reports: 361.8342 Correction factor: 0.9949307

CCW For every 360-degree turn your gyro reports: -361.6644 Correction factor: -0.9953979 For every 360-degree turn your gyro reports: -361.7399 Correction factor: -0.9951902

[laurensvalk]

Interesting side fact that due to the 62.4mm wheel the robot is not perfectly level (our design flaw), yet the gyro seems to perform very well.

Although one way to correct for this is by adjusting the top axis, running the calibration routine will also correct for this :slightly_smiling_face:

[laurensvalk]

You can try it out now! Using this firmware file for Prime Hub or Technic Hub or Essential Hub

For @DrTom, there is a setting that lets you do it right below the hub initialization.

For @ggramlich, there is an interactive routine to determine the value.

In both cases, the value is saved on the hub in the same way.

from pybricks.hubs import InventorHub
from pybricks.tools import vector, wait

# Initialize the hub. Optional: Choose a custom orientation
hub = InventorHub(top_side=vector(3, 0, 4), front_side=vector(-4, 0, 3))

# Optional: Set the value yourself.
hub.imu.settings(heading_correction=363.2)

# Optional: check the result.
print(hub.imu.settings())

# Run the update routine. This will give you instructions to update it interactively.
hub.imu.update_heading_correction()

After that, and even after reboot, the following program should result in near-perfect 360-degree values if you make a full turn:

from pybricks.hubs import InventorHub
from pybricks.tools import vector, wait

# Set up all devices. (Optional: Use the same custom orientation as before)
hub = InventorHub(top_side=vector(3, 0, 4), front_side=vector(-4, 0, 3))

# The main program starts here.
while True:
    print(hub.imu.heading())
    wait(100)

Eventually, there might be blocks for this too.

If your hub has the default orientation (flat), then you can also simply click the >>> button and paste hub.imu.update_heading_correction() into the terminal and press Enter. Instead of running a script like the above.