I have recoded the motor part and added the encoder for the other motor and made a PID for each motor that takes the output from the current PID. This for trying to get the two motors to behave equal. The original PID is only looking on the right motor encoder as before. But I’m calculating the current speed in pulses/10ms for each motor and then using that and the output from the old PID to control the motors for a requested speed in pulses/10ms..
That feels better than just sending a value of 0-255 that has no connection to the speed of the motor that are depending on the surface and battery level.