Migrate PID controller to library (part 2)