EncodedMotor.set_speed() inconsistency

[sfe-SparkFro]

See here for problem explanation and replication instructions: https://xrp.discourse.group/t/should-this-code-work-as-designed/690/8?u=sparkfro

I suspect the PID controller is not being reset correctly when set_speed() is called. If I add self.speedController.clear_history() to the end of set_speed(), then I get consistent behavior. Not sure if that's the "right" solution, but it does seem to imply that's where the root problem is.

[ksiegall]

Looking into this further - In the last release, I removed a line of code that reset the speed controller history following the setting of a new speed. This allowed for the speed to get set within a loop without the integral term needed to restart for gradual changes, but causes issues like this where if set speed is called in separate instances multiple times, the integral term causes a large jump trying to chase the prior term.

Maybe the correct solution is to add the clear history into the drivetrain.stop() / encoded_motor.set_speed(0) cases, which would prevent this bleedthrough between separate instances of the function call.

[ksiegall]

I've tested this out and it seems to make the behavior consistent and performs better, even on the first trial after a hardware reset. Making a pull request.