[Feature] Basic IMU heading support. Feedback wanted!

[laurensvalk]

Update: all functionality is now available for use via https://beta.pybricks.com/

The links and functionality described below is outdated and should not be used anymore.

---

Is your feature request related to a problem? Please describe.

We get a lot of requests for gyro support. While full 3D attitude estimation or heading in arbitrary reference frames is still a long way off, there is perhaps something we can do to start with that works for certain use cases.

In this case, I am talking about the use case of drive bases (FLL/WRO), where the hub is flat (parallel to a horizontal table) most of the time.

We want your help to test it! (Preparation step)

Download this experimental firmware:

• SPIKEPrime Hub / MINDSTORMS Robot Inventor Hub
• SPIKE Essential Hub
• Technic Hub

Installing it works just like installing Pybricks normally. But on the hub selection page, you now click Advanced to select the zip file you just downloaded:

We want your help to test it! (Question 1)

For the exciting stuff, skip to part 2. But we'd still like to get your feedback on part 1 :)

We'd love to get your help to pick certain sensible defaults so the gyro works for everyone. Please run the program below and read through the explanations in the comments. Increase the two noise values until you think it works well enough under all expected conditions.

Once we've settled on good values, others should not need to worry about it (and we'll remove this debug method).

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

hub = PrimeHub()

#
# FEEDBACK WANTED, Question 1:
#
# In the program below, we make the light turn green when
# the hub sits stationary for a while. It should
# turn red immediately on movement. When the light is green,
# the hub is automatically re-calibrating the gyro.
#
# We define stationary as the "raw values not changing much".
# "Not much" is defined as the sensor values not changing above
# a selected threshold value that we have to pick.
#
# Lower values are "better" because this makes the hub
# recalibrate only when it's really-really-really still.
#
# But it shouldn't be too low, or the robot will never get
# "still enough" for the light to turn green!
# 
# We still want this to work when students are jumping up and
# down next to the competition table...
#
#
#
# So in short: What is the lowest we can go for both values below, while 
# ensuring the light still turns green after one second of not touching it?
#
# Can you test this in the school cafeteria, etc? :-)
#
hub.imu.debug(gyro_noise=20, accelerometer_noise=100)

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

We want your help to test it! (Question 2)
Now for the exciting part!

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

hub = PrimeHub()

hub.imu.debug(gyro_noise=20, accelerometer_noise=100)

while True:

    if hub.imu.stationary():
        hub.light.on(Color.GREEN)
    else:
        hub.light.on(Color.RED)

    # Get heading.
    #
    # FEEDBACK WANTED, Question 2: Is this good enough? :-)
    #
    heading = hub.imu.heading()

    # Display heading on the LED matrix.
    hub.display.number(round(heading))

    # Print heading and wait a moment to we don't print so much.
    print("{:.1f}".format(heading))
    wait(25)

    # You can easily reset the heading to arbitrary values.
    # No special wait operations are required here. Just reset and go.
    if hub.buttons.pressed():
        hub.imu.reset_heading(0)

NB: Positive is counterclockwise

A positive angle is a counterclockwise turn. This is consistent with most engineering definitions.

The DriveBase defaults to positive for clockwise to be similar to the LEGO apps. We will add an option to flip this. See the drive base examples further down in this thread.

That way, your drive base direction and gyro values will match, saving you some headaches.

NB: For now it only works in horizontal orientation

We have not yet implemented 3D attitude estimation. So if you lift the robot from the table and put it back, it won't give the same value.

It is only implemented for the default orientation, where the hub is parallel to the table.

This might not be so bad; after all, the EV3 also had just one gyro axis. But we'd love to get your thoughts.

[laurensvalk]

This was requested/discussed by:

• @davecparker in [Bug] hub.imu.heading() is not implemented #912
• @TheWendyPower in FLL Challange Team Code and Trials #987
• @Xylar52 in gyroscope spike prime #980
• @8Root in Method for the drivebase.angle() function? #965
• @lopataaa in When is imu.heading() coming for pybricks for SPIKE Prime? #925
• @ws1088 in Primehub imu.heading() #242
• @johnscary-ev3 and @cmyers734 in How to get Yaw angle or Heading on Robot Inventor Hub? #675
• @CasperClarke in Adds Mahony filter based IMU orientation estimation. pybricks-micropython#135

