PID library fixes

Make PID library more safe, parameters can be set only from pid_set_parameters(), but not from pid_init(). Some cleanup. Tested a bit.

Need more testing before merging.