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

[BertLindeman]

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:

1. run the program below
2. 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	angle before move	first angle	1st done?	angle before move	second angle	2nd done?
True	Stop.COAST_SMART	True	0.000	59.760	True	0.000	60.320	True
True	Stop.COAST	True	0.000	60.445	True	0.000	61.045	True
True	Stop.BRAKE	True	0.000	59.189	True	0.000	61.787	True
True	Stop.HOLD	True	0.000	59.762	True	0.000	UNEXPECTED 114.346	True
True	Stop.NONE	True	0.000	UNEXPECTED 428.404	True	0.000	UNEXPECTED 889.373	True
False	Stop.COAST_SMART	True	0.000	UNEXPECTED 294.662	True	0.000	59.260	True
False	Stop.COAST	True	0.000	60.713	True	0.000	60.482	True
False	Stop.BRAKE	True	0.000	59.859	True	0.000	61.688	True
False	Stop.HOLD	True	0.000	59.910	True	0.000	UNEXPECTED 116.215	True
False	Stop.NONE	True	0.000	UNEXPECTED 427.986	True	0.000	UNEXPECTED 866.410	True

Bert

[dlech]

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

[BertLindeman]

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	angle before move	first angle	1st done?	angle before move	second angle	2nd done?
True	Stop.COAST_SMART	True	0.000	59.146	True	0.000	60.779	True
True	Stop.COAST	True	0.000	60.168	True	0.000	60.563	True
True	Stop.BRAKE	True	0.000	59.623	True	0.000	60.482	True
True	Stop.HOLD	True	0.000	60.520	True	0.000	UNEXPECTED 114.053	True
True	Stop.NONE	True	0.000	UNEXPECTED 428.457	True	0.000	UNEXPECTED 897.045	True
False	Stop.COAST_SMART	True	0.000	UNEXPECTED 298.236	True	0.000	57.717	True
False	Stop.COAST	True	0.000	60.363	True	0.000	60.760	True
False	Stop.BRAKE	True	0.000	59.895	True	0.000	61.125	True
False	Stop.HOLD	True	0.000	59.539	True	0.000	UNEXPECTED 113.326	True
False	Stop.NONE	True	0.000	UNEXPECTED 429.625	True	0.000	UNEXPECTED 860.912	True

[laurensvalk]

Thanks!

Related, while we are looking at this: #1449

[laurensvalk]

Also related #1617

[laurensvalk]

As for drivebase.reset(), see #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 #1617.

[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.

[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 😄 )

[laurensvalk]

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

This is implemented in pybricks/pybricks-micropython@fee5925

[dlech]

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

[laurensvalk]

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.

[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.

[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!