support [Bug] drivebase.turn with Stop.HOLD forgets that headling angle has been reset to zero

Describe the bug Reset the drivebase heading angle to zero and do a turn with Stop.HOLD. First time goes OK, each next time (despite a heading reset each time) the robot turns n times the angle.

To reproduce Steps to reproduce the behavior:

run the program below
See the differences between Stop.BRAKE and Stop.HOLD

Expected behavior Except for some precision the stop mode should not have very much influence. The angle turned should be in the same order.

Screenshots None now.

test program issue_1767_report.py


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

from pybricks import version
print(version)

ANGLE_TO_TURN = 180
LOW = ANGLE_TO_TURN * 0.9
HIGH = ANGLE_TO_TURN * 1.1

hub = PrimeHub()
left_motor = Motor(Port.C, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.A)

drive_base = DriveBase(left_motor, right_motor, wheel_diameter=54, axle_track=79)
drive_base.use_gyro(True)

wait_or_not = True
for stop_mode in [Stop.BRAKE, Stop.HOLD]:
    for i in range(3):
        hub.imu.reset_heading(0)
        start_angle = hub.imu.heading() 
        drive_base.turn(ANGLE_TO_TURN, stop_mode, wait_or_not)
        print(f"{i+1:>3} turn {ANGLE_TO_TURN} degrees with {str(stop_mode):<12} from {start_angle}", end=" ")
        wait(1000)
        this_angle = hub.imu.heading()
        print(f'done? {drive_base.done()}; End_angle ', end=" ")
        if HIGH >= this_angle >= LOW:
            print(f'{this_angle:.3f} ')
        else:
            print(f'UNEXPECTED {this_angle:.3f} ')
    print()

It gets really funny if Stop.NONE is used. The robot goes berserk.

Probably related to #1767 and maybe #1811 although I see no heading reset or stop mode set there.

TL;DR

A larger program that prints markdown text from the results

issue_1767_multitest.py

from pybricks.hubs import  PrimeHub
# from pybricks.hubs import TechnicHub
# from pybricks.iodevices import XboxController
from pybricks.parameters import Button, Color, Direction, Port, Stop
from pybricks.pupdevices import Light, Motor
from pybricks.robotics import DriveBase
from pybricks.tools import wait
from pybricks import version
print(f'```\n {version}\n```')

hub = PrimeHub()
left_motor = Motor(Port.C, Direction.COUNTERCLOCKWISE)
right_motor = Motor(Port.A)

r_speed, r_accel, r_torque = right_motor.control.limits()
l_speed, l_accel, l_torque = left_motor.control.limits()

drive_base = DriveBase(left_motor, right_motor, wheel_diameter=54, axle_track=79)
drive_base.use_gyro(True)

ANGLE_TO_TURN = 60
LOW = ANGLE_TO_TURN * 0.9
HIGH = ANGLE_TO_TURN * 1.1

wait_or_not = True

# r_accel_high = 16000
r_accel_high = r_accel
right_motor.control.limits(r_speed, r_accel_high , r_torque)
left_motor.control.limits(l_speed, r_accel_high , l_torque)

print(f'\n```\nright motor control limits {right_motor.control.limits()}')
print(f'left  motor control limits {left_motor.control.limits()}\n```\n')

print("\n| use_gyro | stop mode | wait | angle before move | first angle | 1st done? | angle before move | second angle | 2nd done? |")
print("|---|---|---|---:|---:|---|---:|---:|---|")


def test_turn_degrees(gyro, wait_or_not, stop_mode):
    hub.imu.reset_heading(0)
    print(f'| {gyro} | {stop_mode} | {wait_or_not}', end=" ")
    print(f'| {hub.imu.heading():.3f}', end=" ") 
    drive_base.turn(ANGLE_TO_TURN, stop_mode, wait_or_not)
    wait(1000)
    this_angle = hub.imu.heading()
    if HIGH >= this_angle >= LOW:
        print(f'| {this_angle:.3f} ', end=" ")
    else:
        print(f'| UNEXPECTED {this_angle:.3f} ', end=" ") 
    print(f'| {drive_base.done()}', end=" ")

    hub.imu.reset_heading(0)
    print(f'| {hub.imu.heading():.3f} ', end=" ") 
    drive_base.turn(ANGLE_TO_TURN, stop_mode, wait_or_not)
    wait(1000)
    this_angle = hub.imu.heading()
    if HIGH >= this_angle >= LOW:
        print(f'| {this_angle:.3f} ', end=" ")
    else:
        print(f'| UNEXPECTED {this_angle:.3f} ', end=" ")
    print(f'| {drive_base.done()}', end=" ")
    print(" |")
    wait(1000)


use_gyro = True
wait_or_not = True
test_turn_degrees(use_gyro, wait_or_not, Stop.COAST_SMART)
test_turn_degrees(use_gyro, wait_or_not, Stop.COAST)
test_turn_degrees(use_gyro, wait_or_not, Stop.BRAKE)
test_turn_degrees(use_gyro, wait_or_not, Stop.HOLD)
test_turn_degrees(use_gyro, wait_or_not, Stop.NONE)
wait(1000)

use_gyro = False
wait_or_not = True
test_turn_degrees(use_gyro, wait_or_not, Stop.COAST_SMART)
test_turn_degrees(use_gyro, wait_or_not, Stop.COAST)
test_turn_degrees(use_gyro, wait_or_not, Stop.BRAKE)
test_turn_degrees(use_gyro, wait_or_not, Stop.HOLD)
test_turn_degrees(use_gyro, wait_or_not, Stop.NONE)
wait(1000)

