DriveBase heading control on an absolute scale especially when using use_gyro()

[afarago]

I know we have had several discussions on whether the DriveBase should exceute methods momentarily or on a global scale.
Coaching an FLL and a WRO teams working with robots that should perform consistently for 120-150 seconds I see a very nice value add to extend the current momentary execution logic to a global scale. Currently they need to find workarounds, including aligning to walls and objects, but I believe we have a better way to do this.
(OFDL WRO team Taiwan had a Global Yaw System posted somewhere in 2018?)

Current logic:

• target: drivebase.turn(90)
• actual movement: starts pivot turn and stops based on acceleration/deceleration logic around heading_control.target_tolerances()[1].
• target: drivebase.turn(90)
• actual movement: starts pivot turn and stops based on acceleration/deceleration logic around heading_control.target_tolerances()[1]
• repeated n times - the n errors are accumulated, pointing typically in the same direction based on the heading control and robot mechanical specifics -> resulting in an overshoot or undershoot of n*err degrees.

My Suggestion

Workaround

• keep an ideal_heading variable manually adding any turn/curve based operations
• check heading error before the turn (actual heading and ideal_heading diff) and compensate the operation parameter accordingly
• execute the drivebase operation and then update the ideal_heading

Suggestion:

• add an extra parameter to DriveBase similar to use_gyro() e.g. use_global() that accumulates and accounts all actions based on a global scale.

Additional idea:

• Based on empirical / or a calibration step check the expected overshoot/undershoot and compensate e.g. target turn based on this value.

Further topics to consider:

• straight() should use ideal_heading to calculate direction

Demonstration of the problem when using basic turns

[heading]         0 [target_turn] 90   [heading]   88.3555 [count]  1 / 0.981727 [error] 1.644531
[heading]   88.3555 [target_turn] 90   [heading]   177.916 [count]  2 /  1.97684 [error] 2.083984
[heading]   177.916 [target_turn] 90   [heading]   265.803 [count]  3 /  2.95336 [error] 4.197266
[heading]   265.811 [target_turn] 90   [heading]   358.051 [count]  4 /  3.97834 [error] 1.949219
[heading]   358.055 [target_turn] 90   [heading]    447.16 [count]  5 /  4.96845 [error] 2.839844
[heading]    447.16 [target_turn] 90   [heading]   536.193 [count]  6 /   5.9577 [error] 3.806641
[heading]   536.193 [target_turn] 90   [heading]   625.809 [count]  7 /  6.95343 [error] 4.191406
[heading]   625.809 [target_turn] 90   [heading]   716.639 [count]  8 /  7.96265 [error] 3.361328
[heading]   716.637 [target_turn] 90   [heading]   805.809 [count]  9 /  8.95343 [error] 4.191406
[heading]   805.811 [target_turn] 90   [heading]   894.807 [count] 10 /   9.9423 [error] 5.193359
[heading]   894.807 [target_turn] 90   [heading]   984.184 [count] 11 /  10.9354 [error] 5.816406
[heading]   984.184 [target_turn] 90   [heading]   1075.05 [count] 12 /   11.945 [error] 4.947266
[heading]   1075.05 [target_turn] 90   [heading]   1163.63 [count] 13 /  12.9293 [error] 6.365234
[heading]   1163.63 [target_turn] 90   [heading]   1253.07 [count] 14 /   13.923 [error] 6.927734
[heading]   1253.07 [target_turn] 90   [heading]   1342.31 [count] 15 /  14.9146 [error] 7.6875
[heading]   1342.31 [target_turn] 90   [heading]    1433.6 [count] 16 /  15.9288 [error] 6.404297
[heading]    1433.6 [target_turn] 90   [heading]   1522.97 [count] 17 /  16.9219 [error] 7.029297
[heading]   1522.97 [target_turn] 90   [heading]   1611.96 [count] 18 /  17.9107 [error] 8.035156
[heading]   1611.96 [target_turn] 90   [heading]   1702.36 [count] 19 /  18.9151 [error] 7.644531
[heading]   1702.36 [target_turn] 90   [heading]   1792.01 [count] 20 /  19.9113 [error] 7.986328
[heading]   1792.01 [target_turn] 90   [heading]   1881.22 [count] 21 /  20.9024 [error] 8.783203
[heading]   1881.22 [target_turn] 90   [heading]   1970.69 [count] 22 /  21.8966 [error] 9.308594
[heading]   1970.69 [target_turn] 90   [heading]   2060.43 [count] 23 /  22.8937 [error] 9.568359
[heading]   2060.43 [target_turn] 90   [heading]      2150 [count] 24 /  23.8889 [error] 10.0
[heading]   2150.01 [target_turn] 90   [heading]   2239.66 [count] 25 /  24.8851 [error] 10.33984
[heading]   2239.66 [target_turn] 90   [heading]   2329.45 [count] 26 /  25.8827 [error] 10.55469
[heading]   2329.45 [target_turn] 90   [heading]   2419.24 [count] 27 /  26.8804 [error] 10.76367

Example code:

from pybricks.hubs import InventorHub
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch
from pybricks import version
from umath import copysign, fabs

