Pybricks algorithm for consistent train speed

AdaptibleSpeedLGauge.py

# good introduction video to "Pybricks":
# https://www.youtube.com/watch?v=Jmh4NLxJ_8c
from pybricks.pupdevices import DCMotor, ColorDistanceSensor
from pybricks.hubs import CityHub
from pybricks.tools import StopWatch
from pybricks.parameters import Port, Color

# Initialize the motor and the sensor.
motor = DCMotor(Port.B)
sensor = ColorDistanceSensor(Port.A)
# remote = Remote()
hub = CityHub()

# These are the sensor reflection values in this setup.
# Adapt them to match your ambient light conditions.
# Values at home on the floor
# LIGHT = 57
# DARK = 16
# Moduverse track at home
LIGHT = 26
DARK = 17

# Threshold values. We add a bit of hysteresis to make
# sure we skip extra changes on the edge of each track.
hysteresis = (LIGHT - DARK) / 4
threshold_up = (LIGHT + DARK) / 2 + hysteresis
threshold_down = (LIGHT + DARK) / 2 - hysteresis

# Initial position state.
on_track = True
position = 0

# Desired drive speed in mm per second.
SPEED = 60

# It's two studs (16 mm) for each position increase.
MM_PER_COUNT = 8

# Start a timer.
watch = StopWatch()


while True:

    # Measure the reflection.
    reflection = sensor.reflection()

    # print(reflection)

    # If the reflection exceeds the threshold, increment position.
    if (reflection > threshold_up and on_track) or (reflection < threshold_down and not on_track):
        on_track = not on_track
        position += 1
        
    # Compute the target position based on the time.
    target_count = watch.time() / 1000 * SPEED / MM_PER_COUNT

    # The duty cycle is the position error times a constant gain.
    duty = 2*(target_count - position)

    # Apply the duty cycle.
    print("New desired speed:")
    print(duty)
    motor.dc(duty)
    # remote.light.on(Color.RED if duty > SPEED else Color.GREEN) - this slows down the train significantly
    hub.light.on(Color.RED if duty > SPEED else Color.GREEN)

    # FINDINGS:
    # The train will move slower when not connected to Pybricks
    # The train will try to calculate 
    # IDEAS:
    # Could we make a "calibrate step"?
    # so you could place the train on the tile, and then blank, track
    # it could record the track's reflection values on a trial run over the track?