Please give this a go if you have time and tell us what you think. 😄 Thank you!

If this kind of heading support is "good enough", we can start from here and add full 3D support later.

[TheWendyPower]

This is exactly what the team needs to use their turn-by-gyro function. Here is what it looks like in their LEGO code:

def right_turn(angle):
    p0w.motion_sensor.reset_yaw_angle()
    motor_pair.start(speed=10,steering=100)
    wait_until(p0w.motion_sensor.get_yaw_angle,greater_than_or_equal_to,int(angle))
    motor_pair.stop()
    wait_for_seconds(.5)

def left_turn(angle):
    p0w.motion_sensor.reset_yaw_angle()
    motor_pair.start(speed= 10, steering= -100)
    wait_until(p0w.motion_sensor.get_yaw_angle, less_than_or_equal_to, int(angle))
    motor_pair.stop()
    wait_for_seconds(.5)

We are more than happy to test imu.heading() and give you some feedback on it.

[laurensvalk]

Taking things a step further, we can optionally let users use the gyro with the existing drivebase methods. (This is not included in the build above yet, but it works quite well).

For example, when driving straight or turning, you'd get automatic corrections like this:

VID_20230316_141516.mp4

We could add a keyword argument use_gyro to the DriveBase class for this.

It would probably default to False, but setting it to True would enable the behavior above.

We could also accept a callable like use_gyro=my_gyro_angle_function so all of this can also work with an external gyro or compass, as was the case for EV3 and NXT.

[TheWendyPower]

YES!!!!! We were using a drive straight function as well. Between the two it would give the kids the functionality they had before, plus all the additional stuff Pybricks lets them work with.

[laurensvalk]

The download links above have now been updated to include the drivebase-with-gyro feature. You can use it as shown below.

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)

# 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]

OK, this is fun. 😄 This is the almost the same program as above but with higher speed/acceleration:

drive_base.settings(
    straight_speed=500,
    straight_acceleration=1000,
    turn_rate=500,
    turn_acceleration=2000
)

VID_20230316_205400.mp4

[TheWendyPower]

@laurensvalk I LOVE how accurate and fast that is moving!!!!

I updated the firmware to the latest one (the gyro worked grate in the previous one BTW), and added the arguments to DriveBase:

positive_direction=Direction.CLOCKWISE, 
use_gyro=True)

However, I get ValueError: Invalid argument do I need a different version of the Pybricks App too?

Here is the full block:

left_motor = Motor(Port.B,Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.C,Direction.CLOCKWISE)
motor_pair = DriveBase(
    left_motor=left_motor,
    right_motor=right_motor,
    wheel_diameter=87.56,
    axle_track=111.46,
    positive_direction=Direction.CLOCKWISE,
    use_gyro=True)

[laurensvalk]

You have to use the DriveBase with positive_direction=Direction.COUNTERCLOCKWISE to match the gyro for now. See an example above.

We'll enable both directions both ways at a later stage. For now this restriction made it easier to implement while we work out what to do with the gyro (not just multiple directions, but also multiple axes, etc.)

[TheWendyPower]

Makes sense. When I made the functions earlier that used imu.heading() I cheated with

gyro_angle = P0W.imu.heading()*-1

to reverse it for me in the functions.

[BertLindeman]

We want your help to test it! (Question 1)

I am no longer at school (retired a decade) but tried anyway.
Used both a Robot Inventor hub and a Prime hub.
They are the same, but small internal differences might be there.

To test I placed the hubs on the armrest of my chair.
Your defaults work well. I could go down to gyro_noise=18, accelerometer_noise=75 on the Prime Hub.
Much lower caused the led to hardly show green. Even if I did not sit in my chair.
Feels like the RobotInventor hub is a bit more sensitive than the prime hub.
But no scientific proof for that.
The RobotInventor hub would react similar to the prime hub at gyro_noise=19, accelerometer_noise=80

This is fun-stuff.

Bert

[laurensvalk]

I am no longer at school (retired a decade) but tried anyway.

