I tried to implement a closed-loop system to keep the actual speed the same by increasing power to the motors if e.g. stuck on an object. Results were… amusing.
The speed was correctly being calculated from readings from the ultrasonic rangefinder. There turned out to be a combination of 2 problems (a variable overflowing, and something specific to the format of data that the Sabertooth motor speed controller requires), but they’re fixed and it’s working much better now. I’m still fine-tuning it, as it’s still far from ideal, though (either slow to respond or over-aggressive and dangerous, and interferes with other systems such as the part that detects if it’s stuck).