[Bug] drive_base.distance_control.target_tolerances(position=) doesn't seem to work?

[abliss]

Describe the bug
If I understand the docs right, I should be able to control the distance PID's tolerances, but I don't seem to be able to.

To reproduce
Steps to reproduce the behavior:

1. Assemble Prime Hub with 2 small motors in A and B, no wheels
2. Python code:

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

hub = PrimeHub()
right_motor = Motor(Port.B, positive_direction=Direction.CLOCKWISE)
left_motor = Motor(Port.A, positive_direction=Direction.COUNTERCLOCKWISE)
drive_base = DriveBase(left_motor, right_motor, 57.2, 96.1)
drive_base.reset(distance=0)
#drive_base.distance_control.target_tolerances(position=1)
drive_base.straight(1000)

while True:
    print(f"Drove: {drive_base.distance()}")
    wait(500)

Expected behavior
When I run this with no wheels it prints 1003 in 10/10 runs.
(With wheels and a real robot actually driving, I get random numbers between 999 and 1004.
I expected that uncommenting the commented line would get me consistent numbers at least in the 999-1001 range. I also tried position=0.9, but any number between 0 and 1 seems to hang without ever printing anything. At no point did I ever observe the wheel spinning backwards at the end of the maneuver to correct for the overshoot.

[laurensvalk]

Great catch!

The issue is not with the tolerances, but we do have a rounding error here. Try running for 2000 or 3000 and you'll find that the error scales along. I'm surprised nobody reported this before, though maybe 10 mm off per 3m is too low to be noticeable in the real world after taking slipping into account.

I've found where this happens, and we should be able to fix it before the next release.

[laurensvalk]

Would you like to try the fix? To do so,

• You can download the firmware file for your hub at the bottom of this page.
• You can install it as explained here.

You can then run your original program again. I've updated your example above to keep printing it (the motors stop when the program ends, so otherwise you don't get to see returning to target if you push it beyond it).

[abliss]

thanks for the prompt response! I will give the fix a try tonight.
By the way, is there any way to get the unrounded driving distance for sub-millimeter precision? (I know it may not be possible to get that accurate in a real robot, but I would be curious to at least look at the residuals on a sub-millimeter scale.)

[laurensvalk]

Internally, we use millidegrees for the motors as the control unit to help smooth out speed and acceleration curves.

The motors only report whole degrees by default, but we're using a special mode that gives us about 0.3 degrees of resolution. Depending on your drivebase configuration, you can work out what the corresponding millimeter resolution would be.

[abliss]

Thanks. Is there any way for the python code to access the millidegree control variable?

[laurensvalk]

There are some ways to use it for datalogging. What's your use case? What is the value that you would like to read?

[abliss]