Jumping in excitement also counts as simulating the ambient acceleration/vibration of an FLL final 😄

Thanks for testing Bert!

As the converse challenge, how high you can make them such that it still always shows red when you try to move it?

[TheWendyPower]

The team did some testing today. Our meeting room is on the 3rd floor and it is very creeky. In order to walk around the robot table and have the light stay green our settings were gyro_noise=700, accelerometer_noise=700. However, we were running the robot perfectly well using the base settings you gave. We have 3 Spike Prime robots at home. We will do some testing on them and see what readings we get.

As for the gyro feature built into the hub, we ran into a little snag. The kids launched the robot from home. It removed the energy cell from the hydro dam, goes back home, and then the kids reposition it to start the next mission. The robot wants to stick with the last heading and will turn as such. This is adding positive_direction=Direction.COUNTERCLOCKWISE, use_gyro=True to the drive base and .straight() in their code. I took a quick video to show you. Do note this is also using the base settings you gave for debugging. We also did an experiment where we put the next mission in its own python file, then imported it into a full run file, like this:

from functions import *

import Run_01 #Hydro Dam
wait_until_I_say_so(.5) #the kids wait for button press function
import Run_02 #Oil Rig, Solar Farm, Collect Rech. Battery

raise SystemExit

It has the same downside of wanting to go back to the last 0° heading.

gyro_test_small.mp4

[dlech]

The robot wants to stick with the last heading and will turn as such.

Did you try the hub.imu.reset_heading(0) method to reset the heading?

[TheWendyPower]

Did you try the hub.imu.reset_heading(0) method to reset the heading?

No, but after rereading the documentation that would make sense. It uses the degree being read at the time of the .straight() command. Therefore, resetting after the run is over would allow it to be repositioned before .straight() is utilized again.

[laurensvalk]

While resetting the heading after you've moved the robot manually is indeed very important, there's something else going on here:

By default, any drive base command will use Stop.HOLD at the end. This means it will keep trying to maintain the distance/heading after it completes.