hub = InventorHub()
motor_left = Motor(Port.E, Direction.COUNTERCLOCKWISE)
motor_right = Motor(Port.F)
robot = DriveBase(motor_left,motor_right,62.4,125)
robot.use_gyro(True)
robot.settings(217, 316, 166, 200)
robot.distance_control.target_tolerances(15, 8)
robot.heading_control.target_tolerances(15, 7)
heading_tolerance_position = robot.heading_control.target_tolerances()[1]

def test_turns_basic():
    cnt = 0
    while True:
        
        target_turn = 90
        print (
            "[heading]", f'{hub.imu.heading():>9}',
            "[target_turn]", target_turn,
            end='')
        robot.turn(target_turn)

        h = hub.imu.heading()
        cnt = cnt + 1
        print("  ",
            "[heading]", f'{h:>9}',
            "[count]", f'{cnt:>2} / {h/90:>8}',
            "[error]", fabs(target_turn-h%90)
        )
        # wait(1000)

def test_turns_compensated():
    cnt = 0
    ideal_heading = 0
    while True:
        
        target_turn = 90

        actual_turn = ideal_heading + target_turn - hub.imu.heading()
        actual_turn = actual_turn + copysign(heading_tolerance_position/3, target_turn)
        print (
            "[heading]", f'{ideal_heading:>4} {hub.imu.heading():>9}',
            "[target_turn]", target_turn,
            end='')
        robot.turn(actual_turn)
        ideal_heading = ideal_heading + target_turn

        h = hub.imu.heading()
        cnt = cnt + 1
        print("  ",
            "[heading]", f'{h:>9}',
            "[count]", f'{cnt:>2} / {h/90:>8}',
            "[error]", fabs(target_turn-h%90)
        )
        # wait(1000)

test_turns_basic()
# test_turns_compensated()

See also #1094 .

[laurensvalk]

Thanks for opening this. While additional methods are certainly worth discussing, the builtin turn method is already supposed to account for accumulation.

So if it isn't doing that, that's a bug that we should fix, even aside from extra methods 🙂

(Naturally, this is aside from the actual gyro error accumulation compared to a compass, but you are already disregarding that, correct?)

Separately from that, I would generally not recommend making 27 turns in the same direction, but I understand why you do it for this experiment.

For absolute target control, you could in principle just do:

robot.turn(target - robot.heading())

The tolerances are not related to this. They only come into play when Coast or Brake is used, where accumulation is not corrected. But I think you're using the default, hold, so the tolerances should not be relevant in terms of accumulation ideally. But let's explore if something is not right.

[afarago]

Interesting.
Yet if we have turn accounting for accumulated errors (using "hold"), how come we get this visible error for 20+ turns.

Yes, the compensated one uses the suggested method of
robot.turn(target - robot.heading())

The proposal is to have that as built in.

I will need to run more experiments and check the code to understand what you refer to. I strongly believed that for heading control DriveBase is pretty much stateless.

[laurensvalk]

Yes, I meant I agree that if that error does accumulate, then it is a bug 🙂 It's definitely worth looking into this, so thanks a lot for gathering your data!

[ggramlich]

@laurensvalk Could you elaborate on the effect of using Coast / Brake / Hold with turns or curves and useGyro with respect to accumulating heading error. I have seen https://beta.pybricks.com/static/docs/v2.19.0/parameters/stop.html and do not fully grasp it. Also COAST_SMART is no option for Block Coding (maybe it should be?).

If we have some point in the program where we align (on a wall or because line following is stable). What is your recommendation to make the following curves / turns as reliable as possible?
I told the kids to switch useGyro off before alignment and on after it. (That was before the [drivebase reset distance and angle] block was available.) I am not sure if that is good advice.

[laurensvalk]

For what it's worth, I'm working on a website update at the moment, and this time it will have a proper blog page. I'm hoping to include a bunch of these thinking-out-loud posts on a page where people can actually find this... And hopefully keep it going for a while 😄

[ggramlich]

Thanks a lot @laurensvalk. That was actually very helpful.
We were able to understand and correct some strange behavior that happened after putting together parts which worked very well in isolation by switching useGyro off and on again between those parts.
For longer parts with lots of turns and curves we were able to improve consistency by using hold at the relevant points. Before that I was advising against using hold at most of the points, because I feared that it would slow down execution. But if that were the case at all that is negligible especially with the gain of consistency that we have now.

[laurensvalk]

Sounds good! By the way, hold does not impact execution time. It just defines the behavior of the motor after completion.

[afarago]

Hi Laurens,
I believe that hold affects execution time on a bigger scale.

Before PyBricks (b.p. ;) we have had this robot round emulating the "continue" with a few blocks in the standard LEGO environment.

If we would have used the standard "hold" this would have been a much different and longer run.

https://youtu.be/0IPYT9jOqM8?si=lCWVMG4gbeNQGCBm

[laurensvalk]

I suppose using hold might have made a difference in runtime in the official V1/V2 firmware. But not in the Pybricks firmware or the SPIKE V3 firmware.