[Bug] Bad trajectory calculation for some accelerations

[laurensvalk]

Describe the bug
Under some conditions, the reference trajectory calculation fails. Instead of going to the target gradually, it jumps there right away.

On the bright side, the controller is so stable now that it doesn't even blink and just goes there without complaining (like track_target), but clearly not at the commanded speed and acceleration.

Since this is only reproducable on the large motor while the trajectory code is motor independent, this is indicative that it happens for certain acceleration parameters, which do differ between motors.

To reproduce

On today's master build. This likely also affects the 3.2.0 beta1 release.

Attach a large angular motor to port A. Then run:

from pybricks.pupdevices import Motor
from pybricks.tools import wait
from pybricks.parameters import Port, Stop
from pybricks import version

print(version)

# Initialize the motor.
motor = Motor(Port.A)

# Allocate the data logs.
DURATION = 4000
motor.log.start(DURATION)
motor.control.log.start(DURATION)

# Preposition motor to reproduce bug
motor.run_target(500, -170)

# Bug occurs here:
motor.run_target(1000, 360)

# Wait so we can also log the stopped behavior.
wait(500)
motor.stop()

# Transfer data logs.
print("Transferring data...")
motor.log.save("servo.txt")
motor.control.log.save("control.txt")
print("Done")

[laurensvalk]

The following reproduces it on Move Hub using the internal motor

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

print(version)

# Initialize the motor.
motor = Motor(Port.A)
motor.run_target(1500, 360)
wait(2000)

[laurensvalk]

This should be fixed and covered by tests in test_trajectory.c.