This is usually great in between consecutive moves because it preserves accuracy, avoiding error buildup. (@TheWendyPower, this is one reason why you don't need that 0.5 second pause between moves you talked about).

But if you turn 45 degrees like in this video and lift it from the table, it is still going to try to get back to 45 when you put it down.

To prevent that, just call drive_base.stop() at the end of your mission. Alternatively, use Stop.COAST in the last (or all) movements.

So generally, do:

hub.imu.reset_heading(0)

do_mission_one()

drive_base.stop()   # <--- This will stop the robot from trying to keep up with the last commanded gyro angle

# prep robot for next mission

# put it back down / position it / wait for button

hub.imu.reset_heading(0)

do_mission_two()

drive_base.stop()

# prep robot for next mission

# put it back down / position it / wait for button

# and so on

[laurensvalk]

That said, amazing to see the new gyro feature on a real FLL robot already! Go team! 😄

[BertLindeman]

As the converse challenge, how high you can make them such that it still always shows red when you try to move it?

Assume that the variable used for these noise settings are 2 byte int.
Using 32768 never gets green, so it's -1 I think.

Running with Spike Prime hub with
gyro_noise = 24000; accelerometer_noise = 13000
never gets red, only if violently shaking by hand, so probably not 100% flat horizontal.

It is a matter of how fast you move the hub.
I turned the hub by hand, not very scientific.

Automated (more or less)

Sorry for the long post.

So build a "turntable":

Run the turntable with program turn_test.py:

from pybricks.hubs import InventorHub
from pybricks.pupdevices import Motor
from pybricks.parameters import Port
from pybricks.tools import wait

hub = InventorHub()

motor = Motor(Port.F)
print(motor.control.limits())
motor.control.limits(2000, 20000, 20000)
print("using control.limits(speed, acceleration, torque)", motor.control.limits())
while True:
    motor.run_angle(700, 90)
    motor.stop
    wait(1000)
    motor.run_angle(250, -90)
    motor.stop
    wait(1000)

Test the IMU with the Spike Prime hub with:

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

hub = PrimeHub()
clock = StopWatch()

gyro_noise_top = 28000
accelerometer_noise_top = 18000

# print header:
print("gyro_ \taccel")
print("noise \tnoise")

# start with a few seconds NOT measuring
clock.reset()
while clock.time() < 2000:
    wait(100)

for accelerometer_noise in range(accelerometer_noise_top, 11000 - 1, -500):
    # range                (start,          stop,       step)
    for gyro_noise in range(gyro_noise_top, 24000 - 1, -2000):
        hub.imu.debug(gyro_noise, accelerometer_noise)
        wait(10)
        red = 0
        seen_green = False
        clock.reset()
        # do at least for 4 seconds, so the turntable did two "shakes"
        while clock.time() < 4000:
            if hub.imu.stationary():
                hub.light.on(Color.GREEN)
                seen_green = True
            else:
                hub.light.on(Color.RED)
                red += 1
        print(gyro_noise, "\t", accelerometer_noise, "\t", "red seen", red, "times, green seen:", seen_green)
        wait(250)
print("program ended")
hub.speaker.beep(70, 50)

Resulting in:

gyro_   accel
noise   noise
28000    18000   red seen 0 times, green seen: True
26000    18000   red seen 0 times, green seen: True
24000    18000   red seen 0 times, green seen: True
28000    17500   red seen 0 times, green seen: True
26000    17500   red seen 0 times, green seen: True
24000    17500   red seen 0 times, green seen: True
28000    17000   red seen 0 times, green seen: True
26000    17000   red seen 0 times, green seen: True
24000    17000   red seen 0 times, green seen: True
28000    16500   red seen 0 times, green seen: True
26000    16500   red seen 0 times, green seen: True
24000    16500   red seen 0 times, green seen: True
28000    16000   red seen 843 times, green seen: True
26000    16000   red seen 5944 times, green seen: True
24000    16000   red seen 8406 times, green seen: True
28000    15500   red seen 0 times, green seen: True
26000    15500   red seen 8016 times, green seen: True
24000    15500   red seen 9484 times, green seen: True
28000    15000   red seen 11023 times, green seen: True
26000    15000   red seen 5205 times, green seen: True
24000    15000   red seen 11408 times, green seen: True
28000    14500   red seen 9113 times, green seen: True
26000    14500   red seen 0 times, green seen: True
24000    14500   red seen 0 times, green seen: True
28000    14000   red seen 0 times, green seen: True
26000    14000   red seen 18224 times, green seen: True
24000    14000   red seen 15981 times, green seen: True
28000    13500   red seen 13615 times, green seen: True
26000    13500   red seen 15220 times, green seen: True
24000    13500   red seen 2985 times, green seen: True
28000    13000   red seen 21731 times, green seen: True
26000    13000   red seen 16562 times, green seen: True
24000    13000   red seen 0 times, green seen: True
28000    12500   red seen 16202 times, green seen: True
26000    12500   red seen 16967 times, green seen: True
24000    12500   red seen 13468 times, green seen: True
28000    12000   red seen 17522 times, green seen: True
26000    12000   red seen 7475 times, green seen: True
24000    12000   red seen 18581 times, green seen: True
28000    11500   red seen 20702 times, green seen: True
26000    11500   red seen 14951 times, green seen: True
24000    11500   red seen 16340 times, green seen: True
28000    11000   red seen 30351 times, green seen: True
26000    11000   red seen 18103 times, green seen: True
24000    11000   red seen 13684 times, green seen: True
program ended

I wonder why some observations do not get "red".
Like these:

28000    15500   red seen 0 times, green seen: True

26000    14500   red seen 0 times, green seen: True
24000    14500   red seen 0 times, green seen: True
28000    14000   red seen 0 times, green seen: True

24000    13000   red seen 0 times, green seen: True

Maybe the measurements are not very well synchronized with the movements of the turntable?

Hope it helps a bit.
Or I could do the measurements differently . . .

And if not I had fun with it. 😉

[johnscary-ev3]

@laurensvalk
Wow, this is really great!
I played with your two test programs above.
The first one is quite sensitive with those settings, but I can get green if I don't tap my desk or anything.
Probably could use it for earthquake detection, as I live in California ;0)
If I increase settings to (500,1000) it is much less sensitive, but I noticed if I use those levels in second program, I can lose some accuracy in the heading readings. I guess some actual rotation is cut off as noise. So, there is a trade off on this?