I've been exploring the idea of improving odometry on an FLL map by fusing the angle readings from the wheel encoders and the angular velocity from the gyro (plus possibly also the accelerometer readings if I can manage to pull enough signal out of the noise) with the readouts from a couple color sensors on the known mat image. Right now I am doing everything offline to try to figure out if it's even possible to get a worthwhile accuracy improvement, so just getting a timestamped datalog of the PID control variables for distance/angle would be helpful. (If I manage to get anything working, then the dream is to someday integrate the improved position estimation into the control loop itself, either in python (preferably, if it's fast enough) or (failing that) inside the C bindings on the firmware.)

[abliss]

I tried the linked firmware, running the updated program with the wait, and with the precision of 1mm requested: it printed 999 9 times and 1000 1 time. That's a definite improvement, though I still expected a more balanced mixture of 999/1000/1001. It seems that, at least after all the rounding is done and the number is reported to the user, there is likely still some kind of systematic bias away from the target distance.

[abliss]

Perhaps relatedly, I tried this program to measure the angle PID tolerance:

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


hub = PrimeHub()
right_motor = Motor(Port.B, positive_direction=Direction.CLOCKWISE)
left_motor = Motor(Port.A, positive_direction=Direction.COUNTERCLOCKWISE)
drive_base = DriveBase(left_motor, right_motor, 57.2, 96.1)
drive_base.reset(distance=0, angle=0)
#drive_base.distance_control.target_tolerances(position=1)
#drive_base.heading_control.target_tolerances(position=1)

drive_base.turn(360)

while True:
    print(f"Turned: {drive_base.angle()}")
    wait(500)

I ran this on my simple two-wheeled robot 10 times. The results were:

361.1012
361.755968
362.0238
360.625
360.74408
361.1012
360.8036
361.07144
361.3988
360.9524

Since every single one is over 360 and the mean is 361.16, there seems again to be a systematic bias.

I'm not sure what position=1 is meant to do exactly in the angle control. Is that 1 degree? Setting it to 1 or 1.1 seems to cause a hang, but 1.2 seems to work? What is the valid range?

[abliss]

There also seems to be some kind of bad interaction between the heading PID and use_gyro(True)? See this example program (again, running with 2 small motors as the wheels of the robot. The Spike Prime hub is mounted so that its center (where I think the IMU is) is directly above the midpoint of the two wheels.)

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

hub = PrimeHub()
right_motor = Motor(Port.B, positive_direction=Direction.CLOCKWISE)
left_motor = Motor(Port.A, positive_direction=Direction.COUNTERCLOCKWISE)
drive_base = DriveBase(left_motor, right_motor, 56.034, 96.1)
#drive_base.use_gyro(True)
drive_base.reset(distance=0, angle=0)
print(drive_base.heading_control.target_tolerances()) # prints (29, 15)
#drive_base.heading_control.target_tolerances(position=3)
print(drive_base.heading_control.target_tolerances())
drive_base.turn(-90, wait=True, then=Stop.HOLD)

while True:
    print(f"Turned: {drive_base.angle()}") 
    wait(500)

This very consistently prints -86 with use_gyro(True), and -91 without it.
If the unit of the target tolerance is indeed degrees, I am surprised that 15 would be the default, as this seems pretty wide.
If I set the tolerance to 3, I pretty consistently get -87, but occasionally the robot will hang (Perhaps it is stopping around 86.9, and trying to close the gap with a low voltage/duty-cycle due to the P value of the PID, but it's so low that the wheels don't move at all, and the robot just sits, maybe because its static friction is bigger than the dynamic load that the PID was trained on? If I nudge the robot, the program resumes.)

I was hoping that by setting tolerance to 3 degrees, I would get some distribution around a mean of -90 in the range of -87 to -93, but the PID always seems to err on the same side.

It does seem that setting drive_base.settings(turn_acceleration = 40) causes the final number to get closer to 90, Whereas, directly capping the turn_rate at 45 causes a similarly slow turn but does NOT seem to improve the final number. (However, I tried using multitasking to lower the turn_acceleration only when the turn was approaching its final endpoint, and this had no effect. It seems like the turn() command maybe only reads the turn_acceleration setting at the beginning?)

Playing some more, it really does seem like when I set the tolerance low, the robot is "trying" to finish the maneuver, but very consistently, when it about 97% of the way there, the voltage to the wheels becomes too low to cause any actual movement, and the robot sits still. If that's within my tolerances, the program gives up and declares success (always about 3% short of the target). If not, the robot sits still and keeps trying. Occasionally after some seconds it will manage a twitch, and sometimes that can bring it over the line.

Maybe this is specific to my robot/motors/wheels? In which case, would there be any way that I could ask for a minimum turn rate in the same way that drive_base.settings lets me set a maximum? (Relatedly: if I set the maximum turn rate to any number below 40, the robot lurches and stops repeatedly instead of turning in a smooth motion. I would love to set the minimum above this threshold to prevent this kind of movement from ever happening.)

Some more support for my theory: in a quiet room, I can run the above program, then carefully pick the robot up and pull off its wheels. If I hold it level and steady at exactly the 90 degree turn, the robot is quiet and still. If I turn it >5 degrees in one direction or the other, the robot will start spinning its motors trying to return to its target. The farther I turn away from the target, the faster the motors spin. As I turn the robot back closer to the target, the motors slow down. There is a "dead zone" of about 5 degrees on either side of the target where the robot's wheels don't move, but I hear a high-pitched whine. Within that, there is a "quiet zone" of about 3 degrees where the whine disappears. I think the whine in the "dead but not quiet" zone means the robot is trying to turn its wheels to get back inside the target tolerance, but at a voltage and/or duty-cycle that is too low to overcome the motors' internal friction. (Static friction, in the case of whining but not moving, or dynamic friction, in the case of slowing down while approaching the target and stopping 3% short.)

(My theory does not explain why the problem gets much worse with use_gyro(true). I wonder if the answer to that somehow involves the heading_correction. I had previously tried to use heading_correction to compensate for this problem because I thought it was a sensor issue. It works okay sometimes but is inconsistent, and I think my theory explains why.)

Poking around, it seems to me that a reasonable place to add my requested feature might be here:
https://github.com/pybricks/pybricks-micropython/blob/0b003f4fe031f29461f7ff3b9c547d94d6ad3aed/lib/pbio/src/servo.c#L103
it looks like there's a control.settings.actuation_max_temporary that we clamp to, so maybe there could also be an actuation_min as well? (I suppose 0 would have to be special-cased as an acceptable value too, so maybe it should really be called actuation_min_nonzero?)