Lego duplo train-like behavior

[Gcopper1984]

Hello,

here it is a little experiment: I tryed to copy the behavior of lego duplo train for my 42124 model.
In this case you can simple "launch" the model by simply drag it by hand and then it will go forward (or backward) indefinitely or until motor stall (like against the wall)

Here it is:

from pybricks.tools import StopWatch
from pybricks.tools import wait
from pybricks.pupdevices import Motor
from pybricks.parameters import Port

exampleMotor = Motor(Port.A)
exampleMotor.control.target_tolerances(50, 10)
exampleMotor.control.stall_tolerances(200, 200)
exampleMotor.stop()
exampleMotor.control.limits(1000, 720, 100, 300)
#print(exampleMotor.control.pid()) # 4000 600 1000 45 5
exampleMotor.control.pid(4000, 600, 1000, 45, 5)
actualSpeed = 0
travelPower = 40
minSpeed = 200

stopwatchSample = StopWatch()
collectSampleIntervalTime = 1
actualSample = 0
numSample = 50

stopwatchSpeed = StopWatch()
updateSpeedIntervalTime = 100
actualSpeedSample = 0
numSpeedSample = 3

while True:
    if stopwatchSample.time() >= collectSampleIntervalTime: # collect a sample
        stopwatchSample.reset()             
        actualSpeed += exampleMotor.speed()
        actualSample += 1

        if actualSample >= numSample:
            actualSpeed /= numSample
            actualSample = 0
            #print("Speed: ", actualSpeed)

    if stopwatchSpeed.time() >= updateSpeedIntervalTime: # collect a sample
        #print("Control: ", actualSpeed)
        stopwatchSpeed.reset()

        if actualSpeed < -minSpeed:      
            actualSpeedSample += 1

            if actualSpeedSample >= numSpeedSample: 
                exampleMotor.dc(-travelPower)

        elif actualSpeed > minSpeed:
            actualSpeedSample += 1

            if actualSpeedSample >= numSpeedSample:
                exampleMotor.dc(travelPower)

        else:
            actualSpeedSample = 0
            exampleMotor.stop()

    elif exampleMotor.control.stalled() == True:
        exampleMotor.stop() 
        wait(1000)
        actualSpeed = 0
        actualSpeedSample = 0
        stopwatchSample.reset()  
        stopwatchSpeed.reset() 

Hope this help.