One thing I was wondering about is that as I understand it the hub has three axis gyros so would it be possible for users to pick a different axis to use. Some robots may be using a different orientation. Currently I am using my hub in a "Blast" robot.

Thanks for the hard work on this. Really appreciate it.

[dlech]

I guess some actual rotation is cut off as noise. So, there is a trade off on this?

Try reducing the gyro sensitivity while keeping the acceleration sensitivity higher.

One thing I was wondering about is that as I understand it the hub has three axis gyros so would it be possible for users to pick a different axis to use.

You can do this by passing the orientation to the hub constructor, e.g InventorHub(top_side=Axis.X, front_side=-Axis.Y)

[johnscary-ev3]

@dlech
Thanks for the inputs.
I tried to change axis orientation for the heading function as you mentioned. That should be the "Blast" orientation, I think.
However, it made no difference. Seems like heading function is hard coded to use the hub physical Top_side axis instead of the changeable Z axis.

[dlech]

As Laurens said earlier:

NB: For now it only works in horizontal orientation

[laurensvalk]

Thanks @johnscary-ev3!

If I increase settings to (500,1000) it is much less sensitive, but I noticed if I use those levels in second program, I can lose some accuracy in the heading readings. I guess some actual rotation is cut off as noise. So, there is a trade off on this?

That's exactly the trade off I was hoping to get some input on, but I probably didn't explain it very well 😄

It sounds like the current defaults are working for everybody. We'll probably increase them slightly and hard code those in the firmware so nobody else has to worry about picking values.

Selecting an arbitrary axis and positive direction should be possible fairly soon. Stay tuned :)

[johnscary-ev3]

Thanks @laurensvalk
My Blast robot is waiting patiently to test out the "arbitrary axis" implementation ;0)

[johnscary-ev3]

@laurensvalk
Wow, this is working great!
Blast robot is very happy now that he has a good heading to use ;0)

[JuliaFiMSFT]

The Rockin' Robots tried Question 1. These are their results:
Test surface: our robotics room. Both on our table and on the wood floor. The results were identical in both locations.
When everyone is still for a moment, gyro_noise=20 and accelerometer_noise=100 works. The light is mostly green.
We simulated a judge or someone else walking past, 3-5 feet away. During that test a gyro_noice=500 and accelerometer_noise=500 both resulted in a mostly red light with occasional flashes to green.

Based on our experience, we think it would be good for teams to be able to manually set their gyro_noise and accelerometer_noise if they need to. Our regional and semi-finals competitions are in high school gymnasiums that likely have a lot of vibration. Our state championships are in a convention center that has cement floors. It likely has less vibration.

Thank you for doing this work! We only need single directional gyro information so we know which direction the robot is pointing on the FLL Challenge table. We have tried PyBricks on our Spike Prime but found that it doesn't recover when it goes off track. With the LEGO firmware we are able to get it to recover by checking the gyro and rerouting ourselves.

[laurensvalk]

Thanks for the feedback, this is really helpful!

With the heading() method available as in question 2, is your robot able to recover using Pybricks as well, or did you find some issues with this method? Thanks!

[JuliaFiMSFT]

We will have to do some testing and coding during our meeting on Sunday to answer that question. Here is our "drive straight" function from this season. We check the yaw value constantly while we are driving so if the robot hits something it will straighten itself out. It also works really well when there is a hair on the wheel that causes the wheel to slip a lot. We ignore how much the wheel has turned and focus on the direction the robot is facing. We need to figure out how to incorporate the PyBricks drive_base.straight() with the gyro checks.

We love how much faster and accurate the PyBricks robot is compared to the LEGO robot. We had a series of competitions between two identical robots with the two different firmwares and it made us VERY excited to use PyBricks next year!

[laurensvalk]

Thanks for the update.

We need to figure out how to incorporate the PyBricks drive_base.straight() with the gyro checks.

While you could do the same as you did above (using heading in place of yaw), you can now also rely on the newly builtin functionality to do this for you.

If you use use_gyro=True, then commands like straight() or turn() will automatically correct using the gyro.

