Working code

This code works but does not lock very well. The PII controller has be
reduced to a PI controller.