Migrate PID controller to library