Here's a video to demonstrate that it can be quite effective. This is just a single straight() command that keeps adjusting itself:

VID_20230316_141516.mp4

[Xylar52]

Here's a video to demonstrate that it can be quite effective. This is just a single straight() command that keeps adjusting itself:

I tried to do that with a straight() command and it works but whenever I change the straight speed in drivebase.settings, instead of instantly adjusting itself, it does a big curve just to get to the initial angle. I think think it's because when the robot turn to adjust, it only accelerate one motor and don't slow down the other one, so when the speed is too high, it tries to accelerate even more only one motor.

[laurensvalk]

Can you share a code sample and a video that reproduces that behavior? You can just attach the video here.

[JuliaFiMSFT]

We added the use_gyro=True and now our code runs the same as our LEGO-based code. Only significantly more quickly and accurately. Note: we haven't spent time fine-tuning the Pybricks code while we spent months fine-tuning the LEGO code. So there is a ton of room for improvement in the Pybricks run.

Compressing the videos to less than 10MB was challenging so we put them on a google drive. https://drive.google.com/drive/folders/1jyi3Dk_I8OmWCnQdgSpUm3Tnm6N6C4Nd?usp=share_link

We tested this route because it has a large attachment that extends on the left so it is more challenging for the robot to drive straight. This route starts by going forward 10cm then turning 14 degrees to the right. Our logging shows that it never turns the full 14 degrees. It only turns 10-12 degrees and if varies up to 1.8 degrees. Here is our logging from 8 repetitions:

Move: 100
Current distance: 99
Current angle: 1 Turn to: -14 Need to turn: -15
New direction: -11.15928
Current time: 0.936 seconds
Move: 100
Current distance: 98
Current angle: 1 Turn to: -14 Need to turn: -15
New direction: -10.45954
Current time: 0.94 seconds
Move: 100
Current distance: 100
Current angle: 1 Turn to: -14 Need to turn: -15
New direction: -11.62943
Current time: 0.935 seconds
Move: 100
Current distance: 99
Current angle: 1 Turn to: -14 Need to turn: -15
New direction: -11.45204
Current time: 0.935 seconds
Move: 100
Current distance: 100
Current angle: 0 Turn to: -14 Need to turn: -14
New direction: -11.48282
Current time: 0.935 seconds
Move: 100
Current distance: 100
Current angle: 0 Turn to: -14 Need to turn: -14
New direction: -10.13301
Current time: 0.92 seconds
Move: 100
Current distance: 99
Current angle: 0 Turn to: -14 Need to turn: -14
New direction: -10.38118
Current time: 0.925 seconds
Move: 100
Current distance: 99
Current angle: 0 Turn to: -14 Need to turn: -14
New direction: -11.93741
Current time: 0.94 seconds

[laurensvalk]

We added the use_gyro=True and now our code runs the same as our LEGO-based code. Only significantly more quickly and accurately. Note: we haven't spent time fine-tuning the Pybricks code while we spent months fine-tuning the LEGO code. So there is a ton of room for improvement in the Pybricks run.

That's awesome, thanks for sharing! 😄

Our logging shows that it never turns the full 14 degrees. It only turns 10-12 degrees and if varies up to 1.8 degrees.

The drive base (with or without gyro) has a tolerance that defines when a move is "done". This normally ensures that:

• The robot doesn't get stuck waiting forever if it doesn't finish the move by just a few degrees
• The robot doesn't twitch at the end as it tries to fix overshoot

These tolerances are all configurable if you want to dig deeper. Since this is independent of the gyro, it might be better to start a separate discussion about this if you're interested. You can also make your own functions as you did previously, where you essentially set your own tolerances.

[johnscary-ev3]

@laurensvalk
I have been working to integrate the new heading() method into my Blast Robot.
It is working fine but I also use the tilt() method so I can get pitch and roll to check if robot has fallen over.
This used to work ok giving me pitch and roll ~(0, 0) unless I tilt the robot.
Now I am getting ~ (0, -179). If I turn the robot to stand on its head I get (0, 0) again.
Seems like something has changed with respect to roll or something.
Below is the heading() test program with pitch, roll added for Blast.
Any comments?

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