print("\n")

Result of a testrun:

 ('primehub', '3.5.0', 'v1.20.0-23-g6c633a8dd on 2024-04-11')

right motor control limits (1000, 2000, 199)
left  motor control limits (1000, 2000, 199)

use_gyro	stop mode	wait	first angle	1st done?	second angle	2nd done?
True	Stop.COAST_SMART	True	59.760	True	60.320	True
True	Stop.COAST	True	60.445	True	61.045	True
True	Stop.BRAKE	True	59.189	True	61.787	True
True	Stop.HOLD	True	59.762	True	UNEXPECTED 114.346	True
True	Stop.NONE	True	UNEXPECTED 428.404	True	UNEXPECTED 889.373	True
False	Stop.COAST_SMART	True	UNEXPECTED 294.662	True	59.260	True
False	Stop.COAST	True	60.713	True	60.482	True
False	Stop.BRAKE	True	59.859	True	61.688	True
False	Stop.HOLD	True	59.910	True	UNEXPECTED 116.215	True
False	Stop.NONE	True	UNEXPECTED 427.986	True	UNEXPECTED 866.410	True

Bert

Sep 10 '24 20:09 BertLindeman

I don't see where you are calling drive_base.reset() to reset the drive base angle.

Sep 10 '24 20:09 dlech

You got me. (and as always as quick as a scalded dog)

I did

        hub.imu.reset_heading(0)

And I thought (did I?) the drive base would know about it. Yet I see strange results depending on the stop mode.

[EDIT] added drive_base.reset() to the larger program just after the hub.imu.reset_heading(0) and that shows little difference:

 ('primehub', '3.5.0', 'v1.20.0-23-g6c633a8dd on 2024-04-11')

right motor control limits (1000, 2000, 199)
left  motor control limits (1000, 2000, 199)

use_gyro	stop mode	wait	first angle	1st done?	second angle	2nd done?
True	Stop.COAST_SMART	True	59.146	True	60.779	True
True	Stop.COAST	True	60.168	True	60.563	True
True	Stop.BRAKE	True	59.623	True	60.482	True
True	Stop.HOLD	True	60.520	True	UNEXPECTED 114.053	True
True	Stop.NONE	True	UNEXPECTED 428.457	True	UNEXPECTED 897.045	True
False	Stop.COAST_SMART	True	UNEXPECTED 298.236	True	57.717	True
False	Stop.COAST	True	60.363	True	60.760	True
False	Stop.BRAKE	True	59.895	True	61.125	True
False	Stop.HOLD	True	59.539	True	UNEXPECTED 113.326	True
False	Stop.NONE	True	UNEXPECTED 429.625	True	UNEXPECTED 860.912	True

Sep 10 '24 20:09 BertLindeman

Thanks!

Related, while we are looking at this: https://github.com/pybricks/support/issues/1449

Sep 16 '24 12:09 laurensvalk

Also related https://github.com/pybricks/support/issues/1617

Sep 16 '24 12:09 laurensvalk

As for drivebase.reset(), see https://github.com/pybricks/support/issues/1617.

As for this issue:

Suppose you start and do drivebase.straight(100). It is now still holding the angle 0.
If you do hub.imu.reset_heading(90), the heading is 90. But the drivebase will still attempt to hold 0, thus "move unexpectedly", back to what it now knows as the new 0.

Calling hub.imu.reset_heading(90) is a bit like changing your autopilot instruments mid-flight. This is probably not intended, and I don't know that making it magically work is necessarily going to be any less confusing.

To reset anything to the drivebase, it is probably better to call drivebase.reset(). But maybe it is missing some handles, so we can review this use case as part of that consideration in https://github.com/pybricks/support/issues/1617.

Sep 19 '24 10:09 laurensvalk

This was also asked in https://github.com/orgs/pybricks/discussions/1767. I'm not decided what's best here.

It seems odd that interacting with the hub object should interact with the DriveBase object...

OK - here's a way that could make sense. If we think of the DriveBase being passed the imu object on initialization as the angle source, then it can set a callback to have its odometry reset and movement stopped if you you called reset_heading. This will also take care of the initialized hub orientation, which is currently kind of implicit.

Sep 19 '24 15:09 laurensvalk

Put simply, I think we'll just have to make a drive base stop if you reset the gyro angle while the drivebase is using it.

Does that make sense? Any objections?

( If you deliberately throw off your instruments mid flight by 90 degrees, you better be prepared to restart your engines :smile: )

Sep 20 '24 18:09 laurensvalk

make a drive base stop if you reset the gyro angle while the drivebase is using it.

This is implemented in https://github.com/pybricks/pybricks-micropython/commit/fee592555d9c56e5cb61705349a5ff36458b4772

Sep 20 '24 18:09 laurensvalk

Would it be better to raise an exception to not allow calling the function rather than just stopping the drivebase?

Sep 23 '24 08:09 dlech

I don't really know what's best. See e.g. https://github.com/orgs/pybricks/discussions/1767

Stopping kind of makes sense when resetting the heading with DriveBase.reset(), so I was contemplating making the reset via hub.imu work the same.

Sep 23 '24 10:09 laurensvalk

Would it be better to raise an exception to not allow calling the function rather than just stopping the drivebase?

Yes, it is probably more Pythonic. We could do:

In examples we'll probably want to just always use the drive base blocks.

Sep 24 '24 08:09 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!

Sep 24 '24 14:09 laurensvalk