Why doesn't motor hold its position even though I used the hold function ?

[sidneychen115]

I was transferring our turn function from last season's Spike Prime to Pybricks. The function is using left motor and right motor run in different direction at the same speed to make spin turn. And it is also using the hub.imu.heading() to compare with the target heading degree and stop the movement when the difference of these 2 values is within a tolerance range.

Although I used the the motor.hold() function to stop the motor and hold its position, the hub.imu.heading() still changes ( it becomes stable after around 30 ms , but the heading is way off the target heading I set for the turn)

Here is the code:

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

left_wheel = Motor(Port.C, Direction.COUNTERCLOCKWISE)
right_wheel = Motor(Port.B)

hub = PrimeHub()

drive_base = DriveBase(left_wheel, right_wheel, wheel_diameter=64.2, axle_track=160)

straight_speed = 200
straight_acc = 300
turn_rate = 200
turn_acc = 100

drive_base.settings(straight_speed, straight_acc, turn_rate, turn_acc)

drive_base.use_gyro(True)

def turn_degree(target_degree):
    speed = 100


    error = hub.imu.heading() - target_degree
    
    # Stop the turn if error < 0.5 degree
    while (abs(error) > 0.5):

        error = hub.imu.heading() - target_degree 
    
        left_wheel.run(speed)
        right_wheel.run(speed*-1)
        wait(10)
    right_wheel.hold()
    left_wheel.hold()

turn_degree(60)
counter = 0
while True:
    print(hub.imu.heading())
    wait(10)
    counter = counter+1
    if counter > 100:
        break

I added the last piece to keep on printing heading every 10 ms and here is top part of the output:
60.10498
60.60291
61.09998
61.46851
61.67529
61.78113
61.77881
61.71069
61.6438
61.61548
.....

As you can see, the function worked well and the heading was close to the target (60 degrees). But it is still increasing until stabilized at 61.5 degrees.

Could someone explain to me why it doesn't work as I expected? The only thing I can think off is motors were still moving in the first 30 ms, and the hold function worked after that. But why it didn't hold right away?

(I thought even the built-in drive_base.turn() function might be implemented with the same mechanism (running left and right wheels at different speed). How does it work if the motor can only hold the position after ~30 ms ? )

Thanks for your help!

[laurensvalk]

Robots are mechanical systems with some inertia. In your code when it reaches the target, the robot is still turning at your given speed. Forcing the motors to hold won't stop the robot immediately, but rather just slip a bit further due to the inertia.

The builtin turn function smoothly ramps up and down the speed to take this into account, by beginning to slow down in time to arrive at the target.

That said, for individual moves, a 1-degree error is not bad at all as long as the next move will take this into account, and continue to correct it. This is what the turn and straight methods do by default.

Why doesn't motor hold its position even though I used the hold function ?

It does. But holding the motor position doesn't mean the heading stays constant if the wheels slip due to physics. To see this, perhaps also print out the right_wheel and left_wheel angles in your print loop.

[sidneychen115]

Thanks a lot for the quick response!

The builtin turn function smoothly ramps up and down the speed to take this into account, by beginning to slow down in time to arrive at the target.

I added the ramp down feature in my code:

min_speed = 30
max_speed = 200

kp=3

error = hub.imu.heading() - target_heading

while (abs(error) > 0.9):

        error = hub.imu.heading() - target_heading 
        
        # propotional control on the turn speed
        # the robot starts to turn at high speed and reduce the turn speed while approaching the target heading (smaller error)
        speed = error * kp*-1  

        # speed can't be over or below max and min speed
        if abs(speed) >= max_speed:
            speed = max_speed 

        if abs(speed) <= min_speed:
            speed = min_speed 


        left_wheel.run(speed)
        right_wheel.run(speed*-1)

        wait(1)

right_wheel.hold()
left_wheel.hold()

I have to set a min speed and can't let it ramp down too low, as at a very low speed, the motor can't generate enough force to turn the robot. And I repeated the testing, and noticed that the higher min_speed I set, the larger difference the heading has at the end. So it is the inertia as you mentioned in your reply. However, it is hard to find a good balance between the good min_speed (higher min speed generates larger error at the end but makes the turn smoother. lower min speed generates smaller error but makes the robot wiggle at the end of the turn).

I eventually set this min speed to 30, and change the tolerance range from 0.5 to 0.9. And this is the output:
(Exit and stop the turn early, and the inertia makes it to closer heading at the end)

heading: 59.12988
heading: 59.36646
heading: 59.55652
heading: 59.72412
heading: 59.83948
heading: 59.93311
heading: 59.98022

I'm OK with the current result despite the robot wiggles a little bit at the end and takes a little longer time to finish. Thanks so much for your clarification.

That said, for individual moves, a 1-degree error is not bad at all as long as the next move will take this into account, and continue to correct it. This is what the turn and straight methods do by default.

Does this correction happen only when I set use_gyro(True)? Or it corrects no matter what?

It does. But holding the motor position doesn't mean the heading stays constant if the wheels slip due to physics. To see this, perhaps also print out the right_wheel and left_wheel angles in your print loop.

I also tried this and you are correct, the wheel angles don't change (or 1-2 degrees) after the motor holds.