#hub
#This is how the hub is mounted in BLAST in the 51515 set.
hub = PrimeHub(top_side=Axis.X, front_side=-Axis.Y)

hub.imu.debug(gyro_noise=20, accelerometer_noise=100)

while True:

   if hub.imu.stationary():
       hub.light.on(Color.GREEN)
   else:
       hub.light.on(Color.RED)

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

   # Get Pitch and Roll.
   pitch, roll = hub.imu.tilt()

   # Display heading on the LED matrix.
   hub.display.number(round(heading))

   # Print heading and wait a moment to we don't print so much.
   print("h= ","{:.1f}".format(heading),"p= ", "{:.1f}".format(pitch),"r= ", "{:.1f}".format(roll))
   wait(100)

   # You can easily reset the heading to arbitrary values.
   # No special wait operations are required here. Just reset and go.
   if hub.buttons.pressed():
       hub.imu.reset_heading(0)

Initial Data after starting robot with no movment
h=  0.0 p=  4.0 r=  -179.0
h=  0.0 p=  4.0 r=  -179.0
h=  -0.0 p=  4.0 r=  -179.0
h=  0.0 p=  4.0 r=  -179.0
h=  -0.0 p=  4.0 r=  -179.0
h=  0.0 p=  4.0 r=  -179.0
h=  -0.0 p=  4.0 r=  -179.0
h=  -0.0 p=  4.0 r=  -179.0

[BertLindeman]

Below is the heading() test program with pitch, roll added for Blast.

I can confirm.
Running this on my Spike Prime hub I see:

('primehub', '3.3.0b2', 'v3.3.0b2-78-g86f27b429 on 2023-03-21')
h=    0.0 p=    0.0 r=  179.0
h=    0.0 p=    0.0 r=  179.0
h=    0.0 p=    0.0 r=  179.0
h=    0.0 p=    0.0 r=  179.0
h=    0.0 p=    0.0 r=  179.0

So +179 not -179.

Running (older firmware) on the inventor hub:

('primehub', '3.3.0b2', 'v3.3.0b2-49-g0f343064 on 2023-03-16')
h=   -0.0 p=    1.0 r=    0.0
h=    0.0 p=    1.0 r=    0.0
h=   -0.0 p=    1.0 r=    0.0
h=   -0.0 p=    1.0 r=    0.0
h=   -0.0 p=    1.0 r=    0.0

[EDIT] updated inventor hub output

[laurensvalk]

Thanks @johnscary-ev3 and @BertLindeman! Proper gyro+accelerometer support is currently in progress.

This issue was opened mainly to get some feedback on the heading concept. Since the response was so positive, we're now in the process of getting this merged in properly (without breaking the accelerometer 😉)

If you can't wait, you can follow our progress in pybricks/pybricks-micropython#156.

We'll keep this issue open for now, and ask everyone to move over to the public beta once we've released it :)

[johnscary-ev3]

@laurensvalk
I downloaded a more recent build #2646 and the roll is working ok again. Thanks.

By the way, I have a comment about the new calls being called "heading".
From what I can tell heading should be a number from 0 - 360 dg like on a compass.
Seems to me what we are calling heading is more like "yaw angle".
So, I wrote a function to get compass style heading angle from yaw angle.
See code below. Any comment on this naming? Could be confusing for users.

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

# Get Heading Angle from Yaw Angle 
def get_heading_angle(yaw_angle):
    if yaw_angle <= 0:
        heading_angle = (-yaw_angle % 360) 
    else:
        heading_angle = 360-(yaw_angle % 360)
    return(heading_angle)

#hub = PrimeHub()
# For example, this is how the hub is mounted in BLAST in the 51515 set.
hub = PrimeHub(top_side=Axis.X, front_side=-Axis.Y)


#hub.imu.debug(gyro_noise=20, accelerometer_noise=100)

while True:

    if hub.imu.stationary():
        hub.light.on(Color.GREEN)
    else:
        hub.light.on(Color.RED)

    # Get heading.
    #
    # FEEDBACK WANTED, Question 2: Is this good enough? :-)
    #
    yaw_angle = hub.imu.heading()
    heading_angle = get_heading_angle(yaw_angle)

    # Display heading on the LED matrix.
    hub.display.number(round(yaw_angle))

    # Print heading and wait a moment to we don't print so much.
    print('yaw_angle= ',"{:.1f}".format(yaw_angle), 'heading_angle= ',"{:.1f}".format(heading_angle))
    wait(25)

    # You can easily reset the heading to arbitrary values.
    # No special wait operations are required here. Just reset and go.
    if hub.buttons.pressed():
        hub.imu.reset_heading(0)

[Xylar52]

Can you share a code sample and a video that reproduces that behavior? You can just attach the video here.

from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor,
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop, Icon
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch

hub = PrimeHub()
motorl = Motor(Port.A, Direction.COUNTERCLOCKWISE)
motorr = Motor(Port.B)
motorl.control.limits(acceleration=[2000, 400])
motorr.control.limits(acceleration=[2000, 400])
drive_base = DriveBase(
    left_motor=motorl, 
    right_motor=motorr, 
    wheel_diameter=55, 
    axle_track=143,
    positive_direction=Direction.COUNTERCLOCKWISE,
    use_gyro=True)

drive_base.settings(
    straight_speed=500,
    straight_acceleration=1000,
    turn_acceleration=2000,
    turn_rate=500
    )

while True:
    drive_base.straight(1000, then=Stop.NONE)

This code concludes in this :

20230405_133825.mp4

However, if you delete the straight speed in settings :

from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor, ForceSensor,
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop, Icon
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch

hub = PrimeHub()
motorl = Motor(Port.A, Direction.COUNTERCLOCKWISE)
motorr = Motor(Port.B)
motorl.control.limits(acceleration=[2000, 400])
motorr.control.limits(acceleration=[2000, 400])
drive_base = DriveBase(
    left_motor=motorl, 
    right_motor=motorr, 
    wheel_diameter=55, 
    axle_track=143,
    positive_direction=Direction.COUNTERCLOCKWISE,
    use_gyro=True)

drive_base.settings(
    straight_acceleration=1000,
    turn_acceleration=2000,
    turn_rate=500
    )

while True:
    drive_base.straight(1000, then=Stop.NONE)`

It concludes in the normal behaviour :

20230405_133902.mp4

[Xylar52]

Any news about my issue ?

[laurensvalk]

Thank you, I was able to reproduce it. I will open a separate ticket for this.

[BrBarry]

OK, I have tried most of the code samples about, and for the life of could not get the robot to turn square (90, 180, 270, 360)
I assumed it was our RoboCup Junior competition robot configuration (the hub is at the back and sideways) but still 'Top Up'.

Removed the Spike Prime hub and run the Yaw/heading output code against a square edge, rotating through all angles. Still out, and the same on 2 other hubs.

I'm getting an accumulated error in my heading values of by an average -0.975 per degree out, on all our Spike Prime Hubs (3).

One sample below

[laurensvalk]

Thanks @BrBarry! We have since made considerable improvements that should get it a lot closer to 360 degrees.

We will be publishing a proper beta release next week (i.e. not hidden here in a GitHub discussion). We didn't publish anything this week because the FLL finals are happening right now, and we didn't want any teams to run into accidental changes during their finals.

Even after these improvements, there will still be some variations between hubs. If needed, we might address that with a calibration routine that people can run on their hubs.

[laurensvalk]

All functionality discussed here is now available via https://beta.pybricks.com/.

Just install the firmware with the usual steps; no custom files required.

Some implementation details have changed based on your feedback. Please consult the documentation in app via the link above for using the gyro and drive bases. If you've been using the experimental versions in this thread, here are the most important updates:

• There is now a GyroDriveBase class that acts as a drop-in replacement for the regular DriveBase. No need to set special settings as in this thread.
• The direction convention for heading is now positive-clockwise based on user feedback. This also makes the GyroDriveBase easier to use since everything works the same way.
• The threshold setters have been updated. Based on comments from e.g. @JuliaFiMSFT we have decided to keep them accessible for users. See the documentation of the settings() method.

Thanks everyone for your feedback! We will now archive this issue. Please feel free to open new discussions and issues if anything comes up.

Also feel free to open a discussion if it "just works". That helps us too 😄